clear
clc
% yalmip('clear')
addpath('.\Powerflow')
[~, ~, ~, ~,Volt,Vangle,Y,Yangle,r,c,newwordParameter,PG,QG,PD,QD,Balance]=pf('ieee4-DN.dat', '0');
% 'E:\算例\feeder33\feeder33ieee.txt'
%% 开始生成量测量
sigma=0.03;% 标准差
%% 电压
%电压幅值
rVolt=Volt; %幅值
BalanceVolt=Volt(Balance);
mVolt=rVolt.*(normrnd(0,sigma,length(Volt),1)+1);%电压量测量
rVAngel=Vangle;
%% 电流
%注入电流
cmpY=Y.*exp(1j*sparse(r,c,Yangle,length(Y),length(Y)));%复数导纳矩阵
cmpV=Volt.*exp(1j*Vangle); %复数电压
cmpI=cmpY*cmpV;% 注入电流
rI=abs(cmpI); %注入电流量测量要的是电流幅值
mI=rI.*(normrnd(0,sigma,length(rI),1)+1);%电流量测量
%% 支路电流
% 支路电流
lineI=newwordParameter.line.lineI;
lineJ=newwordParameter.line.lineJ;
lineR=newwordParameter.line.lineR;
lineX=newwordParameter.line.lineX;
lineB2=newwordParameter.line.lineB2;
lineG=real(1./(lineR+1j*lineX));
lineB=imag(1./(lineR+1j*lineX));
cmpBranchI=BranchI( cmpV,lineI,lineJ,lineR,lineX );%复数支路电流
rBranchI=abs(cmpBranchI);% 支路电流幅值
mBranchI=rBranchI.*(normrnd(0,sigma,length(rBranchI),1)+1);%支路电流量测量
%% 支路功率
rBranchP= BranchP( cmpV,cmpBranchI,lineI,lineB2 );
mBranchP=rBranchP.*(normrnd(0,sigma,length(rBranchP),1)+1);%支路功率量测量
rBranchQ=BranchQ( cmpV,cmpBranchI,lineI,lineB2 );
mBranchQ=rBranchQ.*(normrnd(0,sigma,length(rBranchQ),1)+1);%支路功率量测量
%% 变压器功率
transI=newwordParameter.trans.transI;
transJ=newwordParameter.trans.transJ;
transK=newwordParameter.trans.transK;
transR=newwordParameter.trans.transR;
transX=newwordParameter.trans.transX;
transG=real(1./(transR+1j*transX));
transB=imag(1./(transR+1j*transX));
rTransP=TransPower( newwordParameter,rVolt,rVAngel );
rTransQ=TransReactivePower( newwordParameter,rVolt,rVAngel );
mTransP=rTransP.*(normrnd(0,sigma,length(rTransP),1)+1);
mTransQ=rTransQ.*(normrnd(0,sigma,length(rTransQ),1)+1);
%% 注入功率
rPD=PD;
PDi=find(PD~=0);
rQD=QD;
QDi=find(QD~=0);
rPG=PG;
PGi=find(PG~=0);
rQG=QG;
QGi=find(QG~=0);
mPD=rPD.*(normrnd(0,sigma,length(rPD),1)+1);
mQD=rQD.*(normrnd(0,sigma,length(rQD),1)+1);
mPG=rPG.*(normrnd(0,sigma,length(rPG),1)+1);
mQG=rQG.*(normrnd(0,sigma,length(rQG),1)+1);
%% 0注入节点
zerosInjectionIndex=1:length(Volt);
zerosInjectionIndex=zerosInjectionIndex( ~(PD~=0|QD~=0|PG~=0|QG~=0) );
% zerosInjectionIndex=zeros(0,0);
%% 发电机注入功率
% 先找到只有发电机的节点
PDQDi=union(PDi,QDi);
onlyPG=setdiff(PGi,PDQDi);
onlyQG=setdiff(QGi,PDQDi);
%% 计算方差
% measureSigma=abs(([rVolt;rBranchP;rBranchQ;rTransP;rTransQ].*sigma));
measureSigma=abs(([rVolt;rPD(PDi);rQD(QDi);].*sigma));
measureSigma(measureSigma<1e-6)=mean(measureSigma(measureSigma>1e-6));
W=sparse(diag(1./measureSigma.^2)) ;
% W=eye(length(W));
% W=sparse(1:length(W),1:length(W),400,length(W),length(W));
%% 冗余度计算
stateVarCount=2*length(Volt);
measurements=length(mVolt)+length(mBranchI)+length(mBranchP)+length(mBranchQ)+length(mPG)+length(mQG)+length(mTransP)+length(mTransQ);
fprintf('冗余度 %f\n',measurements/stateVarCount);
%% save
% save('mVolt','mVolt');
% save('mPG','mPG');
% save('mQG','mQG');
% save('mBranchI','mBranchI');
% save('mBranchP','mBranchP');
% save('mBranchQ','mBranchQ');
% save('mTransP','mTransP');
% save('mTransQ','mTransQ');
%% load
% load('mVolt');
% load('mPG');
% load('mQG');
% load('mBranchI');
% load('mBranchP');
% load('mBranchQ');
% load('mTransP');
% load('mTransQ');
%% 自己写的微分代码
% 初始化一些值
SEVolt=sparse(ones(length(mVolt),1));
SEVolt(Balance)=rVolt(Balance);
SEVAngle=sparse(-0.00*ones(length(mVolt),1));
% SEVolt=rVolt;
% SEVAngle=rVAngel;
maxD=1000;
Iteration=0;
optimalCondition=100;
eps=1e-5;
mu=0;
v=2;
ojbFunDecrease=1000;% 目标函数下降
% 以下都是Jacobi矩阵
% while max(abs(g))>1e-5;
% while maxD>1e-5
while max(abs(optimalCondition))>eps
    % 电压
    dV_dV=sparse(1:length(mVolt),1:length(mVolt),1,length(mVolt),length(mVolt));%电压量测量的微分
    dV_dTyta=sparse(length(mVolt),length(mVolt));
    % 线路支路
    dLPij_dVi=sparse(1:length(lineI),lineI, ...
        -SEVolt(lineJ).*( ...
        lineG.*cos(SEVAngle(lineI)-SEVAngle(lineJ)) +lineB.*sin(SEVAngle(lineI)-SEVAngle(lineJ))...
        )...
        +2*(lineG).*SEVolt(lineI) ...
        ,length(lineI),length(mVolt));%线路的
    dLPij_dVj=sparse(1:length(lineI),lineJ, ...
        -SEVolt(lineI).*( ...
        lineG.*cos(SEVAngle(lineI)-SEVAngle(lineJ)) +lineB.*sin(SEVAngle(lineI)-SEVAngle(lineJ))...
        ) ...
        ,length(lineI),length(mVolt));%线路的
    dLPij_dThetai=sparse(1:length(lineI),lineI, ...
        SEVolt(lineI).*SEVolt(lineJ).*( ...
        lineG.*sin(SEVAngle(lineI)-SEVAngle(lineJ)) -lineB.*cos(SEVAngle(lineI)-SEVAngle(lineJ))...
        )...
        ,length(lineI),length(mVolt));%线路的
    dLPij_dThetaj=sparse(1:length(lineI),lineJ, ...
        -SEVolt(lineI).*SEVolt(lineJ).*( ...
        lineG.*sin(SEVAngle(lineI)-SEVAngle(lineJ)) -lineB.*cos(SEVAngle(lineI)-SEVAngle(lineJ))...
        )...
        ,length(lineI),length(mVolt));%线路的
    
    dLQij_dVi=sparse(1:length(lineI),lineI, ...
        -SEVolt(lineJ).*( ...
        lineG.*sin(SEVAngle(lineI)-SEVAngle(lineJ)) -lineB.*cos(SEVAngle(lineI)-SEVAngle(lineJ))...
        )...
        -2*(lineB+lineB2).*SEVolt(lineI) ...
        ,length(lineI),length(mVolt));%线路的
    dLQij_dVj=sparse(1:length(lineI),lineJ, ...
        -SEVolt(lineI).*( ...
        lineG.*sin(SEVAngle(lineI)-SEVAngle(lineJ)) -lineB.*cos(SEVAngle(lineI)-SEVAngle(lineJ))...
        ) ...
        ,length(lineI),length(mVolt));%线路的
    dLQij_dThetai=sparse(1:length(lineI),lineI, ...
        -SEVolt(lineI).*SEVolt(lineJ).*( ...
        lineG.*cos(SEVAngle(lineI)-SEVAngle(lineJ)) +lineB.*sin(SEVAngle(lineI)-SEVAngle(lineJ))...
        ) ...
        ,length(lineI),length(mVolt));%线路的
    dLQij_dThetaj=sparse(1:length(lineI),lineJ, ...
        SEVolt(lineI).*SEVolt(lineJ).*( ...
        lineG.*cos(SEVAngle(lineI)-SEVAngle(lineJ)) +lineB.*sin(SEVAngle(lineI)-SEVAngle(lineJ))...
        ) ...
        ,length(lineI),length(mVolt));%线路的
    % 变压器支路
    dTPij_dVi=sparse(1:length(transI),transI, ...
        -SEVolt(transJ)./transK.*( ...
        transG.*cos(SEVAngle(transI)-SEVAngle(transJ)) +transB.*sin(SEVAngle(transI)-SEVAngle(transJ))...
        )...
        +2*transG.*SEVolt(transI)./(transK.^2) ...
        ,length(transI),length(mVolt));%变压器
    dTPij_dVj=sparse(1:length(transI),transJ, ...
        -SEVolt(transI)./transK.*( ...
        transG.*cos(SEVAngle(transI)-SEVAngle(transJ)) +transB.*sin(SEVAngle(transI)-SEVAngle(transJ))...
        ) ...
        ,length(transI),length(mVolt));%变压器
    dTPij_dThetai=sparse(1:length(transI),transI, ...
        SEVolt(transI)./transK.*SEVolt(transJ).*( ...
        transG.*sin(SEVAngle(transI)-SEVAngle(transJ)) -transB.*cos(SEVAngle(transI)-SEVAngle(transJ))...
        )...
        ,length(transI),length(mVolt));%变压器
    dTPij_dThetaj=sparse(1:length(transI),transJ, ...
        -SEVolt(transI)./transK.*SEVolt(transJ).*( ...
        transG.*sin(SEVAngle(transI)-SEVAngle(transJ)) -transB.*cos(SEVAngle(transI)-SEVAngle(transJ))...
        )...
        ,length(transI),length(mVolt));%变压器
    dTQij_dVi=sparse(1:length(transI),transI, ...
        -SEVolt(transJ)./transK.*( ...
        transG.*sin(SEVAngle(transI)-SEVAngle(transJ)) -transB.*cos(SEVAngle(transI)-SEVAngle(transJ))...
        )...
        -2*transB.*SEVolt(transI)./(transK.^2) ...
        ,length(transI),length(mVolt));%变压器
    dTQij_dVj=sparse(1:length(transI),transJ, ...
        -SEVolt(transI)./transK.*( ...
        transG.*sin(SEVAngle(transI)-SEVAngle(transJ)) -transB.*cos(SEVAngle(transI)-SEVAngle(transJ))...
        ) ...
        ,length(transI),length(mVolt));%变压器
    dTQij_dThetai=sparse(1:length(transI),transI, ...
        -SEVolt(transI)./transK.*SEVolt(transJ).*( ...
        transG.*cos(SEVAngle(transI)-SEVAngle(transJ)) +transB.*sin(SEVAngle(transI)-SEVAngle(transJ))...
        ) ...
        ,length(transI),length(mVolt));%变压器
    dTQij_dThetaj=sparse(1:length(transI),transJ, ...
        SEVolt(transI)./transK.*SEVolt(transJ).*( ...
        transG.*cos(SEVAngle(transI)-SEVAngle(transJ)) +transB.*sin(SEVAngle(transI)-SEVAngle(transJ))...
        ) ...
        ,length(transI),length(mVolt));%变压器
    %% 考虑注入功率
    % 等式约束的Jacobi
    r=newwordParameter.r;
    c=newwordParameter.c;
    Yangle=newwordParameter.Yangle;
    VAngleIJ=sparse(r,c,SEVAngle(r)-SEVAngle(c) -Yangle,length(mVolt),length(mVolt)) ;
    YdotSin=Y.* ( spfun(@sin,VAngleIJ) );
    YdotCos=Y.* ( spfun (@cos, VAngleIJ ) );
    diag_Volt_YdotCos=diag(SEVolt)*YdotCos;
    diag_Volt_YdotSin=diag(SEVolt)*YdotSin;
    YdotCosVolt=YdotCos*SEVolt;
    YdotSinVolt=YdotSin*SEVolt;
    diag_Volt_YdotCosVolt=diag_Volt_YdotCos*Volt;
    diag_Volt_YdotSinVolt=diag_Volt_YdotSin*Volt;
    diag_YdotSinVolt_=diag(YdotSinVolt);
    diag_YdotCosVolt_=diag(YdotCosVolt);
    dPdTyta=diag_Volt_YdotSin*diag(SEVolt)-diag_YdotSinVolt_*diag(SEVolt); % 简化第三次
    dQdTyta=-diag_Volt_YdotCos*diag(SEVolt)+diag_YdotCosVolt_*diag(SEVolt);%dQ/dThyta
    dPdV=diag_YdotCosVolt_+diag_Volt_YdotCos;%dP/dV
    dQdV=diag_YdotSinVolt_+diag_Volt_YdotSin;%dQ/dV
    %采用我自己推导的公式
    dPdV_=diag(SEVolt)*YdotCos+diag(YdotCos*SEVolt);
    dQdV_=diag(SEVolt)*YdotSin+diag(YdotSin*SEVolt);
    dPdTyta_=diag(SEVolt)*(YdotSin*diag(SEVolt)-diag(YdotSin*SEVolt));
    dQdTyta_=diag(SEVolt)*(-YdotCos*diag(SEVolt)+diag(YdotCos*SEVolt));
    if any(abs(dPdV_-dPdV)>1e-5)
        abc=1;
    end
    if any(abs(dQdV_-dQdV)>1e-5)
        abc=1;
    end
    if any(abs(dPdTyta_-dPdTyta)>1e-5)
        abc=1;
    end
    if any(abs(dQdTyta_-dQdTyta)>1e-5)
        abc=1;
    end
    %     % C 是等式约束 c 的Jacobi
    %     C=[dPdV dPdTyta;
    %         dQdV dQdTyta];
    %     C=C(zerosInjectionIndex,:);
    %     % 形成等式约束 c
    %     nodeP=diag_Volt_YdotCosVolt;
    %     nodeQ=diag_Volt_YdotSinVolt;
    %     nodePQ=[nodeP;nodeQ];
    %     c=nodePQ(zerosInjectionIndex);
    
    %% 考虑等式约束
    % 等式约束的Jacobi
    %     r=newwordParameter.r;
    %     c=newwordParameter.c;
    %     Yangle=newwordParameter.Yangle;
    %     VAngleIJ=sparse(r,c,SEVAngle(r)-SEVAngle(c) -Yangle,length(mVolt),length(mVolt)) ;
    %     YdotSin=Y.* ( spfun(@sin,VAngleIJ) );
    %     YdotCos=Y.* ( spfun (@cos, VAngleIJ ) );
    %     diag_Volt_YdotCos=diag(SEVolt)*YdotCos;
    %     diag_Volt_YdotSin=diag(SEVolt)*YdotSin;
    %     YdotCosVolt=YdotCos*Volt;
    %     YdotSinVolt=YdotSin*Volt;
    %     diag_Volt_YdotCosVolt=diag_Volt_YdotCos*Volt;
    %     diag_Volt_YdotSinVolt=diag_Volt_YdotSin*Volt;
    %     diag_YdotSinVolt_=diag(YdotSinVolt);
    %     diag_YdotCosVolt_=diag(YdotCosVolt);
    %     dPdTyta=diag_Volt_YdotSin*diag(SEVolt)-diag_YdotSinVolt_*diag(SEVolt); % 简化第三次
    %     dQdTyta=-diag_Volt_YdotCos*diag(SEVolt)+diag_YdotCosVolt_*diag(SEVolt);%dQ/dThyta
    %     dPdV=diag_YdotCosVolt_+diag_Volt_YdotCos;%dP/dV
    %     dQdV=diag_YdotSinVolt_+diag_Volt_YdotSin;%dQ/dV
    %     % C 是等式约束 c 的Jacobi
    %     C=[dPdV dPdTyta;
    %         dQdV dQdTyta];
    %     C=C(zerosInjectionIndex,:);
    %     % 形成等式约束 c
    %     nodeP=diag_Volt_YdotCosVolt;
    %     nodeQ=diag_Volt_YdotSinVolt;
    %     nodePQ=[nodeP;nodeQ];
    %     c=nodePQ(zerosInjectionIndex);
    %% 进入迭代
