tic
clear
%[kmax,Precision,UAngel,Volt,Busnum,PVi,PVu,Balance,Y,Angle,P0,Q0,r,c,GB,Linei,Linej,Transfori,Transforj,GenU,GenL,GenC,LineLimti,LineLimtj,LinePLimt,PG,QG,PD,QD,CenterA,LineCount,PGi,PVQU,PVQL]=pf('5sj.txt');
[kmax,Precision,UAngel,Volt,Busnum,PVi,PVu,Balance,Y,Angle,P0,Q0,r,c,GB,Linei,Linej,Transfori,Transforj,GenU,GenL,GenC,PG,QG,PD,QD,CenterA,PGi,PVQU,PVQL]=pf('ieee10471PG.dat');
GB=full(GB);
%PVi电压节点序号
%PVu电压节点电压标幺值
Volt;
UAngel*180/3.1415926;
%sprintf('%f\n',Volt);
%sprintf('%f\n',Angel);
%% 初值
PG0=PG;
PD0=PD;
[Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wG,wD,PD]=OPF_Init(Busnum,Balance,PG,QG,Volt,GenU,GenL,PVi,PGi,PVQU,PVQL,PD0);
Gap=(Init_L*Init_Z'-Init_U*Init_W');
KK=0;
plotGap=zeros(1,50);
ContrlCount=size(PVi,1)+size(PGi,1)+Busnum*3;
kmax=600;
%% 20120523 临时
QD_NON_ZERO=QD(PD==0 & QD~=0);
QD_NON_ZERO_IND=find(PD==0 & QD~=0);
%%
while(abs(Gap)>Precision)
    if KK>kmax
        break;
    end
    plotGap(KK+1)=Gap;
    Init_u=Gap/2/RestraintCount*CenterA;
    AngleIJMat=repmat(UAngel',1,Busnum)-repmat(UAngel,Busnum,1);
    %% 开始计算OPF
    %% 形成等式约束的雅克比
    deltH=func_deltH(Busnum,Volt,PVi,AngleIJMat,Y,GB,PGi);
    %% 形成不等式约束的雅克比
    deltG=func_deltG(Busnum,PVi,PGi);
    %%
    L_1Z=diag(Init_Z./Init_L);
    U_1W=diag(Init_W./Init_U);
    %% 形成海森阵
    deltdeltF=func_deltdeltF(Busnum,GenC,PVi,PGi,wG,wD,PD0);
    %% 形成ddHy
    ddh=func_ddh3(AngleIJMat,GB,Volt,Init_Y,Busnum,PVi,PGi,Y);
    %%  开始构建ddg
    ddg=func_ddg(PGi,PVi,Busnum,RestraintCount);
    %%  开始构建deltF
    deltF=func_deltF(PG,PVi,GenC,ContrlCount,PGi,wG,wD,PG0,PD0,PD,Busnum);
    
    %% 形成方程矩阵
    Luu=Init_U'.*Init_W'+Init_u*ones(RestraintCount,1);
    Lul=Init_L'.*Init_Z'-Init_u*ones(RestraintCount,1);
    Mat_G=FormG(Volt,PVi,PGi,PG,QG,PD);
    Mat_H=FormH(Busnum,GB,AngleIJMat,Volt,PG,PD,QG,QD,Y,QD_NON_ZERO,QD_NON_ZERO_IND);
    Ly=Mat_H;
    Lz=FormLz(Mat_G,Init_L,GenL,Busnum,PVQL,PD0);
    Lw=FormLw(Mat_G,Init_U,GenU,Busnum,PVQU,PD0);
    Lx=FormLx(deltF,deltH,Init_Y,deltG,Init_Z,Init_W);
    YY=FormYY1(Lul,Lz,Ly,Luu,Lw,Lx);
    %% 开始解方程
    XX=SolveIt(deltF,deltG,Init_L,Init_Z,Init_U,Init_W,deltdeltF,ddh,ddg,deltH,Init_Y,Ly,Lz,ContrlCount,Lw,Lul,Luu,RestraintCount,Lx,Balance,PVi,PGi,Busnum);
    %%取各分量
    [deltZ,deltL,deltW,deltU,deltX,deltY]=AssignXX1(XX,ContrlCount,RestraintCount,Busnum);
    [Init_Z,Init_L,Init_W,Init_U,Init_Y,PG,QG,Volt,UAngel,PD]=Modification(Init_Z,Init_L,Init_W,Init_U,Init_Y,deltZ,deltL,deltW,deltU,deltX,deltY,PG,QG,Volt,UAngel,PVi,ContrlCount,Balance,Busnum,PGi,PD);
    Gap=(Init_L*Init_Z'-Init_U*Init_W');
    KK=KK+1;
end
fprintf('迭代次数%d\n',KK);
%CalCost(GenC,PG,PGi);
ObjectiveFun(PG,PG0,PGi,PD,PD0,wG,wD)
DrawGap(plotGap);
Volt=Volt';
toc