clc
clear
lineZ=readLineZ('.\feeder13\lineParameter.txt');
[ fsY0, fsY1, fsY2,phaseASpotLoadP,phaseBSpotLoadP,phaseCSpotLoadP ...
    phaseASpotLoadQ,phaseBSpotLoadQ,phaseCSpotLoadQ,setIJ,nodeNum,Balance,phaseABCY]=dataRead(lineZ,'.\feeder13\data1.txt');
a=exp(1j*2*pi/3);
Tp2f=1/3*[1 1 1;
    1 a a^2;
    1 a^2 a];
Tp2f=sparse(Tp2f);
Tf2p=inv(Tp2f);
fsY1amp=abs(fsY1);
[r,c,fsY1ang]=find(fsY1);
fsY1ang=angle(fsY1ang);
Pabc=phaseASpotLoadP+phaseBSpotLoadP+phaseCSpotLoadP;
Qabc=phaseASpotLoadQ+phaseBSpotLoadQ+phaseCSpotLoadQ;
busNum=length(phaseASpotLoadP);
%给序电压赋初值
Vmf1=sparse(ones(busNum,1));
Vmf2=sparse(ones(busNum,1));
Vmf0=sparse(busNum,1);
Vaf1=sparse(busNum,1);
Vaf2=sparse(busNum,1);
Vaf0=sparse(busNum,1);
%先求解正序的
PQi=nodeNum;
PG=sparse(busNum,1);
QG=sparse(busNum,1);
QGi=[Balance];
PD=Pabc/3;
QD=Qabc/3;
maxD=100000;% 最大不平衡量
EPS=1e-5;
k=0;
kmax=20;
fsY11=fsY1;
fsY00=fsY0;
fsY22=fsY2;
Vf2=sparse(busNum,1);
If2=sparse(busNum,1);
Vf0=sparse(busNum,1);
If0=sparse(busNum,1);
%准备序矩阵
fsY2(Balance,:)=0;
fsY2(:,Balance)=0;
fsY2=fsY2+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
fsY0(Balance,:)=0;
fsY0(:,Balance)=0;
fsY0=fsY0+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
while(k<=kmax && maxD> EPS)
    k=k+1;
    [dP, dQ, YdotSinVolt, YdotCosVolt, diag_Volt_YdotSin, diag_Volt_YdotCos]=Unbalance(Balance,busNum, ...
        PQi,PG,QG,QGi,PD,QD,Vmf1,Vaf1,fsY1amp,fsY1ang,r,c,Vf2,If2,Vf0,If0);%不平衡量
    maxD=max(abs([dP;dQ;]));
    jaco=Jacobi(Balance,busNum,QGi,Vmf1,YdotSinVolt,YdotCosVolt,diag_Volt_YdotSin,diag_Volt_YdotCos);%雅克比矩阵
    [dV, dVangle]=Solv(busNum,jaco,dP,dQ);%解出修正量
    [Vmf1, Vaf1]=Modify(Vmf1,Vaf1,dV,dVangle,1);
    fprintf('第 %d 次迭代， 最大不平衡量为 %f\n\n',k,full(maxD));
    %%
    %转换为三相电压
    VoltpABC=Tf2p*conj([ Vf0'; (Vmf1.*exp(1j*Vaf1))'; Vf2']);
    VoltpA=conj(VoltpABC(1,:)');
    CurpA=-conj((phaseASpotLoadP+1j*phaseASpotLoadQ)./VoltpA);
    VoltpB=conj(VoltpABC(2,:)');
    CurpB=-conj((phaseBSpotLoadP+1j*phaseBSpotLoadQ)./VoltpB);
    VoltpC=conj(VoltpABC(3,:)');
    CurpC=-conj((phaseCSpotLoadP+1j*phaseCSpotLoadQ)./VoltpC);
    f012=Tp2f*conj([CurpA';CurpB';CurpC']);
    If0=conj(f012(1,:)');
    If1=conj(f012(2,:)');
    If2=conj(f012(3,:)');
    If0(Balance)=0;
    If2(Balance)=0;
    Vf0=fsY0\If0;
    Vf2=fsY2\If2;
end
Vf1=Vmf1.*exp(1j*Vaf1);
%%
% V=[1.01*exp(1j*0);1.01*exp(-1j*pi/360*120);1.01*exp(-1j*pi/360*240)];
% abs(Tp2f*V)
% Tf2p*Tp2f*V
% Vf0=Vf0+refV0;
% Vf2=Vf2+refV2;
% Vf0=Vf0*exp(-1j*(-1.0471975511966));
% Vf1=Vf1*exp(-1j*(1.0471975511966));
%%
(Vf0.*conj(fsY00*Vf0)+Vf1.*conj(fsY11*Vf1)+Vf2.*conj(fsY22*Vf2))*3;
conj(Tf2p*[If0(2);If1(2);If2(2)]).*(Tf2p*[Vf0(2);Vf1(2);Vf2(2)]);
IpABC=Tf2p*conj([If0';If1';If2']);
%转换回三相电压
VoltpABC=Tf2p*conj([ Vf0'; Vf1'; Vf2']);
disp(['                  A                        B                          C'])
full(abs(VoltpABC'))
fprintf('节点号对应\n');
disp([setIJ,nodeNum ])
checkSSatisfied(Balance,phaseABCY,VoltpABC, ...
    phaseASpotLoadP,phaseBSpotLoadP,phaseCSpotLoadP, ...
    phaseASpotLoadQ,phaseBSpotLoadQ,phaseCSpotLoadQ );
%%检查反推回去的功率是否满足