%     H=[dV_dV,dV_dTyta;
%         dLPij_dVi+dLPij_dVj,dLPij_dThetai+dLPij_dThetaj ;
%         dLQij_dVi+dLQij_dVj,dLQij_dThetai+dLQij_dThetaj ;
%         dTPij_dVi+dTPij_dVj,dTPij_dThetai+dTPij_dThetaj;
%         dTQij_dVi+dTQij_dVj,dTQij_dThetai+dTQij_dThetaj];%jacobi
    H=[dV_dV,dV_dTyta;
        dPdV(PDi,:),dPdTyta(PDi,:);
        dQdV(QDi,:),dQdTyta(QDi,:)];%jacobi
    
    SEBranchI=BranchI( SEVolt.*exp(1j*SEVAngle),lineI,lineJ,lineR,lineX );%复数支路电流
    SEBranchP=BranchP( SEVolt.*exp(1j*SEVAngle),SEBranchI,lineI,lineB2 );
    SEBranchQ=BranchQ( SEVolt.*exp(1j*SEVAngle),SEBranchI,lineI,lineB2 );
    SETransP=TransPower( newwordParameter,SEVolt,SEVAngle );
    SETransQ=TransReactivePower( newwordParameter,SEVolt,SEVAngle );
%     rAngleIJ=sparse(r,c,rVAngel(r)-rVAngel(c) -Yangle,length(mVolt),length(mVolt)) ;
%     diag(rVolt)*Y.* ( spfun (@cos, rAngleIJ ) )*rVolt;
    SEPD=diag(SEVolt)*Y.* ( spfun (@cos, VAngleIJ ) )*SEVolt;
    SEQD=diag(SEVolt)*Y.* ( spfun(@sin,VAngleIJ) )*SEVolt;
    
    h=[SEVolt;SEPD(PDi);SEQD(QDi);];
    
