tic
clc
clear
lineZ=readLineZ('feeder13\lineParameter.txt');
[ fsY0, fsY1, fsY2,phaseASpotLoadP,phaseBSpotLoadP,phaseCSpotLoadP ...
    phaseASpotLoadQ,phaseBSpotLoadQ,phaseCSpotLoadQ,setIJ,nodeNum,Balance,phaseABCY ...
    cap]=dataRead(lineZ,'feeder13\data1.txt');
busNum=length(nodeNum);
Busnum=busNum;
PQi=setxor(nodeNum,Balance);
QGi=[Balance];
% fprintf('开始牛顿法迭代\n');
[r,c,GB]=find(phaseABCY);
Y=abs(phaseABCY);
Yangle=angle(GB);
Vp3=sparse(ones(busNum*3,1));%给电压赋初值
Vp3(2:3:end)=Vp3(2:3:end)*exp(1j*-120/180*pi);
Vp3(3:3:end)=Vp3(3:3:end)*exp(1j*+120/180*pi);
PQi3P=zeros(length(PQi)*3,1);
PQi3P(1:3:end)=(PQi-1)*3+1;
PQi3P(2:3:end)=(PQi-1)*3+2;
PQi3P(3:3:end)=(PQi-1)*3+3;
PD3P=sparse(busNum*3,1);
QD3P=sparse(busNum*3,1);
PD3P(1:3:end)=phaseASpotLoadP;
PD3P(2:3:end)=phaseBSpotLoadP;
PD3P(3:3:end)=phaseCSpotLoadP;
QD3P(1:3:end)=phaseASpotLoadQ;
QD3P(2:3:end)=phaseBSpotLoadQ;
QD3P(3:3:end)=phaseCSpotLoadQ;
QGi3P=zeros(length(QGi)*3,1);
QGi3P(1:3:end)=(QGi-1)*3+1;
QGi3P(2:3:end)=(QGi-1)*3+2;
QGi3P(3:3:end)=(QGi-1)*3+3;
PGA=sum(PD3P(1:3:end))*1.03;
PGB=sum(PD3P(2:3:end))*1.03;
PGC=sum(PD3P(3:3:end))*1.03;
QGA=sum(QD3P(1:3:end))*1.03;
QGB=sum(QD3P(2:3:end))*1.03;
QGC=sum(QD3P(3:3:end))*1.03;
PG3P=sparse( (Balance-1)*3+1:(Balance-1)*3+3,1,[PGA,PGB,PGC],Busnum*3,1);
QG3P=sparse( (Balance-1)*3+1:(Balance-1)*3+3,1,[QGA,QGB,QGC],Busnum*3,1);
Vp3m=abs(Vp3);
Vp3a=angle(Vp3);
Balance3P=zeros(length(Balance)*3,1);
Balance3P(1:3:end)=(Balance-1)*3+1;
Balance3P(2:3:end)=(Balance-1)*3+2;
Balance3P(3:3:end)=(Balance-1)*3+3;
Vp3a((Balance-1)*3+1)=0;
Vp3a((Balance-1)*3+2)=-120/180*pi;
Vp3a((Balance-1)*3+3)=+120/180*pi;
Loadi=PQi3P;
wPD=ones(length(Loadi),1);
wQD=ones(length(Loadi),1);
PD0=PD3P;
QD0=QD3P;
% orderedPDPG=sparse(PQi,1,PD-PG,busNum,1); %节点功率按顺序排列
% orderedQDQG=sparse(PQi,1,QD-QG,busNum,1); %节点功率按顺序排列

% [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:/算例/新民Ⅰ906_2729823_2012-09-06/newFIle20.txt');
%% 计算功率因数

%     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;
%%
RestraintCount=size(Loadi,1)*2; %约束条件数,放开所有QD
Init_Z=sparse(ones(1,RestraintCount));
Init_W=sparse(-1*ones(1,RestraintCount));
Init_L=1*sparse(ones(1,RestraintCount));
Init_U=1*sparse(ones(1,RestraintCount));
Init_Y=sparse(1,2*Busnum*3);%与学姐一致

% [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*6;
kmax=30;
%% 20120523 临时
% QD_NON_ZERO=QD(PD==0 & QD~=0);
% QD_NON_ZERO_IND=find(PD==0 & QD~=0);
%%
% Precision=Precision/1;
Precision=1e-5;
CenterA=0.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));
%% 读变压器容量
% noDataTransCapacity=0;
Volt=Vp3m;
UAngel=Vp3a;
while(abs(Gap)>Precision)
    if KK>kmax
        break;
    end
%     plotGap(KK+1)=Gap;
    Init_u=Gap/2/RestraintCount*CenterA;
    AngleIJMat=0;
    %% 开始计算OPF
    %% 形成等式约束的雅克比
    deltH=func_deltH(Busnum,Volt,Y,UAngel,r,c,Yangle,Loadi);
    %% 形成不等式约束的雅克比
    deltG=func_deltG(Busnum,Loadi);
    %%
    L_1Z=diag(Init_Z./Init_L);
    U_1W=diag(Init_W./Init_U);
    %% 形成海森阵
    deltdeltF=func_deltdeltF(wPD,wQD,ContrlCount);
%     deltdeltF=0;
    %% 形成ddHy
    ddh=func_ddh(Volt,Init_Y,Busnum,Y,UAngel,r,c,Yangle,Loadi,ContrlCount);
    %%  开始构建ddg
%     ddg=func_ddg(PGi,PVi,Busnum,RestraintCount,Loadi,PD,QD);
    ddg=0;
    %%  开始构建deltF
    deltF=func_deltF(wPD,wQD,PD0,PD3P,QD3P,QD0,Busnum,Loadi);
%     deltF=0;
    %% 形成方程矩阵
    Luu=Init_U'.*Init_W'+Init_u*ones(RestraintCount,1);
    Lul=Init_L'.*Init_Z'-Init_u*ones(RestraintCount,1);
    Mat_G=FormG(PD3P,QD3P,Loadi);
    Mat_H=FormH(Busnum,Volt,PG3P,PD3P,QG3P,QD3P,Y,UAngel,r,c,Yangle);
    Ly=Mat_H;
    Lz=FormLz(Mat_G,Init_L,Loadi);
    Lw=FormLw(Mat_G,Init_U,Loadi);
    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,Lx,Balance,Busnum,Loadi);
    %%取各分量
    [deltZ,deltL,deltW,deltU,deltX,deltY]=AssignXX(XX,ContrlCount,RestraintCount,Busnum);
    [Init_Z,Init_L,Init_W,Init_U,Init_Y,PG,QG,Volt,UAngel,PD3P,QD3P]=Modification(Init_Z,Init_L,Init_W,Init_U,Init_Y,deltZ,deltL,deltW,deltU,deltX,deltY,PG3P,QG3P,Volt,UAngel,ContrlCount,Balance,Busnum,PD3P,QD3P,Loadi);
    Gap=(Init_L*Init_Z'-Init_U*Init_W');
    fprintf('%f\n',full(Gap));
    KK=KK+1;
end
fprintf('迭代次数%d\n',KK);
% fprintf('目标值%f\n',full(ObjectiveFun(PG,PG0,PGi,QG,QG0,PVi,PD,PD0,QD,QD0,wPG,wQG,wPD,wQD,Loadi)));
%% 计算总线损
% totalLoss=(sum(PG)-sum(PD(Loadi)))*100;
% fprintf('总的损耗为%f(MW 有名值)\n',full(totalLoss));
% fprintf('线损率为 %f\n',full(totalLoss/sum(PG)));

toc