%% 利用先把负荷转换为电流的方法。这个方法要求知道电压量。
%
close all
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));
    %先不要电容
    SA=0;
    SB=0;
    SC=0;
    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,:)');
%    %试着算一下正序电流
%     fsY11*V1;
%    %形成负荷序电流的测量值
%    mIf0=If0;
%    mIf1=If1;
%    mIf1(3)=-mIf1(2);
%    mIf2=If2;
%    %计算
%     fsY11=fsY11+sparse(Balance,Balance,ones(length(Balance),1),busNum,bus
%     Num);%这里要置0，置1，否则是奇异的
    %%做最小二乘法
    [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,:)');
    CurpA=-conj((iterPhaseASpotLoadP+1j*iterPhaseASpotLoadQ)./VoltpA);
    VoltpB=conj(VoltpABC(2,:)');
    CurpB=-conj((iterPhaseBSpotLoadP+1j*iterPhaseBSpotLoadQ)./VoltpB);
    VoltpC=conj(VoltpABC(3,:)');
    CurpC=-conj((iterPhaseCSpotLoadP+1j*iterPhaseCSpotLoadQ)./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;
    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))))
%% 开始进入状态估计
% clear PD QD PG QG;
%准备量测量
iterPhaseASpotLoadP=phaseASpotLoadP;
iterPhaseBSpotLoadP=phaseBSpotLoadP;
iterPhaseCSpotLoadP=phaseCSpotLoadP;
iterPhaseASpotLoadQ=phaseASpotLoadQ;
iterPhaseBSpotLoadQ=phaseBSpotLoadQ;
iterPhaseCSpotLoadQ=phaseCSpotLoadQ;
%全部转换为负荷电流
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);
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,:)');
%试着算一下正序电流
%     fsY11*V1;
%形成负荷序电流的测量值
%% 设定量测误差
sigma=0.03;
mIf0=-If0;
mIf1=-If1;%mIf1是注入电流,相当于发电机电流
mIf2=-If2;
%% 先算正序的
%平衡节点电流
fprintf('正序\n');
BalI1r=real(-sum(mIf1));
BalI1i=imag(-sum(mIf1));
%电压
%制作量测量
mfsY11=fsY11;
mfsY11(:,Balance)=0;
mfsY11(Balance,:)=0;
mfsY11=mfsY11+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
rV1=inv(mfsY11)*(mIf1)+1;
sigmaV1=normrnd(0,sigma,length(Loadi),1);
V1measurement=rV1(Loadi).*(1+sigmaV1);
wV1r=abs(real( rV1(Loadi).*sigmaV1 ));
wV1i=abs(imag( rV1(Loadi).*sigmaV1 ));
sigmaI1=normrnd(0,sigma,length(Loadi),1);
I1measurement=mIf1(Loadi).*(1+sigmaI1);%测量值是等效发电机电流
wI1r=abs( real(mIf1(Loadi).*sigmaI1) );
wI1i=abs( imag(mIf1(Loadi).*sigmaI1) );
% [ V1r,V1i,I1r,I1i ]=IPMLoop(V1measurement,wV1r,wV1i,I1measurement,wI1r,wI1i,BalI1r,BalI1i,busNum,Loadi,fsY11,Balance,1 );
% f=sum(([real(I1measurement);imag(I1measurement)]-[I1r;I1i]).^2)+sum((real(rV1)-V1r).^2)+sum((imag(rV1)-V1i).^2);
% fprintf('目标值 %f\n',full(f));
%% 算负序的
fprintf('负序\n');
BalI2r=real(-sum(mIf2));
BalI2i=imag(-sum(mIf2));
%电压
%制作量测量
mfsY22=fsY22;
mfsY22(:,Balance)=0;
mfsY22(Balance,:)=0;
mfsY22=mfsY22+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
rV2=inv(mfsY22)*(mIf2);
sigmaV2=normrnd(0,sigma,length(Loadi),1);
V2measurement=rV2(Loadi).*(1+sigmaV2);
wV2r=abs(real( rV2(Loadi).*sigmaV2 ));
wV2i=abs(imag( rV2(Loadi).*sigmaV2 ));
sigmaI2=normrnd(0,sigma,length(Loadi),1);
I2measurement=mIf2(Loadi).*(1+sigmaI2);%测量值是等效发电机电流
wI2r=abs( real(mIf2(Loadi).*sigmaI2) );
wI2i=abs( imag(mIf2(Loadi).*sigmaI2) );
% [ V2r,V2i,I2r,I2i ]=IPMLoop(V2measurement,wV2r,wV2i,I2measurement,wI2r,wI2i,BalI2r,BalI2i,busNum,Loadi,fsY22,Balance,0 );
% f=sum(([real(I2measurement);imag(I2measurement)]-[I2r;I2i]).^2)+sum((real(rV2)-V2r).^2)+sum((imag(rV2)-V2i).^2);
% fprintf('目标值 %f\n',full(f));
%% 算零序
fprintf('零序\n');
BalI0r=real(-sum(mIf0));
BalI0i=imag(-sum(mIf0));
%电压
%制作量测量
mfsY00=fsY00;
mfsY00(:,Balance)=0;
mfsY00(Balance,:)=0;
mfsY00=mfsY00+sparse(Balance,Balance,ones(length(Balance),1),busNum,busNum);
rV0=inv(mfsY00)*(mIf0);
sigmaV0=normrnd(0,sigma,length(Loadi),1);
V0measurement=rV0(Loadi).*(1+sigmaV0);
wV0r=abs(real( rV0(Loadi).*sigmaV0 ));
wV0i=abs(imag( rV0(Loadi).*sigmaV0 ));
sigmaI0=normrnd(0,sigma,length(Loadi),1);
I0measurement=mIf0(Loadi).*(1+sigmaI0);%测量值是等效发电机电流
wI0r=abs( real(mIf0(Loadi).*sigmaI0) );
wI0i=abs( imag(mIf0(Loadi).*sigmaI0) );
% matlabpool local 3
tic
for II=1:3
    
    if II==1
        [ V1r,V1i,I1r,I1i ]=IPMLoop(V1measurement,wV1r,wV1i,I1measurement,wI1r,wI1i,BalI1r,BalI1i,busNum,Loadi,fsY11,Balance,1 );
    end
    if II==2
        [ V2r,V2i,I2r,I2i ]=IPMLoop(V2measurement,wV2r,wV2i,I2measurement,wI2r,wI2i,BalI2r,BalI2i,busNum,Loadi,fsY22,Balance,0 );
    end
    if II==3
        [ V0r,V0i,I0r,I0i ]=IPMLoop(V0measurement,wV0r,wV0i,I0measurement,wI0r,wI0i,BalI0r,BalI0i,busNum,Loadi,fsY00,Balance,0 );
    end
    