%     h=[SEVolt;SEBranchP;SEBranchQ;SETransP;SETransQ];
%     z=[mVolt;mBranchP;mBranchQ;mTransP;mTransQ];

    z=[mVolt;-mPD(PDi);-mQD(QDi)];


    G=H'*W*H;
    g=-H'*W*(z-h);
    
    % 形成大的求解矩阵
    %     a=[G C';
    %         C zeros(size(C,1),size(C,1))];
    %     b=[-g;-c];
    % 利用  Levenber-Marquardt
    %     if Iteration==0
    %         mu=max(diag(G));
    %     end
    a=G;
    b=-g;
    
    
    % 平衡节点相角恒定
    a(length(mVolt)+Balance,:)=0;
    a(:,length(mVolt)+Balance)=0;
    a=a+sparse(length(mVolt)+Balance,length(mVolt)+Balance,1,size(a,1),size(a,1));
    b(length(mVolt)+Balance)=0;
    % 平衡节点电压恒定;
    a(Balance,:)=0;
    a(:,Balance)=0;
    a=a+sparse(Balance,Balance,1,size(a,1),size(a,1));
    b(Balance)=0;
    dX=a\b;
    dXStep=1;
    %     dXStep=Armijo(z,newwordParameter,W,SEVolt,SEVAngle,dX,g );
    maxD=max(abs(dX))
    fprintf('max abs g:%f\n',full(max(abs(g))));
    SEVolt=SEVolt+dX(1:length(mVolt))*dXStep;
    SEVAngle=SEVAngle+dX(length(mVolt)+1:length(mVolt)*2)*dXStep;
    lamda=-dX(length(mVolt)*2+1:end);
    %     % 计算目标函数下降量
    %     preObjFun=(z-h)'*W*(z-h);
    %     oldSEVolt=SEVolt;
    %     oldSEVAngle=SEVAngle;
    % 求更新控制指标 p
    %     p=(objfun( SEVolt,SEVAngle,W,z,newwordParameter ) - objfun( SEVolt+dX(1:length(mVolt))*dXStep,SEVAngle+dX(length(mVolt)+1:length(mVolt)*2)*dXStep,W,z,newwordParameter ) )/( .5*dX'*(mu*dX-g) );
    %     if p>0 %接受更新
    %         % 更新变量
    %         SEVolt=SEVolt+dX(1:length(mVolt))*dXStep;
    %         SEVAngle=SEVAngle+dX(length(mVolt)+1:length(mVolt)*2)*dXStep;
    %         lamda=-dX(length(mVolt)*2+1:end);
    %         mu=mu*max([1/3,1-(2*p-1)^3]);
    %         v=2;
    %     else
    %         mu=mu*v;
    %         v=2*v;
    %     end
    
    Iteration=Iteration+1;
    
    
    %     % 更新后目标函数
    %     h=[SEVolt;SEBranchP;SEBranchQ;SETransP;SETransQ];
    %     ojbFunDecrease=preObjFun-(z-h)'*W*(z-h)
    %     if ojbFunDecrease<1e-3
    %         mu=100000;
    %     else
    %         mu=0;
    %     end
    optimalCondition=-g;
    optimalCondition(Balance)=0;
    optimalCondition(Balance+length(mVolt))=0;
end
%% 输出结果
fprintf('迭代%d次\n',Iteration);
fval=full((z-h)'*W*(z-h));
fprintf('目标函数为:%f\n',fval);
fprintf('相对误差\n');
(abs(rVolt-double(SEVolt)))./(rVolt);
MaxDeviation(rVolt,SEVolt,rVAngel,SEVAngle)
plotAndComparison( rVolt,rVAngel,SEVolt,SEVAngle )
% 检查最优性条件

if any(abs(optimalCondition)>eps)
    fprintf('最优性条件不满足\n')
else
    fprintf('最优性条件满足\n')
end