%% 利用先把负荷转换为电流的方法。这个方法要求知道电压量。
%
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');
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));
Vaf1=sparse(zeros(busNum,1));
%先求解正序的
PQi=nodeNum;
PG=sparse(busNum,1);
QG=sparse(busNum,1);
QGi=[Balance];
PD=Pabc/3;
QD=Qabc/3;
Loadi=find(PD~=0);
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);
%准备序矩阵
%平衡节点置0置1
fsY2(Balance,:)=0;
fsY2(:,Balance)=0;
fsY2=fsY2+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
%平衡节点置0置1
fsY0(Balance,:)=0;
fsY0(:,Balance)=0;
fsY0=fsY0+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
%%LU分解
[fsY0L,fsY0U,fsY0P,fsY0Q,fsY0R]=lu(fsY0);
[fsY2L,fsY2U,fsY2P,fsY2Q,fsY2R]=lu(fsY2);
%算初始补偿功率
tic
VoltpA=sparse(ones(busNum,1));
VoltpB=sparse(ones(busNum,1)).*exp(1j*-120/180*pi);
VoltpC=sparse(ones(busNum,1)).*exp(1j*+120/180*pi);
while(k<=kmax && maxD> EPS)
    k=k+1;
    %把补偿电容看作负荷
    SA=VoltpA.*conj(VoltpA.*sparse(cap.capNode,1,1j*cap.capB(:,1),busNum,1));
    SB=VoltpB.*conj(VoltpA.*sparse(cap.capNode,1,1j*cap.capB(:,2),busNum,1));
    SC=VoltpC.*conj(VoltpA.*sparse(cap.capNode,1,1j*cap.capB(:,3),busNum,1));
    iterPD=PD+real(SA+SB+SC)/3;
    iterQD=QD+imag(SA+SB+SC)/3;
    iterPhaseASpotLoadP=phaseASpotLoadP+real(SA);
    iterPhaseBSpotLoadP=phaseBSpotLoadP+real(SB);
    iterPhaseCSpotLoadP=phaseCSpotLoadP+real(SC);
    iterPhaseASpotLoadQ=phaseASpotLoadQ+imag(SA);
    iterPhaseBSpotLoadQ=phaseBSpotLoadQ+imag(SB);
    iterPhaseCSpotLoadQ=phaseCSpotLoadQ+imag(SC);
    %全部转换为负荷电流
    CurpA=conj((iterPhaseASpotLoadP+1j*iterPhaseASpotLoadQ)./VoltpA);
    CurpB=conj((iterPhaseBSpotLoadP+1j*iterPhaseBSpotLoadQ)./VoltpB);
    CurpC=conj((iterPhaseCSpotLoadP+1j*iterPhaseCSpotLoadQ)./VoltpC);
    %转换为序电流
    f012=Tp2f*conj([CurpA';CurpB';CurpC']);
    %把三序电流分离出来
    If0=conj(f012(1,:)');
    If1=conj(f012(2,:)');
    If2=conj(f012(3,:)');
    %得到三序电压
    V012=Tp2f*conj([VoltpA';VoltpB';VoltpC']);
    %分离出三序电压
    V0=conj(V012(1,:)');
    V1=conj(V012(2,:)');
    V2=conj(V012(3,:)');
    %试着算一下正序电流
    fsY11*V1;
    %形成负荷序电流的测量值
    mIf0=If0;
    mIf1=If1;
    mIf1(1)=0.02;
    mIf1(3)=0.03;
    Loadi=[ 1 2 3];
    mIf2=If2;
    %计算
    fsY11=fsY11+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);%这里要置0，置1，否则是奇异的
    %%做最小二乘法
    %先做正序的
    Z=[%这里要加-号，因为用的Z是注入电流
        -real(mIf1(Loadi));
        -imag(mIf1(Loadi));
        ];
    H=fsY11(Loadi,:);
    H=[
        -real(H),imag(H);
        -imag(H),-real(H);
        ];
    %     J=fsY11(Loadi,:);
    %     J=[
    %         conj(-real(J))',-conj(imag(J))';
    %         conj(imag(J))',-conj(real(J))';
    %     ];
    J=conj(H');
    %J*Z+J*H*X=0 ->J*H*X=-J*Z
    X=-inv(J*H)*J*Z;
    %     X=inv(conj(H)'*H)*conj(H)'*Z;
    Xr=X(1:length(X)/2);
    Xi=X(length(X)/2+1:end);
    Xri=Xr+1j*Xi;
    fsY11*Xri;
    [dP, dQ, YdotSinVolt, YdotCosVolt, diag_Volt_YdotSin, diag_Volt_YdotCos]=Unbalance(Balance,busNum, ...
        PQi,PG,QG,QGi,iterPD,iterQD,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',k,full(maxD));
    %转换为三相电压
    VoltpABC=Tp2f\conj([ Vf0'; (Vmf1.*exp(1j*Vaf1))'; Vf2']);%用Tp2f\ 代替Tf2p*
    VoltpA=conj(VoltpABC(1,:)');
    
    
    
    If0(Balance)=0;
    If2(Balance)=0;
    %Vf0=fsY0\If0;
    Vf0=fsY0Q*(fsY0U\(fsY0L\(fsY0P*(fsY0R\If0))));
    %Vf2=fsY2\If2;
    Vf2=fsY2Q*(fsY2U\(fsY2L\(fsY2P*(fsY2R\If2))));
    fprintf('迭代时间%f\n',toc);
    
    %
end
FortiscueToc=toc;
fprintf('Fortiscue法计算时间 %f\n',FortiscueToc);
Vf1=Vmf1.*exp(1j*Vaf1);
%%
(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 ])
%%检查反推回去的功率是否满足
ub=checkSSatisfied(Balance,phaseABCY,VoltpABC, ...
    phaseASpotLoadP,phaseBSpotLoadP,phaseCSpotLoadP, ...
    phaseASpotLoadQ,phaseBSpotLoadQ,phaseCSpotLoadQ );
fprintf('最大不平衡量为%f\n\n',full(max(abs(ub))))
%% 用牛顿法求解begin
% 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;
% PG=0;
% QG=0;
% 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;
% 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;
% k=0;
% maxD=10000;
% tic
% while(k<=kmax && maxD> EPS)
%     k=k+1;
%     [dP, dQ, YdotSinVolt, YdotCosVolt, diag_Volt_YdotSin, diag_Volt_YdotCos]=Unbalance(Balance3P,busNum*3, ...
%         PQi3P,PG,QG,QGi3P,PD3P,QD3P,Vp3m,Vp3a,Y,Yangle,r,c,0,0,0,0);
%     maxD=max(abs([dP;dQ;]));
%     jaco=Jacobi(Balance3P,busNum*3,QGi3P,Vp3m,YdotSinVolt,YdotCosVolt,diag_Volt_YdotSin,diag_Volt_YdotCos);%雅克比矩阵
%     [dV, dVangle]=Solv(busNum*3,jaco,dP,dQ);%解出修正量
%     [Vp3m, Vp3a]=Modify(Vp3m,Vp3a,dV,dVangle,1);
%     fprintf('第 %d 次迭代， 最大不平衡量为 %f\n',k,full(maxD));
%     fprintf('迭代时间%f\n',toc);
% end
% NewtonToc=toc;
% fprintf('牛顿法计算时间 %f\n',NewtonToc);
% fprintf('加速比为%f\n',NewtonToc/FortiscueToc);
% VoltpA=Vp3m(1:3:end).*exp(1j*Vp3a(1:3:end));
% VoltpB=Vp3m(2:3:end).*exp(1j*Vp3a(2:3:end));
% VoltpC=Vp3m(3:3:end).*exp(1j*Vp3a(3:3:end));
%% 用牛顿法求解end
%% 开始进入状态估计
clear PD QD PG QG;
%状态量
SEVoltpA=sparse(ones(busNum,1));
SEVoltpB=sparse(ones(busNum,1)).*exp(1j*-120/180*pi);
SEVoltpC=sparse(ones(busNum,1)).*exp(1j*+120/180*pi);
SEphaseASpotLoadP=zeros(length(phaseASpotLoadP),1);
SEphaseBSpotLoadP=zeros(length(phaseBSpotLoadP),1);
SEphaseCSpotLoadP=zeros(length(phaseCSpotLoadP),1);
SEphaseASpotLoadQ=zeros(length(phaseASpotLoadQ),1);
SEphaseBSpotLoadQ=zeros(length(phaseBSpotLoadQ),1);
SEphaseCSpotLoadQ=zeros(length(phaseCSpotLoadQ),1);
%
SEVmf1=sparse(ones(busNum,1));
SEVaf1=sparse(zeros(busNum,1));
SEPD=sparse(zeros(busNum,1));
SEQD=sparse(zeros(busNum,1));
KK=0;
plotGap=zeros(1,60);
%初始化
%状态量为 SEPD SEQD SEVmf1 SEVaf1
RestraintCount=length(SEVmf1)+length(Loadi)*2;
ContrlCount=length(SEVmf1)*2+length(Loadi)*2;
CenterA=0.1;
Init_Z=sparse(ones(RestraintCount,1));
Init_W=sparse(-1*ones(RestraintCount,1));
Init_L=1*sparse(ones(RestraintCount,1));
Init_U=1*sparse(ones(RestraintCount,1));
Init_Y=sparse(2*busNum,1);%等式约束乘子
Gap=(Init_L'*Init_Z-Init_U'*Init_W);
PG=sparse(busNum,1);
PG(Balance)=0.1105;
QG=sparse(busNum,1);
QG(Balance)=0.0984;
SEPD(2)=0.1105;
SEQD(2)=0.0984;
while Gap>1e-5 && KK<20
    KK=KK+1;
    Init_u=Gap/2/RestraintCount*CenterA;
    deltH=func_deltH(busNum,SEVmf1,fsY1amp,SEVaf1,r,c,fsY1ang,Loadi);
    deltG=func_deltG(busNum,Loadi);
    L_1Z=diag(Init_Z./Init_L);
    U_1W=diag(Init_W./Init_U);
    deltdeltF=func_deltdeltF(SEPD,ContrlCount);
    ddh=func_ddh(SEVmf1,Init_Y,busNum,fsY1amp,SEVaf1,r,c,fsY1ang,Loadi,ContrlCount);
    ddg=func_ddg();
    deltF=func_deltF(SEPD,ContrlCount);
    Luu=Init_U.*Init_W+Init_u*ones(RestraintCount,1);
    Lul=Init_L.*Init_Z-Init_u*ones(RestraintCount,1);
    Mat_G=FormG(SEVmf1,SEPD,SEQD,Loadi);
    Mat_H=FormH(busNum,SEVmf1,PG,SEPD,QG,SEQD,fsY1amp,SEVaf1,r,c,fsY1ang);
    Ly=Mat_H;
    Lz=FormLz(Mat_G,Init_L,busNum,Loadi);
    Lw=FormLw(Mat_G,Init_U,busNum,Loadi);
    Lx=FormLx(deltF,deltH,Init_Y,deltG,Init_Z,Init_W);
    YY=FormYY(Lul,Lz,Ly,Luu,Lw,Lx);
    %% 开始解方程
    plotGap(KK)=Gap;
    fprintf('迭代次数 %d Gap %f\n',KK,plotGap(KK));
    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,SEVmf1,SEVaf1,SEPD,SEQD]=Modification(Init_Z,Init_L,Init_W,Init_U,Init_Y,deltZ,deltL,deltW,deltU,deltX,deltY,PG,QG,SEVmf1,SEVaf1,ContrlCount,Balance,busNum,SEPD,SEQD,Loadi);
    Gap=(Init_L'*Init_Z-Init_U'*Init_W);
end