end
toc
% matlabpool close

% f=sum(([real(I0measurement);imag(I0measurement)]-[I0r;I0i]).^2)+sum((real(rV0)-V0r).^2)+sum((imag(rV0)-V0i).^2);
% fprintf('目标值 %f\n',full(f));
%% 把三序合成三相
% 三相电压
SEVf0=V0r+1j*V0i;
SEVf1=V1r+1j*V1i;
SEVf2=V2r+1j*V2i;
SEVoltpABC=Tf2p*conj([ SEVf0'; SEVf1'; SEVf2']);
% SEVoltpABC2=Tf2p*conj([ rV0'; rV1'; rV2']);
% 三序电流
SEIf0=I0r+1j*I0i;
SEIf1=I1r+1j*I1i;
SEIf2=I2r+1j*I2i;
SEIpABC=full(Tf2p*conj([SEIf0';SEIf1(1:3)';SEIf2']));
%看一下差多少
VError=(full(abs(VoltpABC))-abs(SEVoltpABC))./abs(VoltpABC)*100;
VError=reshape(VError,size(VError,1)*size(VError,2),1);
barPlot( VError, 10,['相对误差%'],['分布密度'],['电压幅值']);
% 三相负荷
rThreeLoad=[ phaseASpotLoadP'+1j*phaseASpotLoadQ';
    phaseBSpotLoadP'+1j*phaseBSpotLoadQ';
    phaseCSpotLoadP'+1j*phaseCSpotLoadQ';
    ];
rThreeLoad=rThreeLoad(:,setxor(1:size(SEVoltpABC,2),Balance));
SEThreeLoad=SEVoltpABC(:,setxor(1:size(SEVoltpABC,2),Balance)).*conj(-SEIpABC);
phaseLoadPError=real(rThreeLoad-SEThreeLoad)./real(rThreeLoad)*100;
phaseLoadQError=imag(rThreeLoad-SEThreeLoad)./imag(rThreeLoad)*100;
phaseLoadPError=reshape(phaseLoadPError,size(phaseLoadPError,1)*size(phaseLoadPError,2),1);
phaseLoadQError=reshape(phaseLoadQError,size(phaseLoadQError,1)*size(phaseLoadQError,2),1);
figure()
barPlot( phaseLoadPError, 10,['相对误差%'],['分布密度'],['有功负荷误差']);
figure()
barPlot( phaseLoadQError, 10,'相对误差%','分布密度','无功负荷误差');