clc
clear
lineZ=readLineZ('.\feeder13\lineParameter.txt');
[ fsY0, fsY1, fsY2,phaseASpotLoadP,phaseBSpotLoadP,phaseCSpotLoadP ...
    phaseASpotLoadQ,phaseBSpotLoadQ,phaseCSpotLoadQ,nodeNum,Balance,phaseABCY]=dataRead(lineZ,'.\feeder13\data1.txt');
%fsY1(1,1)=fsY1(1,1)+1j*1e-10;
a=exp(1j*2*pi/3);
Tp2f=1/3*[1 1 1;
    1 a a^2;
    1 a^2 a];
Tf2p=inv(Tp2f);
fsY1amp=abs(fsY1);
[r,c,fsY1ang]=find(fsY1);
fsY1ang=angle(fsY1ang);
% fsY2amp=abs(fsY2);
% fsY0ang=angle(fsY0);
% fsY1ang=angle(fsY1);
% fsY2ang=angle(fsY2);
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;
%%把F矩阵转会到相矩阵，看看是哪里有问题
phaseABCYalt=Yf2p( fsY0,fsY1,fsY2 );
%%
    Vf1=-ones(busNum,1);
            fsY11=fsY1;
            fsY00=fsY0;
            fsY22=fsY2;
while(k<=kmax && maxD> EPS)
    k=k+1;
    VoltpABC=Tf2p*conj([ (Vmf0.*exp(1j*Vaf0))'; (Vmf1.*exp(1j*Vaf1))'; (Vmf2.*exp(1j*Vaf2))']);
    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,:)');
    %平衡节点负序电压为0
    fsY2(Balance,:)=0;
    fsY2(:,Balance)=0;
    fsY2=fsY2+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
    If2(Balance)=0;
    Vf2=fsY2\If2;
    Vmf2=abs(Vf2);
    Vaf2=angle(Vf2);
    fsY0(Balance,:)=0;
    fsY0(:,Balance)=0;
    fsY0=fsY0+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
    If0(Balance)=0;
    Vf0=fsY0\If0;
    Vmf0=abs(Vf0);
    Vaf0=angle(Vf0);

    [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([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([ (Vmf0.*exp(1j*Vaf0))'; (Vmf1.*exp(1j*Vaf1))'; (Vmf2.*exp(1j*Vaf2))']);
    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,:)');
    Vf0=fsY0\If0;
    Vmf0=abs(Vf0);
    Vaf0=angle(Vf0);
    Vf2=fsY2\If2;
    Vmf2=abs(Vf2);
    Vaf2=angle(Vf2);
    Vf1=Vmf1.*exp(1j*Vaf1);
    %%
end
(Vf0.*conj(fsY00*Vf0)+Vf1.*conj(fsY11*Vf1)+Vf2.*conj(fsY22*Vf2))*3;
(Vf1.*conj(fsY11*Vf1))/3;
conj(Tf2p*[If0(2);If1(2);If2(2)]).*(Tf2p*[Vf0(2);Vf1(2);Vf2(2)]);
%%
If1=fsY11*Vf1;
If1(Balance)=Vf1(Balance);
fsY1(Balance,:)=0;
fsY1(:,Balance)=0;
% fsY1=fsY1+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
fsY1(3,3)=1;
%VV1=fsY1(1:2,1:2)\If1(1:2);%算一下，虽然不用。
VV1=fsY1\If1;%算一下，虽然不用。
%Vf1=Vmf1.*exp(1j*Vaf1);
If0(Balance)=-sum(If0);
If2(Balance)=-sum(If2);
If1(Balance)=-sum(If1);
fsY11*VV1;
%%
IpABC=Tf2p*conj([If0';If1';If2']);
%转换回三相电压
VoltpABC=Tf2p*conj([ (Vmf0.*exp(1j*Vaf0))'; (Vmf1.*exp(1j*Vaf1))'; (Vmf2.*exp(1j*Vaf2))']);
VoltpABC(:,1);
Tp2f*VoltpABC(:,1);
Tf2p*Tp2f*VoltpABC(:,1);
disp(['                  A                        B                          C'])
abs(VoltpABC')
VoltpA=VoltpABC(1,:);
VoltpB=VoltpABC(2,:);
VoltpC=VoltpABC(3,:);
Vp3=sparse(busNum*3,1);
Vp3(1:3:end)=VoltpA;
Vp3(2:3:end)=VoltpB;
Vp3(3:3:end)=VoltpC;
Ip3=phaseABCY*Vp3;
Sp3=Vp3.*conj(Ip3);
VoltpABC.*conj(IpABC);
I2inj=IpABC(:,2);