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;
    %     %计算负序电流
    %     Vf2=Vmf2.*exp(1j*Vaf2) ;
    %     If2=fsY2*(Vf2);
    %     %计算零序电流
    %     Vf0=Vmf0.*exp(1j*Vaf0) ;
    %     If0=fsY0*(Vmf0.*exp(1j*Vaf0) );
    %转换为三相电压
    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);
    %If1(Balance)=1;
    %fsY1(Balance,:)=0;
    %fsY1(:,Balance)=0;
    %fsY1=fsY1+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
    %Vf1=fsY1\If1;%算一下，虽然不用。
    %平衡节点零序电压为0
    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,fsY11,Vf1);%不平衡量
    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);
    %%
%     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,:)');
    %%
%     If0(Balance)=-sum(If0);
%     If2(Balance)=-sum(If2);
%     If1(Balance)=-sum(If1);
    %%
end
%%
Vtest=[1.02*exp(1j*0);1.01*exp(1j*0.21);1.00*exp(1j*-0.13)];%相电压
%Itest=[0.12*exp(1j*0.15);0.09*exp(1j*1.21);0.81*exp(1j*-0.43)];%相电流
Ytest=diag([0.75*exp(1j*1.7);33*exp(1j*2.1);12*exp(1j*-3)]);
Itest=Ytest*Vtest;
Stest=Vtest.*conj(Itest);%相功率
sum(Stest)/3;
sum((Tp2f*Vtest).*conj(Tp2f*Itest));
YYtest=Ytest;
Ytest(2,:)=0;
Ytest(:,2)=0;
Ytest(2,2)=1;
tttt=Itest(2);
Itest(2)=1.01*exp(1j*0.21);
tV=Ytest\Itest;
Itest(2)=tttt;
YYtest*tV;
%%
(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);
aa=phaseABCY(1:3,1:3)*[VoltpA(1);VoltpB(1);VoltpC(1)];
a1=Tf2p*diag([fsY0(2,1),fsY1(2,1),fsY2(2,1)])*[Vf0(1);Vf1(1);Vf2(1)];
a2=Tf2p*diag([fsY0(2,2),fsY1(2,2),fsY2(2,2)])*[Vf0(2);Vf1(2);Vf2(2)];
a3=Tf2p*diag([fsY0(2,3),fsY1(2,3),fsY2(2,3)])*[Vf0(3);Vf1(3);Vf2(3)];
bb=0;
a1=diag([fsY0(2,2)])*[Vf0(2)];
a2=diag([fsY1(2,2)])*[Vf1(2)];
a3=diag([fsY2(2,2)])*[Vf2(2)];
bb=bb+1*[a1;a2;a3];
a1=diag([fsY0(2,1)])*[Vf0(1)];
a2=diag([fsY1(2,1)])*[Vf1(1)];
a3=diag([fsY2(2,1)])*[Vf2(1)];
bb=bb+1*[a1;a2;a3];
a1=diag([fsY0(2,3)])*[Vf0(3)];
a2=diag([fsY1(2,3)])*[Vf1(3)];
a3=diag([fsY2(2,3)])*[Vf2(3)];
bb=bb+1*[a1;a2;a3];
b=0;
aa=phaseABCY(4:6,4:6)*[VoltpA(2);VoltpB(2);VoltpC(2)];
b=b+aa;
aa=phaseABCY(4:6,1:3)*[VoltpA(1);VoltpB(1);VoltpC(1)];
b=b+aa;
aa=phaseABCY(4:6,7:9)*[VoltpA(3);VoltpB(3);VoltpC(3)];
b=b+aa;
aa=phaseABCY(4:6,:)*[VoltpA(1);VoltpB(1);VoltpC(1);VoltpA(2);VoltpB(2);VoltpC(2);VoltpA(3);VoltpB(3);VoltpC(3)];
conj(aa).*[VoltpA(2);VoltpB(2);VoltpC(2)];
conj(a1+a2+a3).*[VoltpA(2);VoltpB(2);VoltpC(2)];
aa=[15.3502321158364 - 26.9339348391836i,30.6661479563297 - 84.3093119155620i,30.6661479563297 - 84.3093119155620i;30.6661479563297 - 84.3093119155620i,15.3502321158364 - 26.9339348391836i,30.6661479563297 - 84.3093119155620i;30.6661479563297 - 84.3093119155620i,30.6661479563297 - 84.3093119155620i,15.3502321158364 - 26.9339348391836i;];
aa*Vp3(1:3);
fs=Tp2f*aa*Tf2p;
phaseABCY(7,:)=0;
phaseABCY(:,7)=0;
phaseABCY(8,:)=0;
phaseABCY(:,8)=0;
phaseABCY(9,:)=0;
phaseABCY(:,9)=0;
phaseABCY(7,7)=1;
phaseABCY(8,8)=1;
phaseABCY(9,9)=1;
Ip3(7)=1;
Ip3(8)=-0.5 -     0.866025403784439i;
Ip3(9)=-0.5 +     0.866025403784439i;
inv(phaseABCY)*Ip3;
IppABC=sparse(9,1);
IppABC(1:3:end)=IpABC(1,:);
IppABC(2:3:end)=IpABC(2,:);
IppABC(3:3:end)=IpABC(3,:);
IppABC(7)=1;
IppABC(8)=-0.5 -     0.866025403784439i;
IppABC(9)=-0.5 +     0.866025403784439i;
inv(phaseABCY)*IppABC;
aa*Tf2p*[Vf0(1);Vf1(1);Vf2(1)]-Tf2p*[If0(1);If1(1);If2(1)];
diag([fsY0(1),fsY1(1),fsY2(1)])*[Vf0(1);Vf1(1);Vf2(1)]-[If0(1);If1(1);If2(1)];
Tp2f*diag([fsY0(1),fsY1(1),fsY2(1)])*Tf2p;
altY012=sparse(3,3);
altY012(1,1)=fsY0(1,1);
altY012(2,2)=fsY1(1,1);
altY012(3,3)=fsY2(1,1);
V11I=altY012*[Vf0(1);Vf1(1);Vf2(1)];
V11S=[VoltpA(1);VoltpB(1);VoltpC(1)].*conj(Tf2p*altY012*Tp2f*[VoltpA(1);VoltpB(1);VoltpC(1)]);
altY012(1,1)=fsY0(1,2);
altY012(2,2)=fsY1(1,2);
altY012(3,3)=fsY2(1,2);
V12I=altY012*[Vf0(2);Vf1(2);Vf2(2)];
V12S=[VoltpA(1);VoltpB(1);VoltpC(1)].*conj(Tf2p*altY012*Tp2f*[VoltpA(1);VoltpB(1);VoltpC(1)]);
altY012(1,1)=fsY0(1,3);
altY012(2,2)=fsY1(1,3);
altY012(3,3)=fsY2(1,3);
V13I=altY012*[Vf0(3);Vf1(3);Vf2(3)];
V12S+V11S;
[VoltpA(1);VoltpB(1);VoltpC(1)].*(Tf2p*(V11I+V12I));
Tf2p*V11I;
altY012(1,1)=fsY0(1,1);
altY012(2,2)=fsY1(1,1);
altY012(3,3)=fsY2(1,1);
Tf2p*altY012*Tp2f*[VoltpA(1);VoltpB(1);VoltpC(1)];
Tf2p*[Vf0(1);Vf1(1);Vf2(1)];