% Power System State Estimation using Weighted Least Square Method..

function [ result ] = wls_function(zdata, ybus, bsh, g, b)

%% Prelimenary
zdata = sortrows(zdata, 2);
zdata(:,1) = 1:size(zdata,1);
nbus = max(max(zdata(:,4)),max(zdata(:,5))); % Get number of buses..
type = zdata(:,2); % Type of measurement, Vi - 1, Pi - 2, Qi - 3, Pij - 4, Qij - 5..
z = zdata(:,3) % Measurement values..
zdata(:,1)=1:length(z);
    %zdata4exact = [pfres.bus(:,8); (-pfres.bus(1,3)+pfres.gen(2,2))/100; (-pfres.bus(4,3)+pfres.gen(1,2))/100; pfres.branch(1:3, 14)/100; pfres.branch(1:3, 15)/100];
    %z = zdata4exact;
fbus = zdata(:,4); % From bus..
tbus = zdata(:,5); % To bus..
Ri = diag(zdata(:,6)) % Measurement Error..
B = imag(ybus) 
%g, b, bsh is already extracted

V = ones(nbus,1); % Initialize the bus voltages..
O = zeros(nbus,1); % Initialize the bus angles..
%       V = pfres.bus(:,8);
%      O = pfres.bus(:,9)*pi/180;
%[dummy, slack_O] = rect2pol(0.2865 + 1.0123i); %setting Theta_1 to phasor measurement at bus 1
%O = O+10*pi/180;
x = [O(2:end); V];   % State Vector, not including Theta_1
G = real(ybus);

%Indexes in zdata of the different type of measurements:
vi = find(type == 1); % Index of voltage magnitude measurements
ppi = find(type == 2); % Index of active power injection measurements
qi = find(type == 3); % Index of reactive power injection measurements
pf = find(type == 4); % Index of active power flow measurements
qf = find(type == 5); % Index of reactive power flow measurements
vir = find(type == 6); % Index of real voltage measurements
vij = find(type == 7); % Index of imaginary voltage measurements
ifr = find(type == 8); % Index of real current measurements
ifj = find(type == 9); % Index of imaginary current measurements

%Number of measurements of the different types:
nvi = length(vi); % Number of voltage measurements
npi = length(ppi); % Number of real power injection measurements
nqi = length(qi); % Number of reactive power injection measurements
npf = length(pf); % Number of real power flow measurements
nqf = length(qf); % Number of reactive power flow measurements
nvir = length(vir); % Number of real voltage measurements
nvij = length(vij); % Number of imaginary voltage measurements
nifr = length(ifr); % Number of real current measurements
nifj = length(ifj); % Number of imaginary current measurements

iter = 1;
tol = 5;

