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:/算例/东际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;%真值
%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)]);
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;
[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');
%% 加噪音
load('PD0');
load('QD0');
% PD0=(1+normrnd(0,0.05,length(PD0),1)).*PD0;
% save('PD0','PD0');
% QD0=(1+normrnd(0,0.05,length(QD0),1)).*QD0;
% save('QD0','QD0');
%% 目标函数

%% 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='iter';
% x0=[zeros(length(PDi)+length(QDi),1); ...
%     ones(length(Volt),1); ...
%     zeros(length(Volt),1)];
x0=[PD(PDi);QD(QDi);xVolt';xUAngel'];
[~,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
fval=seOpti.obj(x);
fprintf('目标函数: %f\n',fval);
toc
rVolt=Volt0';
rVAngel=xUAngel';
SEVolt=x(length(PDi)+length(QDi)+1:length(Volt)+length(PDi)+length(QDi));
SEVAngel=x(length(Volt)+length(PDi)+length(QDi)+1:end);
fprintf('最大偏差\n')
PD=x(1:length(PDi));
QD=x(length(PDi)+1:length(PDi)+length(QDi));
MaxDeviation(rVolt,SEVolt,rVAngel,SEVAngel,rPD(PDi),rQD(QDi),PD,QD)