tic
clc
clear
%% 存在问题
% 变压器变比的位置没有考虑，由于现在用的变比都是1，所以没有影响。 20130123
%%
thesis=ForThesis(1,62);
[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,Liner,Linex,Lineb,Transforr,Transforx,Branchi,Branchg,Branchb,Transfork0]= ...
    pf('E:\算例\东际911_2751267_2012-09-05\newFIle20-使用22.txt');
%         pf('E:\算例\柳金Ⅰ926_21671693_2012-09-06\newFIle16.txt');
%pf('D:\Project\青秀降损项目\最小化潮流\最小潮流算例\原始\津头站津视922（3-1）_0.5_120%.txt');
%pf('D:\Project\最小化潮流\最小潮流算例\仙海919.txt');
%pf('c:/file31.txt');
%% 计算功率因数
Loadi=QD~=0 | PD~=0;
PF=sqrt(PD(Loadi).^2./(QD(Loadi).^2+PD(Loadi).^2));
%%
Volt;
UAngel*180/3.1415926;
%% 通过潮流计算PG
AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
PGBal=PD+diag(Volt)*Y.*cos(AngleIJ)*Volt';
QGBal=QD+diag(Volt)*Y.*sin(AngleIJ)*Volt';
%% 初值-即测量值
PG0=PG;
QG0=QG;
PD0=PD;
QD0=QD;
Volt0=Volt;
UAngel0=UAngel;
%%
PG0(Balance)=PGBal(Balance);
PG(Balance)=PGBal(Balance);
QG0(Balance)=QGBal(Balance);
QG0(PVi)=QGBal(PVi);
QG(PVi)=QGBal(PVi);
%% 真实值
RealPG=PG0;
RealQG=QG0;
RealPD=PD0;
RealQD=QD0;
%%
[Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wPG,wQG,wPD,wQD,PD,PD0,QD,randPDind,Loadi]=OPF_Init(Busnum,Balance,PG,QG,Volt,GenU,GenL,PVi,PGi,PVQU,PVQL,RealPD,RealQD,QD,PD);
Gap=(Init_L*Init_Z'-Init_U*Init_W');
KK=0;
plotGap=zeros(1,60);
ContrlCount=size(Loadi,1)*2+Busnum*2+Busnum;
kmax=60;
Precision=Precision/1;
%% 加误差
PD0(Loadi)=PD0(Loadi).*(1+normrnd(0,0.05,length(Loadi),1));
QD0(Loadi)=QD0(Loadi).*(1+normrnd(0,0.05,length(Loadi),1));
Vbi=sparse(1*ones(Busnum,1));
kInit_Z=zeros(length(Init_Z),4);
kInit_L=zeros(length(Init_L),4);
kInit_W=zeros(length(Init_W),4);
kInit_U=zeros(length(Init_U),4);
kInit_Y=zeros(length(Init_Y),4);
kPG=zeros(length(PG),4);
kQG=zeros(length(QG),4);
kVolt=zeros(length(Volt),4);
kUAngel=zeros(length(UAngel),4);
kPD=zeros(length(PD),4);
kQD=zeros(length(QD),4);
kVbi=zeros(length(Vbi),4);
kdeltZ=zeros(length(Init_Z),4);
kdeltL=zeros(length(Init_L),4);
kdeltW=zeros(length(Init_W),4);
kdeltU=zeros(length(Init_U),4);
kdeltX=zeros(ContrlCount,4);
kdeltY=zeros(length(Init_Y),4);
while(abs(Gap)>Precision)
    if KK>kmax
        break;
    end
    plotGap(KK+1)=Gap;
    for I=1:4
        Init_u=Gap/2/RestraintCount*CenterA;
        AngleIJMat=0;
        %% 开始计算OPF
        %% 形成等式约束的雅克比
        deltH=func_deltH(Busnum,Volt,PVi,Y,PGi,UAngel,r,c,Angle,Loadi);
        %% 形成不等式约束的雅克比
        deltG=func_deltG(Busnum,PVi,PGi,Loadi,PD,QD);
        %%
        L_1Z=diag(Init_Z./Init_L);
        U_1W=diag(Init_W./Init_U);
        %% 形成海森阵
        deltdeltF=func_deltdeltF(PVi,wPG,wQG,wPD,wQD,ContrlCount);
        %% 形成ddHy
        ddh=func_ddh(Volt,Init_Y,Busnum,PVi,PGi,Y,UAngel,r,c,Angle,Loadi,ContrlCount);
        %%  开始构建ddg
        ddg=func_ddg(PGi,PVi,Busnum,RestraintCount,Loadi,PD,QD);
        %%  开始构建deltF
        deltF=func_deltF(PG,QG,PVi,PGi,wPG,wQG,wPD,wQD,PG0,QG0,PD0,PD,QD,QD0,Busnum,Loadi);
        %% 形成方程矩阵
        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,QD,Loadi,Vbi);
        Mat_H=FormH(Busnum,Volt,PG,PD,QG,QD,Y,UAngel,r,c,Angle,Loadi);
        Ly=Mat_H;
        Lz=FormLz(Mat_G,Init_L,GenL,Busnum,PVQL,PD,RealPD,RealQD,Loadi,KK,PF);
        Lw=FormLw(Mat_G,Init_U,GenU,Busnum,PVQU,PD,RealPD,RealQD,Loadi,KK,PF);
        Lx=FormLx(deltF,deltH,Init_Y,deltG,Init_Z,Init_W);
        YY=FormYY(Lul,Lz,Ly,Luu,Lw,Lx);
        %% 开始解方程
        fprintf('迭代次数 %d Gap %f\n',KK+1,plotGap(KK+1));
        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,Loadi);
        %%取各分量
        [kdeltZ(:,I),kdeltL(:,I),kdeltW(:,I),kdeltU(:,I),kdeltX(:,I),kdeltY(:,I)]=AssignXX(XX,ContrlCount,RestraintCount,Busnum);
        if I==1
            stepT=0.5;
        end
        if I==2
            stepT=0.5;
        end
        if I==3
            stepT=1;
        end
        if I<=3
            [kInit_Z(:,I),kInit_L(:,I),kInit_W(:,I),kInit_U(:,I),kInit_Y(:,I), ...
                kPG(:,I),kQG(:,I),kVolt(:,I),kUAngel(:,I),kPD(:,I),kQD(:,I),kVbi(:,I)]=Modification(Init_Z,Init_L,Init_W,Init_U,Init_Y,stepT*kdeltZ(:,I),stepT*kdeltL(:,I),stepT*kdeltW(:,I),stepT*kdeltU(:,I),stepT*kdeltX(:,I),stepT*kdeltY(:,I),PG,QG,Volt,UAngel,PVi,ContrlCount,Balance,Busnum,PGi,PD,QD,Loadi,Vbi);
            Init_Z=kInit_Z(:,I)';
            Init_L=kInit_L(:,I)';
            Init_W=kInit_W(:,I)';
            Init_U=kInit_U(:,I)';
            Init_Y=kInit_Y(:,I)';
            PG=kPG(:,I);
            QG=kQG(:,I);
            Volt=kVolt(:,I)';
            UAngel=kUAngel(:,I)';
            PD=kPD(:,I);
            QD=kQD(:,I);
            Vbi=kVbi(:,I);
        end
        if I==4
            deltZ=(kdeltZ(:,1)+kdeltZ(:,2)*2+kdeltZ(:,3)*2+kdeltZ(:,4))/6;
            deltL=(kdeltL(:,1)+kdeltL(:,2)*2+kdeltL(:,3)*2+kdeltL(:,4))/6;
            deltW=(kdeltW(:,1)+kdeltW(:,2)*2+kdeltW(:,3)*2+kdeltW(:,4))/6;
            deltU=(kdeltU(:,1)+kdeltU(:,2)*2+kdeltU(:,3)*2+kdeltU(:,4))/6;
            deltX=(kdeltX(:,1)+kdeltX(:,2)*2+kdeltX(:,3)*2+kdeltX(:,4))/6;
            deltY=(kdeltY(:,1)+kdeltY(:,2)*2+kdeltY(:,3)*2+kdeltY(:,4))/6;
            [Init_Z,Init_L,Init_W,Init_U,Init_Y,PG,QG,Volt,UAngel,PD,QD,Vbi]=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,QD,Loadi,Vbi);
        end
    end
    Gap=(Init_L*Init_Z'-Init_U*Init_W');
    KK=KK+1;
end
fprintf('%f\n',sum(full(Vbi)));
toc