% 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
%           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
%%

function [A_M, B_M, C_Mr, C_Mi, B_No] = statespace_formulation_rev08_orig(nl)  
    % EPSdata_ACsystOnly_Transient_rev02
    % Get topology Switch, T-line, Converter, Source and Load matrices
    if nl == 6   
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_6L;
    elseif nl == 9
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_9L;
    elseif nl == 12
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_12L;
    elseif nl == 18
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_18L;
    elseif nl == 24
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_24L;
    elseif nl == 27
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_27L;
    elseif nl == 33
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_33L;
    elseif nl == 36
        [Source_data, TL_data, SW_data, Load_data, Conv_data] = topologyRev03_36L;
    end

    % Create LINK data matrix, which contains T-line and Switch data
    % SW=1, TL=2
    LNK_data= [SW_data ; TL_data];
    % Number of links
    n_LNK = length(LNK_data(:,1));
    % Link Data: SW=1, TL=2
    LNK_data(:,6) = [ones(length(SW_data(:,1)),1) ; 2*ones(length(TL_data(:,1)),1)];
    % Number of Loads
    n_LD = length(Load_data(:,1));
    % Initialize Loop Variables
    B_ndx = 0; % tracks the current index in the B matrix
    b_LNK = 0; % place holder for the current link
    LEV2_nds = 0; % array for the level 2 nodes, which are connected to multiple loads
    ck = 0;
    
    % Get generator level 2 nodes
    for m = 1 : length(Source_data(:,1))
        nd = Source_data(m,1);
        count = 0;
        while count < 2
            for a = 1 : length(TL_data(:,1))
                if TL_data(a,3) == nd
                    nd = TL_data(a,4);
                    count = count + 1;
                elseif TL_data(a,4) == nd
                    nd = TL_data(a,3);
                    count = count + 1;
                end
            end
            for a = 1 : length(SW_data(:,1))
                if SW_data(a,3) == nd
                    nd = SW_data(a,4);
                    count = count + 1;
                elseif SW_data(a,4) == nd
                    nd = SW_data(a,3);
                    count = count + 1;
                end
            end
        end
        Gen_nds(m) = nd;
    end
    
    
    % Get all branch data for loads branches one at a time
    % For each load
    for m = 1:n_LD
        % Combine Real and Imaginary portions of the load m impedence
        LD_Z= Load_data(m,3)+1i*Load_data(m,4);
        % place the load data into the B matrix
        B(sum(B_ndx)+1,:) = [0   0   0  Load_data(m,1)   LD_Z   0  Load_data(m,1)];        
        nd = B(sum(B_ndx)+1,4); % non-zero (ground) node of the load
        count = 0; % number of links connected to load m   
        br_cnt = 1; % counts the number of branches     
        max_its = 1; % Maximum number of while loop iterations 
        while count < 2
            % For each link (switch and t-line)
            for i = 1:n_LNK
                % if the current link is connected to the load
                if LNK_data(i,3) == nd || LNK_data(i,4) == nd % ND(1)
                    count = count + 1; % increment the number of links   
                    b_LNK = i; % saves the link location in LNK_data
                end
            end
            % if the load is not connected to any link it is floating
            if count < 1   
                count = 2; % breaks out of the while loop
            end
            % if the current node is already a level 2 node, break the loop
                for i = 1:length((LEV2_nds))
                    if nd == LEV2_nds(i)
                        count = 2;
                    end
                end
                % generator nodes must not be combined with other links
                for i = 1:length((Gen_nds))
                    if nd == Gen_nds(i)
                        count = 2;
                    end
                end
            if count == 1 % there is only one link connected to the load
                br_cnt = br_cnt + 1; % increment the branch count index     
                count = 0; % resets the count to look for another link
                %  for both nodes of the link
                for i = 3:4
                    % finds the node connected to load m
                    if  nd - LNK_data(b_LNK,i) == 0  && n_LNK > 0
                        nd = LNK_data(b_LNK,7-i); % the other node of the link that is not connected to the load
                        B(sum(B_ndx)+br_cnt,:) = [LNK_data(b_LNK,:) nd]; % put the link data into the B matrix
                        LNK_data(b_LNK,:) = LNK_data(n_LNK,:); % Replaces the visited link with the last one
                        n_LNK = n_LNK - 1; % decrease the number of remaining links
                        if i ==3
                            break
                        end                        
                    end
                end
            end
            max_its = max_its + 1; % increments the number of iterations 
            if max_its > 1000 % breaks out of while loop after 1000 iterations
                count = 2;
            end
        end
        % checks to see if nd is a already a level 2 node, if so ck = 1
        for i=1:length(LEV2_nds)
            if nd == LEV2_nds(i)
                ck = 1;
            end
        end
        % add nd to the list of level 2 nodes if not already in the list
        if ck ~=1
            LEV2_nds(i+1) = nd;
        end
        ck = 0; % reset ck
        B_ndx(m) = br_cnt;  % update B index for the next load
        n_LNK = n_LNK + 1; % increment the total number of "applicable" links
        LNK_data(n_LNK,:) = [B(sum(B_ndx),1:5) 5]; % add the link connected to the level 2 node
    end
    LEV2_nds = LEV2_nds(2:length(LEV2_nds)); % remove 1st element from level 2 nodes since it is a zero
    ii=0; % indexing variable
    for i =1:length(B_ndx) % for each of the branches
        % creat the branch matrix, which combines the impedance of the
        % entire branch
        Branch(i,:) = [ i 0 B(sum(B_ndx(1:i)),7) sum(B( ii+1 : sum(B_ndx(1:i)) ,5))];
        ii=sum(B_ndx(1:i));
    end
