clc
clear
yalmip('clear')
tic
[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:/算例/柳金Ⅰ926_21671693_2012-09-06/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;%真值
%PD0(12)=PD0(12)+0.001;
PG0(Balance)=PGBal(Balance);
QG0(Balance)=QGBal(Balance);
QG0(PVi)=QGBal(PVi);
PG(Balance)=PGBal(Balance);
QG(PVi)=QGBal(PVi);
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)]);
[Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wPG,wQG,wPD,wQD,PD,PD0,QD,randPDind,Loadi,notLoadi]=OPF_Init(Busnum,Balance,PG,QG,Volt,GenU,GenL,PVi,PGi,PVQU,PVQL,PD0,QD0,QD,PD);
%% 定义变量
BalVolt=Volt(Balance);
Volt=sdpvar(Busnum,1);
UAngel=sdpvar(Busnum,1);
% PG=sdpvar(Busnum,1);
% QG=sdpvar(Busnum,1);
PD=sdpvar(Busnum,1);
QD=sdpvar(Busnum,1);
AngleIJ=sdpvar(Busnum,Busnum,'full');
%% 目标函数
Objective=ObjectiveFun(PG,PG0,PGi,QG,QG0,PVi,PD,PD0,QD,QD0,wPG,wQG,wPD,wQD,Loadi);
%AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle,Busnum,Busnum);
%% 赋初值，可以加快求解速度。
assign(Volt(:),1);
assign(UAngel(:),0);
assign(PD(:),PD0(:));
assign(QD(:),QD0(:));
%% YALMIP部分
dP=PG0-PD-diag(Volt)*Y.*cos( sparse(r,c,UAngel(r)-UAngel(c)-Angle,Busnum,Busnum) )*Volt;
dQ=QG0-QD-diag(Volt)*Y.*sin( sparse(r,c,UAngel(r)-UAngel(c)-Angle,Busnum,Busnum) )*Volt;
Loadi=PD0~=0 | QD0~=0 |PG0~=0|QG0~=0;
Constraints = [%AngleIJ-sparse(r,c,UAngel(r)-UAngel(c)-Angle,Busnum,Busnum)==0, ...
    dP(setdiff(1:Busnum,Loadi))==0, ...
    dQ(setdiff(1:Busnum,Loadi))==0, ...
%     dP==0, ...
%     dQ==0, ...
     PD(PD0==0)==0, ...
     QD(QD0==0)==0, ...
    0.9*ones(Busnum,1)<=Volt<=1.1*ones(Busnum,1), ...
    Volt(Balance)==BalVolt, ...
    UAngel(Balance)==0, ...
    0.8*PD0<=PD<=1.2*PD0;
    0.8*QD0<=QD<=1.2*QD0;
    ];
options = sdpsettings('verbose',2,'showprogress',1,'debug',0,'solver','ipopt','usex0','1');
sol = solvesdp(Constraints,Objective,options);
if sol.problem == 0
    fprintf('Volt\n');
    dvolt=double(Volt)
    fprintf('VoltAngle\n');
    dVangle=double(UAngel)
    fprintf('ojb\n');
    optimalObj=double(Objective)
    sol
else
    display('Hmm, something went wrong!');
    sol.info
    sol.solveroutput
    yalmiperror(sol.problem)
end

toc