clc
clear
close all
% yalmip('clear')
global loadFlag;
LineCurs=zeros(32,100);
tic
for I=1:1
    close all;
    [kmax,Precision,UAngel,Volt,Busnum,PVi,PVu,Balance,Y,Angle,P0,Q0,r,c,GB, ...
        Linei,Linej,Transfori,Transforj,GenU,GenL,GenC,PG,QG,PD,QD,CenterA,PGi,PVQU,PVQL, ...
        Liner,Linex,Lineb,Transforr,Transforx,Transfork0]=pf('E:\算例\feeder33\feeder33.txt');
%     'E:\算例\feeder33\feeder33.txt'
    %pf('C:\bpa\长虹世纪线_556844_2013-10-28\长虹世纪线_556844_2013-10-28_iPso_newFil
    %e.txt');
     %   pf('E:/算例/东际911_2751267_2012-09-05/pyth-增加3倍.txt'); 20131125
%     pf('E:/算例/东际911_2751267_2012-09-05/newFIle20-使用.txt');
        
    %% 潮流等式
    AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
    PGBal=PD+diag(Volt)*Y.*cos(AngleIJ)*Volt';
    QGBal=QD+diag(Volt)*Y.*sin(AngleIJ)*Volt';
    %% 初值-即测量值
    PG0=PG;
    QG0=QG;
    PD0=PD;
    QD0=QD;
    PDReal=PD;%真值
    QDReal=QD;%真值
    PG0(Balance)=PGBal(Balance);
    QG0(Balance)=QGBal(Balance);
    QG0(PVi)=QGBal(PVi);
    PG(Balance)=PGBal(Balance);
    QG(PVi)=QGBal(PVi);
    Volt0=Volt;
    % PF=1;
    % AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
    % dP=PG-PD-diag(Volt)*Y.*cos(AngleIJ)*Volt';
    % dPD=abs(dP./PD);
    % dQ=QG-QD-diag(Volt)*Y.*sin(AngleIJ)*Volt';
    % dQD=abs(dQ./QD);
    % maxdPQ=max([dPD(dPD<10);dQD(dQD<10)]);
    xVolt=Volt;
    xUAngel=UAngel;
    % VMatrix=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
    % dP=PG-PD-diag(xVolt)*(Y.*cos(VMatrix))*xVolt';
    rPD=PD;
    rQD=QD;
    rVolt=Volt';
    rVAngel=UAngel';
    BalVolt=Volt(Balance);
    Busnum=length(Volt);
    PDi=find(PD~=0);
    QDi=find(QD~=0);
    rLoadCurrent=LoadCurrent( rVolt,rVAngel,rPD(PDi),rQD(QDi),PDi,QDi );
    %% 电流真实值
    [Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wPG,wQG,wPD,wQD,wVolt,wLoadCurrent,PD,PD0,QD,randPDind,Loadi,noLoadi]=OPF_Init(Busnum,Balance,PG,QG,rVolt,GenU,GenL,PVi,PGi,PVQU,PVQL,PD0,QD0,rQD,rPD,rLoadCurrent);
    %% 开始
    %% 加噪音
%     load('PD0');
%     load('QD0');
%     load('mVolt');
%     load('mLoadCurrent');
    loadFlag=0;
    %% 17号负荷上加负数
    % PD0(17)=-10;
    %% 负荷用Pd=(1+k)Pd0添加负荷
%     QD0(11)=(1.+0.01*I)*rQD(11);
    % % load('mLoadCurrent');
    % load('I');
    sigma=0.1;
    %
    [ mVolt,PD0,QD0,mLoadCurrent ] = GetMeasure(sigma,rVolt,rPD,rQD,rLoadCurrent );
    % sigma=sigma*(I-1)/50;
    % 排除大于3倍sigma的数据
    % save('mVolt','mVolt');
    % save('PD0','PD0');
    % save('QD0','QD0');
    %% 电流测量值
    % save('mLoadCurrent','mLoadCurrent');
    mPD=PD0;
    mQD=QD0;
    %% 目标函数
    %% Opti Toolbox
    seOpti=Opti();
    seOpti=seOpti.init(mVolt,PDi,QDi,wPD,wQD,wVolt,wLoadCurrent,mPD,mQD,rPD(PDi),rQD(QDi),Y,Angle,r,c,PG,QG,Balance,mLoadCurrent,noLoadi);
    %opts=nloptset('algorithm','ipopt');
    opts = optiset('solver','ipopt');
    opts.maxiter=85500;
    opts.maxtime=30000;
    opts.maxfeval=85000;
    opts.maxnodes=85000;
    opts.tolrfun=1e-3;
    opts.tolafun=1e-3;
    opts.warnings='all';
    opts.display='off';
%     x0=[0.95*ones(length(Volt),1); ...
%         zeros(length(Volt),1)];
    x0=[rVolt;rVAngel];
    % x0=[PD(PDi);QD(QDi);xVolt';xUAngel'];
    [~,seOpti]=seOpti.equ(x0);
    cl=seOpti.Getcl();
    cu=seOpti.Getcu();
    Opt = opti('fun',@seOpti.obj,'ndec',length(Volt)*2,'nl',@seOpti.equ,cl,cu,'options',opts)
    % Opt = opti('fun',@seOpti.obj,'ndec',length(Volt)*2+length(PDi)+length(QDi),'options',opts)
    [x,fval,exitflag,info] = solve(Opt,x0);
    info
    fval=seOpti.obj(x);
    fprintf('目标函数: %.20f\n',fval);
    toc
    rVolt=Volt0';
    rVAngel=xUAngel';
    SEVolt=x(1:length(Volt));
    SEVAngel=x(length(Volt)+1:end);
    fprintf('最大偏差\n')
    AngleIJ=sparse(r,c,SEVAngel(r)-SEVAngel(c)-Angle,Busnum,Busnum);
    zeroP=diag(SEVolt)*Y.*cos(AngleIJ)*SEVolt;
    zeroQ=diag(SEVolt)*Y.*sin(AngleIJ)*SEVolt;
    PD=-zeroP(PDi);
    QD=-zeroQ(QDi);
    maxDev1=MaxDeviation(sigma,mVolt,SEVolt,rVAngel,SEVAngel,rPD(PDi),rQD(QDi),PD,QD)
    fprintf('统计偏差\n')
    statDev1=StatDeviation(sigma,rVolt,SEVolt,rVAngel,SEVAngel,rPD(PDi),rQD(QDi),PD,QD)
    %% 约束检查
    % load('maxDev');
    % load('statDev');
    % maxDev=[maxDev;maxDev1];
    % statDev=[statDev;statDev1];
    % save('maxDev','maxDev');
    % save('statDev','statDev');
    % seOpti.equ(x);
    % sum([SEVolt;PD;QD]>cu(length(SEVolt)*2+2:end));
    % sum([SEVolt;PD;QD]<cl(length(SEVolt)*2+2:end));
%     RealValuePlot(mPD(PDi),mQD(QDi),rPD(PDi),rQD(QDi),mVolt,rVolt,rVAngel);
    %measurementN=sum(find(wVolt)>0)+sum(find(wPD)>0)+sum(find(wQD)>0);
    stE=StErrorS(SEVolt,SEVAngel,PD,QD,rVolt,rVAngel,rPD(PDi),rQD(QDi));
    fprintf('真实值统计误差为: %f',stE);
    stE=StErrorZ(SEVolt,PD,QD,mVolt,mPD(PDi),mQD(QDi),noLoadi);
    fprintf('测量值统计误差为: %f',stE);
%     NormalizedResiduals(x,sigma,PDi,QDi,Volt0,mPD,mQD);
    SECurrent=LoadCurrent( SEVolt,SEVAngel,PD,QD,PDi,QDi );
%     [flag,t1,t2]=MaxSigma( x,PDi,QDi,mPD,mQD,mVolt,mLoadCurrent,sigma,rPD,rQD,rVolt,rLoadCurrent );
    figure();
    SEMeasDeivation(PD,full(PD0(PDi)),QD,full(QD0(QDi)),rPD(PDi),rQD(QDi));
    [ok,msg] = checkSol(Opt);
    figure();
    plotError( SEVolt,SEVAngel,rVolt,rVAngel,mVolt,PD,mPD(PDi),rPD(PDi),QD,mQD(QDi),rQD(QDi),PDi );
%     LineCurrent( Linei,Linej,Liner,Linex,SEVolt,SEVAngel )
% LineCurs(:,I)=LineCurrent( Linei,Linej,Liner,Linex,SEVolt,SEVAngel );
    LineP( Linei,Linej,Liner,Linex,SEVolt,SEVAngel );
    LineQ( Linei,Linej,Liner,Linex,SEVolt,SEVAngel );
    if flag==1
        I;
        t1;
        t2;
        %break;
    end
end