%%%%%%%%%%%%%    % End of load Brach definitions  %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    LNK_data = LNK_data(1:n_LNK,:); % remove the unneeded elements
    for i = 1:length(LNK_data(:,1)) % for each remaining link
        % if the link is not part of a branch
        if LNK_data(i,6) < 5
            ck = ck+1; % increment the indexing variable
            temp1(ck,:) = LNK_data(i,:); % hold the temp value
        end
    end
    LNK_data = temp1; % store the new LNK_data matrix
    ck = 0; % reset ck

%Start second level branch search
    max_its2 = 1; % TODO
    while n_LNK > 0                         %while there still are remaining links
        for m = 1:length(LEV2_nds)          %for each level 2 node
            nd = LEV2_nds(m);               %select the first level 2 node
            n_LNK = length(LNK_data(:,1));  %update the number of links in LNK_data 
            b_LNK = 0;                      %reset b_LNK to zero

%STEP 1:    Find link connected to the level 2 (starter) node, "nd"         
            for i = 1:n_LNK % search through both buses of the links
                if LNK_data(i,3) == nd      %if match found as bus 1
                    b_LNK = i; new =4;      %save LNK_data row # and column
                elseif LNK_data(i,4) == nd  %if match found as bus 2
                    b_LNK = i; new =3;      %save LNK_data row # and column
                end
            end
            if n_LNK < 1 || b_LNK == 0      %no match found, m++ and return to Step 1
                     STP_NOW = 1;           
            else                            %match was found, continue
%STEP 2:    Add matching LNK_data row to B and remove mathing row from LNK_data
                nd = LNK_data(b_LNK,new);                   %set nd equal to matching LNK_data row node
                B(sum(B_ndx)+1,:) = [LNK_data(b_LNK,:) nd]; %add matching LNK_data row to B
                LNK_data(b_LNK,:) = LNK_data(n_LNK,:);      %replaces matching LNK_data row with last row
                n_LNK = n_LNK-1;                            %decrement number of links
                LNK_data = LNK_data(1:n_LNK,:);             %remove the last row in LNK_data
