%WLS Estimation HSE

%Convert the current meas from m_IMA into rectangular coordinates and
%combine with m_IRI
temp_IRI=[];
if isempty(m_IMA)==0
    for i=1:length(m_IMA(:,1))
        m=m_IMA(i,2);a=m_IMA(i,3);Rm=m_IMA(i,4);Ra=m_IMA(i,5);
        temp1=Rm*cos(a)*cos(a)+m*m*sin(a)*sin(a)*Ra;
        temp2=Rm*sin(a)*sin(a)+m*m*cos(a)*cos(a)*Ra;
        [temp_IRI]=[temp_IRI;m_IMA(i,1) m*cos(a) m*sin(a) temp1 temp2];
    end
    [temp_IRI]=[temp_IRI;m_IRI];
    clear i m a Rm Ra temp1 temp2;
end
R_VA=[];
if isempty(m_VMA)==0
    for i=1:length(m_VMA(:,1))
        if m_VMA(i,1)~=slack
            [R_VA]=[R_VA;m_VMA(i,5)];
        end
    end
end

temp_R=[m_Pinj(:,3);m_Pflow(:,3);m_Qinj(:,3);m_Qflow(:,3)];
[temp_R]=[temp_R;m_VMA(:,4);R_VA;temp_IRI(:,4);temp_IRI(:,4)];
R=diag(temp_R);W=inv(R);clear temp_R i R_VA;

%Initialize state variable i.e. voltage magnitude and angle
VM(1:nob,1)=ones(nob,1);
VA(1:nob,1)=zeros(nob,1);

iteration=0;flag=0;reg_param_t=[];ddx=[];
tic;
while (flag==0)
    iteration=iteration+1;
    if iteration>MAX_ITER
        disp('MAX ITERATION AND NO CONVERGENCE');
        flag=2;
    else
        VV = VM.*exp(1i*VA);
        
        %Calculate the Jacobian and error for injection P and Q
        [J_P,J_Q,err_P,err_Q] = form_J_inj_polar(VV,connect_m,YBUS,m_Pinj,m_Qinj);            
            
        %Calculate the Jacobian and error for power flow in line
        [J_Pflow,J_Qflow,err_Pflow,err_Qflow]=form_J_flow_polar(line_pi,VV,m_Pflow,m_Qflow);
        
        %Calculate the Jacobian and error for PMU voltage phasor
        [J_VM,J_VA,err_VM,err_VA] = form_J_PMUV(VV,m_VMA,nob,slack);
        
        %Calculate the Jacobian and error for PMU current phasor
        [J_Ireal,J_Iimag,err_Ireal,err_Iimag] = form_J_PMUI(line_pi,VV,temp_IRI);
        
        temp=[J_P;J_Pflow;J_Q;J_Qflow;J_VM;J_VA;J_Ireal;J_Iimag];
        err=[err_P;err_Pflow;err_Q;err_Qflow;err_VM;err_VA;err_Ireal;err_Iimag];            
        
        if slack==1
            J=temp(:,2:2*nob);
        else
            J=[temp(:,1:slack-1) temp(:,slack+1:2*nob)];
        end
        
        if flag_eq==0
            c=[];Cx=[];
        else
            [c,Cx]=eq_constr_polar(VV,connect_m,YBUS,z_Pinj,z_Qinj,slack);
        end
        
        gain_t=J'*W*J;
        E=[gain_t Cx';Cx zeros(length(c))];
        b=[J'*W*err;-c];
                
        if flag_SE_method==1
            dx=E\b;
%         elseif flag_SE_method==5
%             dx=SOLVE_LU(E,b);
        elseif flag_SE_method==3
            dx=pinv(E)*b;
        elseif flag_SE_method==4
            flag_reg=1;%0 for pseudo inverse else tikhonov regularization
            [dx,t_mue]=my_tikh_LC(E,b,flag_reg);%PCHANSEN
        else
            dx=E\b;
        end
        
        [ddx]=[ddx dx];
        if slack==1
            temp=[0;dx];
        else
            temp=[dx(1:slack-1);0;dx(slack:2*nob-1)];
        end
        VM=VM+temp(nob+1:2*nob);
        VA=VA+temp(1:nob);
        clear temp S_calc;        
            
        if max(abs(dx(1:2*nob-1,1)))<=tol
            flag=1;%Converged 
            if flag_SE_method==5
                [reg_param_t]=[reg_param_t t_mue];                
            end
        end
    end
end
sol_time=toc;
VV = VM.*exp(1i*VA);

x_est_HSE=[VM VA real(VV) imag(VV)];

clear J J_P J_Q err_P err_Q J_Pflow J_Qflow err_Pflow err_Qflow;
clear temp err dx Cx E b gain_t len_E  VV VM VA;