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('ieee4.dat');

%% 潮流等式
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);

%% 定义变量
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);
%% YALMIP部分
Constraints = [%AngleIJ==sparse(r,c,UAngel(r)-UAngel(c)-Angle,Busnum,Busnum), ...
    AngleIJ==0
    PG-PD-diag(Volt)*Y.*cos(AngleIJ)*Volt==0, ...
    QG-QD-diag(Volt)*Y.*sin(AngleIJ)*Volt==0, ...
     PG(setxor(1:Busnum, PGi) )==0, ...
     QG(setxor(1:Busnum, PVi) )==0, ...
     PD(setdiff(1:Busnum,PD0~=0))==0, ...
     QD(setdiff(1:Busnum,QD0~=0))==0, ...
    0.9*ones(Busnum,1)<=Volt<=1.1*ones(Busnum,1), ...
    GenL(:,2)<=PG(PGi)<=GenU(:,2), ...
    PVQL<=QG(PVi)<=PVQU, ...
    ];
options = sdpsettings('verbose',2,'showprogress',1,'debug',0,'solver','ipopt');
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
    yalmiperror(sol.problem)
end
toc