%STEP 3:    ?
                count = 0; % number of links connected nd 
                br_cnt = 1; % counts the number of branches 
                max_its = 1; % Maximum number of while loop iterations 

                while count < 2
                    % find and "count" how many LNK_data rows have "nd", record row for last find
                    for i = 1:n_LNK
                        if LNK_data(i,3) == nd || LNK_data(i,4) == nd
                            count = count + 1; % increment the number of links 
                            b_LNK = i; % saves the link location in LNK_data 
                            % QUESTION: what if there are more than one link found? Then count=2 and loop is broken
                        end
                    end
                    % if the current node is already a level 2 node, break the loop
                    for i = 1:length((LEV2_nds))
                        if nd == LEV2_nds(i)
                            count = 2;
                        end
                    end
                    % generator nodes must not be combined with other links
                    for i = 1:length((Gen_nds))
                        if nd == Gen_nds(i)
                            count = 2;
                        end
                    end
                    if count < 1        %if nd connects to nothing, break loop
                        count = 2;
                    end
                    if count == 1       %there is only one link connected
                        br_cnt = br_cnt + 1; % increment the branch count     
                        count = 0; % resets the count to look for another link
                        for i = 3:4
                            %finds the node connected to nd 
                            if  nd - LNK_data(b_LNK,i) == 0  && n_LNK > 0
                                nd = LNK_data(b_LNK,7-i); % the other node of the link that is not connected to the load
                                B(sum(B_ndx)+br_cnt,:) = [LNK_data(b_LNK,:) nd]; % put the link data into the B matrix
                                LNK_data(b_LNK,:) = LNK_data(n_LNK,:); % Replaces the visited link with the last one
                                n_LNK = n_LNK - 1; % decrease the number of remaining links
                                LNK_data = LNK_data(1:n_LNK,:); % removes the link from the list
                                if i ==3
                                    break
                                end
                            end
                        end                      
    
                    end
                    max_its = max_its + 1; % increments the number of iterations 
                    if max_its > 1000 % breaks out of while loop after 1000 iterations
                        count = 2;
                    end
                end
                
