% Delivered to the U.S. Government in Accordance with Contract FA8650-10-C-7080.
% 
% (c) 2011 United Technologies Corp.
% Developed with the sponsorship of the Defense Advanced Research Projects Agency (DARPA).
% Permission is hereby granted, free of charge, to any person obtaining a copy of this data,
% including any software or models in source or binary form, as well as any drawings, 
% specifications, and documentation (collectively "the Data"), to deal in the Data without 
% restriction, including without limitation the rights to use, copy, modify, merge, publish,
% distribute, sublicense, and/or sell copies of the Data, and to permit persons to whom the Data
% is furnished to do so, subject to the following conditions:
%  
% The above copyright notice and this permission notice shall be included in all copies or
% substantial portions of the Data.
%  
% THE DATA IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
% BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS, SPONSORS, DEVELOPERS, CONTRIBUTORS, OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
% OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE DATA OR THE
% USE OR OTHER DEALINGS IN THE DATA.

% Authors:  Fernando Rodriguez                      John Finn
%           Sr Engineer/Scientist                   Contractor (Summer Intern 2011)
%           Power Electronics Systems               Embedded Systems & Networks
%           United Technologies Research Center     United Technologies Research Center
%           411 Silver Lane                         411 Silver Lane
%           East Hartford,CT 06108                  East Hartford,CT 06108
%           rodrigf@utrc.utc.com                    ccfinnje@utrc.utc.com
%           tel: (860) 610-7102                     tel: +1 860-610-7696
%%
%lfnewton.m
%   Power flow solution by Newton-Raphson method derived from power systems
%   textbook: Hadi Saadat "Power System Analysis 2nd Edition", Hadi Saadat, 
%   2002, The McGraw-Hill Companies, Inc.
% %function lfnewton(Ybus, nbr, nl, nr, busdata, linedata)

Vm=0; delta=0; yload=0; deltad=0; % initialize variables to zero
nbus = length(busdata(:,1)); % "nbus" is number of buses
% LOOP 1: Get S,V phasors for each bus
for k=1:nbus                % "nbus" # of entries in busdata
    n=busdata(k,1);         % "n" bus #
    kb(n)=busdata(k,2);     % "kb" bus-code
    Vm(n)=busdata(k,3);     % "Vm" voltage magnitude
    delta(n)=busdata(k,4);  % "delta" voltage angle
    Pd(n)=busdata(k,5);     % "Pd" load real pwr
    Qd(n)=busdata(k,6);     % "Qd" load reactive pwr
    Pg(n)=busdata(k,7);     % "Pg" generated real pwr
    Qg(n) = busdata(k,8);   % "Qg" generated reactive pwr
    P(n) = Pg(n)- Pd(n);     % "P" net real power
    Q(n) = Qg(n)- Qd(n);     % "Q" net reactive power
    delta(n) = 0; %pi/180*delta(n);    % degrees to radians
    Ym=abs(Ybus);       % "Ym" Ybus magnitude
    t = angle(Ybus);    % "t" Ybus angle
    V(n) = Vm(n); %*(cos(delta(n)) + 1i*sin(delta(n))); % V phasor
    S(n) = P(n) + 1i*Q(n);   % "S" net appearant power
end

% LOOP 2: Find size of J which is m x m
ns=0; ng=0;
for k=1:nbus        % "nbus" # of entries in busdata
    if kb(k) == 1   % if slack bus
        ns = ns+1;  % "ns" # of slack bus
    else
    end
    if kb(k) == 2   % if voltage-controlled bus
        ng = ng+1;  % "ng" # of voltage-controlled bus (ng=0)
    else
    end
    ngs(k) = ng;    % "ngs" running count of "ng"
    nss(k) = ns;    % "nss" running count of "ns"
end
m=2*nbus-ng-2*ns;   % J is size m x m...number of unknown |V| & |V|Angles