while(tol > 1e-5 && iter < 30)
    
   % fprintf('Iteration %d \n', iter)
    
    %% Building h vector
    %Measurement Function, h
    h1 = V(fbus(vi),1)
    h2 = zeros(npi,1);
    h3 = zeros(nqi,1);
    h4 = zeros(npf,1);
    h5 = zeros(nqf,1);
    h6 = V(fbus(vir)).*cos(O(fbus(vir)));
    h7 = V(fbus(vij)).*sin(O(fbus(vij)));
    h8 = zeros(nifr, 1);
    h9 = zeros(nifj, 1);
    
    for i = 1:npi
        m = fbus(ppi(i));
        for k = 1:nbus
            h2(i) = h2(i) + V(m)*V(k)*(G(m,k)*cos(O(m)-O(k)) + B(m,k)*sin(O(m)-O(k)));
        end
    end
    
    for i = 1:nqi
        m = fbus(qi(i));
        for k = 1:nbus
            h3(i) = h3(i) + V(m)*V(k)*(G(m,k)*sin(O(m)-O(k)) - B(m,k)*cos(O(m)-O(k)));
        end
    end
    
    for i = 1:npf
        m = fbus(pf(i));
        n = tbus(pf(i));
        h4(i) = V(m)^2*g(m,n) - V(m)*V(n)*(g(m,n)*cos(O(m)-O(n)) + b(m,n)*sin(O(m)-O(n)));
    end
    
    for i = 1:nqf
        m = fbus(qf(i));
        n = tbus(qf(i));
        h5(i) = -V(m)^2*(b(m,n)+bsh(m,n)) - V(m)*V(n)*(g(m,n)*sin(O(m)-O(n)) - b(m,n)*cos(O(m)-O(n)));
    end
    
    for i = 1:nifr
        m = fbus(ifr(i));
        n = tbus(ifr(i));
        h8(i) = ( V(m)*cos(O(m))-V(n)*cos(O(n)) )*g(m,n)  - ( V(m)*sin(O(m)) - V(n)*sin(O(n)) )*b(m,n) - bsh(1,2)*V(m)*sin(O(m));
    end
    
    for i = 1:nifj
        m = fbus(ifj(i));
        n = tbus(ifj(i));
        h9(i) = ( ( V(m)*cos(O(m))-V(n)*cos(O(n)) )*b(m,n)  + ( V(m)*sin(O(m)) - V(n)*sin(O(n)) )*g(m,n) + bsh(m,n)*V(m)*cos(O(m)));
    end
    
    h = [h1; h2; h3; h4; h5; h6; h7; h8; h9];
    
   
    
    % Residue
    r = z - h
   
    %% Building H matrix
    
    % H11 - Derivative of V with respect to angles
    H11 = sparse(zeros(nvi,nbus-1));

    % H12 - Derivative of V with respect to V
    H12 = sparse(zeros(nvi,nbus));
    for k = 1:nvi
        for n = 1:nbus
            if n == k
                H12(k,n) = 1;
            end
        end
    end

    % H21 - Derivative of Real Power Injections with Angles
    H21 = sparse(zeros(npi,nbus-1));
    for i = 1:npi
        m = fbus(ppi(i));
        for k = 1:(nbus-1)
            if k+1 == m
                for n = 1:nbus
                    H21(i,k) = H21(i,k) + V(m)* V(n)*(-G(m,n)*sin(O(m)-O(n)) + B(m,n)*cos(O(m)-O(n)));
                end
                H21(i,k) = H21(i,k) - V(m)^2*B(m,m);
            else
                H21(i,k) = V(m)* V(k+1)*(G(m,k+1)*sin(O(m)-O(k+1)) - B(m,k+1)*cos(O(m)-O(k+1)));
            end
        end
    end
 
    % H22 - Derivative of Real Power Injections with V
    H22 = sparse(zeros(npi,nbus));
    for i = 1:npi
        m = fbus(ppi(i));
        for k = 1:(nbus)
            if k == m
                for n = 1:nbus
                    H22(i,k) = H22(i,k) + V(n)*(G(m,n)*cos(O(m)-O(n)) + B(m,n)*sin(O(m)-O(n)));
                end
                H22(i,k) = H22(i,k) + V(m)*G(m,m);
            else
                H22(i,k) = V(m)*(G(m,k)*cos(O(m)-O(k)) + B(m,k)*sin(O(m)-O(k)));
            end
        end
    end
    
    % H31 - Derivative of Reactive Power Injections with Angles
    H31 = sparse(zeros(nqi,nbus-1));
    for i = 1:nqi
        m = fbus(qi(i));
        for k = 1:(nbus-1)
            if k+1 == m
                for n = 1:nbus
                    H31(i,k) = H31(i,k) + V(m)* V(n)*(G(m,n)*cos(O(m)-O(n)) + B(m,n)*sin(O(m)-O(n)));
                end
                H31(i,k) = H31(i,k) - V(m)^2*G(m,m);
            else
                H31(i,k) = V(m)* V(k+1)*(-G(m,k+1)*cos(O(m)-O(k+1)) - B(m,k+1)*sin(O(m)-O(k+1)));
            end
        end
    end
    
    % H32 - Derivative of Reactive Power Injections with V
    H32 = sparse(zeros(nqi,nbus));
    for i = 1:nqi
        m = fbus(qi(i));
        for k = 1:(nbus)
            if k == m
                for n = 1:nbus
                    H32(i,k) = H32(i,k) + V(n)*(G(m,n)*sin(O(m)-O(n)) - B(m,n)*cos(O(m)-O(n)));
                end
                H32(i,k) = H32(i,k) - V(m)*B(m,m);
            else
                H32(i,k) = V(m)*(G(m,k)*sin(O(m)-O(k)) - B(m,k)*cos(O(m)-O(k)));
            end
        end
    end
    
    % H41 - Derivative of Real Power Flows with Angles
    H41 = sparse(zeros(npf,nbus-1));
    for i = 1:npf
        m = fbus(pf(i));
        n = tbus(pf(i));
        for k = 1:(nbus-1)
            if k+1 == m
                H41(i,k) = V(m)* V(n)*(g(m,n)*sin(O(m)-O(n)) - b(m,n)*cos(O(m)-O(n)));
            elseif k+1 == n
                H41(i,k) = -V(m)* V(n)*(g(m,n)*sin(O(m)-O(n)) - b(m,n)*cos(O(m)-O(n)));
            end
        end
    end
    
    % H42 - Derivative of Real Power Flows with V
    H42 = sparse(zeros(npf,nbus));
    for i = 1:npf
        m = fbus(pf(i));
        n = tbus(pf(i));
        for k = 1:nbus
            if k == m
                H42(i,k) = -V(n)*(-G(m,n)*cos(O(m)-O(n)) - B(m,n)*sin(O(m)-O(n))) - 2*G(m,n)*V(m);
            elseif k == n
                H42(i,k) = -V(m)*(-G(m,n)*cos(O(m)-O(n)) - B(m,n)*sin(O(m)-O(n)));
            end
        end
    end
    
    % H51 - Derivative of Reactive Power Flows with Angles
    H51 = sparse(zeros(nqf,nbus-1));
    for i = 1:nqf
        m = fbus(qf(i));
        n = tbus(qf(i));
        for k = 1:(nbus-1)
            if k+1 == m
                H51(i,k) = -V(m)* V(n)*(-G(m,n)*cos(O(m)-O(n)) - B(m,n)*sin(O(m)-O(n)));
            elseif k+1 == n
                H51(i,k) = V(m)* V(n)*(-G(m,n)*cos(O(m)-O(n)) - B(m,n)*sin(O(m)-O(n)));
            end
        end
    end
    
    % H52 - Derivative of Reactive Power Flows with V
    H52 = sparse(zeros(nqf,nbus));
    for i = 1:nqf
        m = fbus(qf(i));
        n = tbus(qf(i));
        for k = 1:nbus
            if k == m
                H52(i,k) = -V(n)*(-G(m,n)*sin(O(m)-O(n)) + B(m,n)*cos(O(m)-O(n))) - 2*V(m)*(-B(m,n)+ bsh(m,n));
            elseif k == n
                H52(i,k) = -V(m)*(-G(m,n)*sin(O(m)-O(n)) + B(m,n)*cos(O(m)-O(n)));
            end
        end
    end
    
    % H61 - Derivative of V_R with respect to angle 
    H61 = sparse(zeros(nvir,nbus-1));
    for i = 1:nvir
        m = fbus(vir(i));
        for k = 1:(nbus-1)
            if (k+1) == m
                H61(i,k) = -V(m)*sin(O(m));
            end
        end
    end
    
    % H62 - Derivative of V_R with respect to V
    H62 = sparse(zeros(nvir,nbus));
    for i = 1:nvir
        m = fbus(vir(i));
        for k = 1:nbus
            if k == m
                H62(i,k) = cos(O(m));
            end
        end
    end
    
    % H71 - Derivative of V_imag with respect to angle
    H71 = sparse(zeros(nvij,nbus-1));
    for i = 1:nvij
        m = fbus(vij(i));
        for k = 1:(nbus-1)
            if (k+1) == m
                H71(i,k) = V(m)*cos(O(m));
            end
        end
    end

    % H72 - Derivative of V_imag with respect to V
    H72 = sparse(zeros(nvij,nbus));
    for i = 1:nvij
        m = fbus(vij(i));
        for k = 1:nbus
            if k == m
                H72(i,k) = sin(O(m));
            end
        end
    end
    
    % H81 - Derivative of I_real with respect to angle 
    H81 = sparse(zeros(nifr,nbus-1));
    for i = 1:nifr
        m = fbus(ifr(i));
        n = tbus(ifr(i));
        for k = 1:(nbus-1)
            if k+1 == m
                H81(i,k) = -V(m)*( g(m,n)*sin(O(m))+b(m,n)*cos(O(m))+bsh(m)*cos(O(m)));
            elseif k+1 == n
                H81(i,k) = V(n)*( g(m,n)*sin(O(n))+b(m,n)*cos(O(n)));
            end
        end
    end
        
    
    % H82 - Derivative of I_real with respect to V 
    H82 = sparse(zeros(nifr,nbus));
    for i = 1:nifr
        m = fbus(ifr(i));
        n = tbus(ifr(i));
        for k = 1:nbus
            if k == m
                H82(i,n) = g(m,n)*cos(O(m)) - b(m,n)*sin(O(m)) - bsh(m)*sin(O(m));
            elseif k == n
                H82(i,n) = - g(m,n)*cos(O(n))+b(m,n)*sin(O(n));
            end
        end
    end
    
    % H91 - Derivative of I_imag with respect to angle 
    H91 = sparse(zeros(nifj,nbus-1));
    for i = 1:nifj
        m = fbus(ifj(i));
        n = tbus(ifj(i));
        for k = 1:(nbus-1)
            if (k+1) == m
                H91(i,k) = V(m) * (-b(m,n)*sin(O(m)) - g(m,n)*cos(O(m)) + bsh(m)*sin(O(m)));
            elseif (k+1) == n
                H91(i,k) = V(n)* (b(m,n)*sin(O(n))+g(m,n)*cos(O(n)));
            end
        end
    end
    
    % H92 - Derivative of I_imag with respect to V 
    H92 = sparse(zeros(nifj,nbus));
    for i = 1:nifj
        m = fbus(ifj(i));
        n = tbus(ifj(i));
        for k = 1:nbus
            if k == m
                H92(i,k) = (b(m,n)*cos(O(m)) - g(m,n)*sin(O(m)) - bsh(m)*cos(O(m)));
            elseif k == n
                H92(i,k) = (-b(m,n)*cos(O(n))+g(m,n)*sin(O(n)));
            end
        end
    end
    
    % Measurement Jacobian
    
    H = [H11 H12; H21 H22; H31 H32; H41 H42; H51 H52; H61 H62; H71 H72; H81 H82; H91 H92]
    
   %disp(H);
    
    %% Calculating correction dx
   % W=inv(Ri);
    W = speye(length(r))\(sparse(Ri));
    
    % Gain Matrix
    Gm = H'*W*H;
    %fprintf('r: \n');
    %disp(inv(Gm));
    
    %Objective Function
    %J = r'*W*r; 
    
    % State Vector
    dx = Gm\(H'*W*r); 
    x = x + dx;
    %fprintf('x: %f \t %f \t %f \n', x)
    O(2:end) = x(1:nbus-1);
    V = x(nbus:end);
    %fprintf('O: %f \t %f \n', O)
    %fprintf('V: %f \t %f \n', V)
    iter = iter + 1;
    tol = max(abs(dx));
    
    %Vrec(iter-1, :) = V;
    %Orec(iter-1, :) = O*180/pi;
    
    %fprintf('Residual %4g\n', max(abs(dx)))
    
end
%xCov = diag(inv(H'*inv(Ri)*H)); % extracts estimate covariance matrix
%diagonal elements.

Del = 180/pi*O;

fprintf('Converged in %4g iterations\n', iter-1);

result = zeros(nbus,3);
for m = 1:nbus
    result(m,1) = m;
    result(m,2) = V(m);
    result(m,3) = Del(m);
end


% disp(J)
% 
% disp(inv(Gm))
% 
% 
% %% Building h vector
%     %Measurement Function, h
%     h1 = V(fbus(vi),1);
%     h2 = zeros(npi,1);
%     h3 = zeros(nqi,1);
%     h4 = zeros(npf,1);
%     h5 = zeros(nqf,1);
%     h6 = V(fbus(vir)).*cos(O(fbus(vir)));
%     h7 = V(fbus(vij)).*sin(O(fbus(vij)));
%     h8 = zeros(nifr, 1);
%     h9 = zeros(nifj, 1);
%     
%     for i = 1:npi
%         m = fbus(ppi(i));
%         for k = 1:nbus
%             h2(i) = h2(i) + V(m)*V(k)*(G(m,k)*cos(O(m)-O(k)) + B(m,k)*sin(O(m)-O(k)));
%         end
%     end
%     
%     for i = 1:nqi
%         m = fbus(qi(i));
%         for k = 1:nbus
%             h3(i) = h3(i) + V(m)*V(k)*(G(m,k)*sin(O(m)-O(k)) - B(m,k)*cos(O(m)-O(k)));
%         end
%     end
%     
%     for i = 1:npf
%         m = fbus(pf(i));
%         n = tbus(pf(i));
%         h4(i) = V(m)^2*g(m,n) - V(m)*V(n)*(g(m,n)*cos(O(m)-O(n)) + b(m,n)*sin(O(m)-O(n)));
%     end
%     
%     for i = 1:nqf
%         m = fbus(qf(i));
%         n = tbus(qf(i));
%         h5(i) = -V(m)^2*(b(m,n)+bsh(m,n)) - V(m)*V(n)*(g(m,n)*sin(O(m)-O(n)) - b(m,n)*cos(O(m)-O(n)));
%     end
%     
%     for i = 1:nifr
%         m = fbus(ifr(i));
%         n = tbus(ifr(i));
%         h8(i) = ( V(m)*cos(O(m))-V(n)*cos(O(n)) )*g(m,n)  - ( V(m)*sin(O(m)) - V(n)*sin(O(n)) )*b(m,n) - bsh(1,2)*V(m)*sin(O(m));
%     end
%     
%     for i = 1:nifj
%         m = fbus(ifj(i));
%         n = tbus(ifj(i));
%         h9(i) = ( ( V(m)*cos(O(m))-V(n)*cos(O(n)) )*b(m,n)  + ( V(m)*sin(O(m)) - V(n)*sin(O(n)) )*g(m,n) + bsh(m,n)*V(m)*cos(O(m)));
%     end
%     
%     h = [h1; h2; h3; h4; h5; h6; h7; h8; h9];
%     
%    %disp(h);
%     
% Residue


end