%               % checks to see if the new nd is already a level 2 node
                for i=1:length(LEV2_nds)
                    if nd == LEV2_nds(i) % if nd is already a level 2 node set ck to one
                        ck = 1;
                    end
                end
                if ck == 0; % if nd is not a level 2 node, add it
                    LEV2_nds(length(LEV2_nds)+1) = nd;
                end
                ck =0; % reset ck to zero
                B_ndx(length(B_ndx)+1) = br_cnt; % update the B matrix index variable
            end
        end
        % limit the outer while loop to 100 iterations max
        if max_its2 == 100
            n_LNK = 0; % breaks the loop
        end
        max_its2 = max_its2 +1; % increments the while loop counter
    end

    % add level two branches to Branch matrix
    % for all branches that are not loads
    for i =n_LD:length(B_ndx)-1
        nd_x = B(sum(B_ndx(1:i))+1,7); % "next" bus number of non-load branch
        % find the next link in the branch
        % QUESTION: what if there is more than one? should it be if else?
        if B(sum(B_ndx(1:i))+1,3) == nd_x
            nd1 = B(sum(B_ndx(1:i))+1,4); % found the node in column 4 of B
        else
            nd1 = B(sum(B_ndx(1:i))+1,3); % found the node in column 3 of B
        end
        nd2 = B(sum(B_ndx(1:i+1)),7); % the other node
        % add branch to Branch matrix
        Branch(i+1,:) = [ i+1 nd1 nd2 sum(B( sum(B_ndx(1:i))+1 :sum(B_ndx(1:i+1)) , 5  ))];
        % sum(B(9:9,5))];
    end

    % create connectivity matrix "C", which shows which branches are
    % connected to each of the load branches on the path to the source
    % row for each load or loop, column for each branch
    C = [diag(ones(1,n_LD)) zeros(n_LD,length(Branch(:,1))-n_LD)];
    
   
    %Define C matrix entries starting at generator node
    N_LD_Branch = Branch(n_LD+1:length(Branch(:,1)),:);%Non Load Branches
    y_C = zeros(length(LEV2_nds(1,:)),length(Branch(:,1))); %outpu bus voltage coefficient matrix.
    u6=1; BUS_nds = LEV2_nds;
    u10=1;
    for temp =1:n_LD % for each load
        nd = Source_data(1,1); nd_i(1)=nd; % start with generator node
        clear nd_i;  u5=1; nd_i(u5)=nd;
        u1=1; no_nd = 0;
        % find the node connected to nd from source to sink
        while no_nd ~=1 && length(N_LD_Branch(:,1)) ~=0
            u5=u5+1;
            % find the branch connected to nd
            for u2=1:length(N_LD_Branch(:,1))
                if N_LD_Branch(u2,2) == nd
                    nd = N_LD_Branch(u2,3); nd_i(u5)=nd; break % change nd
                elseif N_LD_Branch(u2,3) == nd
                    nd = N_LD_Branch(u2,2); nd_i(u5)=nd; break % change nd
                elseif u2 == length(N_LD_Branch(:,1)) % breaks loop once no more nds found
                    no_nd = 1;
                end
            end
            % if an nd was found
            if no_nd ~=1
                LP(u1) = N_LD_Branch(u2,1); u1=u1+1; % save the found branch # in LP
                p = length(N_LD_Branch(:,1)); % remove the found branch from N_LD
                N_LD_Branch(u2,:) = N_LD_Branch(p,:);
                N_LD_Branch = N_LD_Branch(1:p-1,:);
            end
        end
        
        %LP contains C row #'s that must equal 1 for loads with node = nd
        temp2 = 1;
        for u3 = 1:n_LD
            if Branch(u3,3)== nd  %Checks if nd is a load node, if not, do not update C
                for u4=1:length(LP)     %for each loop Branch
                    C(u3,LP(u4)) =1;    %set C entry equal to 1
                end
                if temp2 == 1 % if first iteration
                    LP_i = [LP u3]; % append the load branch number to LP
                    nd_i;
                    temp2=0;
                    for u7=1:length(nd_i) % for # of branches in loop
                        for u8=1:length(BUS_nds) % see if its a level 2 node
                            if nd_i(u7) == BUS_nds(u8)
                                % zero out BUS_nds location to only visit
                                % it once
                                BUS_nds(u8) = 0;%BUS_nds(length(BUS_nds));
                                %BUS_nds = BUS_nds(1:length(BUS_nds)-1);% Remove bus node 
                                for u9=u7:length(nd_i)
                                    % define which voltages to add up for
                                    % the bus
                                    y_C(u10,LP_i(u9)) =1;
                                end
                                % log the bus numbmer and increment index
                                % variable
                                B_No(u10) = nd_i(u7);
                                u10=u10+1;
                            end
                        end
                    end
                end
            end
        end
        %add all except last LP branch to N_LD_Branch
        if exist('LP','var')
            p = length(N_LD_Branch(:,1));
            for u5=1:length(LP)-1
                N_LD_Branch(p+u5,:)=Branch(LP(u5),:);
            end
            clear LP
            temp = temp+1;
        end
    end
    
    % initialize R, L and alpha matrices
    L = eye(length(B_ndx)); % inductance for each branch
    R = L; % resistance for each branch 
    alpha = zeros(length(L)); % QUESTION: what is the alpha matrix?
    for i=1:length(L) % for each branch
        R(i,i) = real(Branch(i,4)); % branch i resistance
        L(i,i) = imag(Branch(i,4)); % branch i inductance
        % alpha matrix
        if Branch(i,3)==Branch(length(Branch(:,1)),3)
            alpha(i,i) = -1;
        end
    end

    Eqn = [C*L*C' C*R*C' C*alpha]; % generate the circuit equations
    Eqn2 = rref(Eqn); % solve the for the variable coefficients
    % define the A and B matrices of the state space equation
    A_M = -Eqn2(:,n_LD+1:2*n_LD);
    B_M = -Eqn2(:,end); % Got rid of zeros in B matrix!!!
    C_Mr = y_C*diag(real(Branch(:,4)))*C';%eye(length(B_M));
    C_Mi = y_C*diag(imag(Branch(:,4)))*C';%eye(length(B_M));
   
    
end