maxerror = 1; converge=1; iter = 0; % initialize iteration parameters
% Start of iterations
clear A  DC   J  DX
% accuracy and maxiter are set in EPS_B_data.m
while maxerror >= accuracy && iter <= maxiter % Test for max. power mismatch
    A=zeros(m);             % Initializing Jacobian matrix
    iter = iter+1;          % Count up iter
    for n=1:nbus            % "nbus" # of entries in busdata
        nn=n-nss(n);        % "nn" index for ?
        lm=nbus+n-ngs(n)-nss(n)-ns; % "lm" index for ?
        J11=0; J22=0; J33=0; J44=0; % Contribut to J diag's
        for i=1:nbr % "nbr" is the number of rows in linedata
            % Step through line data to find which have bus "n" data
            if nl(i) == n || nr(i) == n % if nl or nr are bus "n"
                % assign "l" the bus # connecting to "n"
                if nl(i) == n, l = nr(i); end
                if nr(i) == n, l = nl(i); end
                
                J11=J11+Vm(n)*Vm(l)*Ym(n,l)*sin(t(n,l)-delta(n)+delta(l)); % dP/d(delta)
                J33=J33+Vm(n)*Vm(l)*Ym(n,l)*cos(t(n,l)-delta(n)+delta(l)); % dQ/d(delta)
                
                if kb(n)~=1 % For load buses and voltage-controlled
                    J22=J22+Vm(l)*Ym(n,l)*cos(t(n,l)-delta(n)+delta(l)); % dP/d|V|
                    J44=J44+Vm(l)*Ym(n,l)*sin(t(n,l)-delta(n)+delta(l)); % dQ/d|V|
                else
                end
                if kb(n) ~= 1  && kb(l) ~=1 % if "n" & "l" are not a slack bus
                    lk = nbus+l-ngs(l)-nss(l)-ns;
                    ll = l -nss(l);
                                                % off diagonal elements of J1
                    A(nn, ll) =-Vm(n)*Vm(l)*Ym(n,l)*sin(t(n,l)- delta(n) + delta(l));
                    if kb(l) == 0               % off diagonal elements of J2
                        A(nn, lk) =Vm(n)*Ym(n,l)*cos(t(n,l)- delta(n) + delta(l));
                    end
                    if kb(n) == 0               % off diagonal elements of J3
                        A(lm, ll) =-Vm(n)*Vm(l)*Ym(n,l)*cos(t(n,l)- delta(n)+delta(l));
                    end
                    if kb(n) == 0 && kb(l) == 0  % off diagonal elements of J4
                        A(lm, lk) =-Vm(n)*Ym(n,l)*sin(t(n,l)- delta(n) + delta(l));
                    end
                else
                end
            else
            end
        end
        % Calculate P(k) an Q (k) for bus n (kth iteration)
        Pk = Vm(n)^2*Ym(n,n)*cos(t(n,n))+J33;
        Qk = -Vm(n)^2*Ym(n,n)*sin(t(n,n))-J11;
        if kb(n) == 1 %  if bus n is a slack bus
            P(n) = Pk;
            Q(n) = Qk;
        end 
        if kb(n) ~= 1
            A(nn,nn) = J11;  % diagonal elements of J1
            DC(nn) = P(n)-Pk;
        end
        if kb(n) == 0
            A(nn,lm) = 2*Vm(n)*Ym(n,n)*cos(t(n,n))+J22;  % diagonal elements of J2
            A(lm,nn)= J33;        % diagonal elements of J3
            A(lm,lm) =-2*Vm(n)*Ym(n,n)*sin(t(n,n))-J44;  % diagonal of elements of J4
            DC(lm) = Q(n)-Qk;
        end
    end
    DX=A\DC';
    % Delta(delta) and delta(|V|) have been calculated for kth iteration
    for n=1:nbus
        nn=n-nss(n);
        lm=nbus+n-ngs(n)-nss(n)-ns;
        if kb(n) ~= 1
            delta(n) = delta(n)+DX(nn);
        end
        if kb(n) == 0
            Vm(n)=Vm(n)+DX(lm);
        end
    end
    maxerror=max(abs(DC));
    if iter == maxiter & maxerror > accuracy 
        fprintf('\nWARNING: Iterative solution did not converged after ')
        fprintf('%g', iter), fprintf(' iterations.\n\n')
        fprintf('Press Enter to terminate the iterations and print the results \n')
        converge = 0; pause
    else
    end
end
 
if converge ~= 1
   tech= ('                      ITERATIVE SOLUTION DID NOT CONVERGE'); else, 
   tech=('                   Power Flow Solution by Newton-Raphson Method');
end   

V = Vm.*cos(delta)+1i*Vm.*sin(delta);
deltad=180/pi*delta;
i=sqrt(-1);
k=0;
for n = 1:nbus
    if kb(n) == 1 % For a slack bus
        k=k+1;
        S(n)= P(n)+1i*Q(n);
        Pg(n) = P(n) + Pd(n);
        Qg(n) = Q(n) + Qd(n);
        Pgg(k)=Pg(n);
        Qgg(k)=Qg(n);
    else
    end
    yload(n) = (Pd(n)- 1i*Qd(n))/(Vm(n)^2);
end
%Update the Bus Voltage and Angle for subsystem Load Flow solution
busdata(:,3)=Vm'; busdata(:,4)=deltad';
%Add Generated and Sunk Power at all buses
Pgt = sum(Pg);  Qgt = sum(Qg); Pdt = sum(Pd); Qdt = sum(Qd);

%end