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);
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)]);
[Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wPG,wQG,wPD,wQD,wVolt,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');
%% 加噪音
PD0=(1+normrnd(0,0.05,length(PD0),1)).*PD0;
QD0=(1+normrnd(0,0.05,length(QD0),1)).*QD0;
%% 目标函数
% Objective=ObjectiveFun(PG,PG0,PGi,QG,QG0,PVi,PD,PD0,QD,QD0,Volt,Volt0,wPG,wQG,wPD,wQD,wVolt,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
%% Opti Toolbox
Busnum=length(Volt);
PDi=find(PD~=0);
QDi=find(QD~=0);
% PD0=PD0(PDi);
% QD0=QD0(QDi);
seOpti=Opti();
seOpti=seOpti.init(Volt0,PDi,QDi,wPD,wQD,PD0,QD0,Y,Angle,r,c,PG,QG,Balance);
opts = optiset('solver','filtersd');
opts.maxiter=85500;
opts.maxtime=3000;
opts.maxfeval=85000;
opts.maxnodes=85000;
opts.tolrfun=1e-4;
opts.tolafun=1e-4;
opts.warnings='all';
opts.display='off';
x0=zeros(length(PDi)+length(QDi)+length(Volt)*2,1);
% [~,seOpti]=seOpti.equ(x0);
[~,seOpti]=seOpti.equ(x0);
nlrhs=seOpti.Geteb();
nle=seOpti.Getgle();
Opt = opti('fun',@seOpti.obj,'ndec',length(Volt)*2+length(PDi)+length(QDi),'nlmix',@seOpti.equ,nlrhs,nle,'options',opts)
% Opt = opti('fun',@seOpti.obj,'ndec',length(PDi)+length(QDi),'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0);
info
fprintf('目标函数: %f\n',fval);
toc