
% Implementation of the algorithm as discussed in Section 5.2 to solve 
% Problem 5.10
% NOTE: This implementation assumes, that there are _no_ fast transient states.
% However, the extension for such a case should be pretty straightforward

% Initialization
N=6; % N = 6 nodes
almostInfinity=250; % number of iterations for Jensen's method

% The initial probability vector
sigma_initial = ones(1,N)/N;

% The generator matrix from section 4.1
Q = [ 
      -4.80  4.50       0    0.30       0       0;
       2.40 -7.35    4.50    0.15    0.30       0;
          0  2.40   -2.55       0    0.15       0;
       0.20  0.20       0   -5.20    4.50    0.30;
          0  0.20    0.20    2.40   -2.95    0.15;
          0     0       0    0.20    0.20   -0.40 ];


% Calculation of the steady-state probability vector
Qnc = Q;
Qnc(:,end) = 1;

b = zeros(1,N);
b(end) = 1;
pi = b * inv(Qnc)

% PI will contain all transient results
PI=[];
% times instances
times=[1,2,3,5,10,12];
for T = times
  % === STEP 1 ===
  % Specify tau >> 1/T: For the sake of simplicity we choose
  % tau = 1 for all time instances
  tau = 1;
  % Find the fast states
  fastStates = find( any( (Q>tau)' ) );
  % Find the slow states
  slowStates = find( all( (Q<=tau)' ) );

  % Qfast is the generator matrix of the fast states
  Qfast = Q(fastStates,fastStates);

  % Qperm is needed to rearrange the generator matrix (see further below)
  Qperm = slowStates;

  % ccs will contain the connected components of the fast states: each row i
  % contains the indices j of the of the fast recurrent subset i
  ccs=[];

  % === STEP 2 ===
  % Are there any fast states?
  if length(fastStates) > 0

    % In order to obtain the (strongly) connected components of
    % Qfast we define an adjacency matrix

    % There is an edge (adj_ij == 1) from i to j iff i != j and Qfast_ij > tau
    % or i == j
    adj = (Qfast>tau) + eye(size(Qfast));
    % Make a stochastic matrix out of adj by normalizing each row
    norm = 1./sum(adj)';
    nadj = diag(norm) * adj;
  
    % Now calculate the closure of the adjacency matrix
    closure=nadj;
    % For k take all values from 1 to the maximum length of a path between two
    % arbitrary nodes
    for k = (1:length(adj)-1)
      closure = closure + closure*nadj ./ length(adj);
    end
    % Extract all values > 0: Two state i and j are in the same
    % fast recurrent set iff closure_ij * closure_ji > 0
    % NOTE: closure defines a equivalence relation iff S_F = empty
    closure = closure > 0;

    % Now we have to find the indices of the nodes in the fast recurrent
    % subsets and store them in ccs
    done=[];
    for state = (1:length(fastStates))
      % Are there any nodes left to process?
      if ~any(done == state)
        % The current cc is empty at first
        cc=[];
        % For any state2 in the same fast recurrent subset of state do...
        for state2 = find(closure(state,:))
          % Is there a path from state2 to state?
          if closure(state2,state)
            done = [ done, state2 ];
            cc = [ cc, state2 ];
          end
        end
        % Now add the current cc to the set of connected components in ccs:
        ccs = [ ccs; [ cc, zeros( 1, length(fastStates)-length(cc)) ] ];
        % Add indices to permutation vector
        Qperm = [ Qperm, fastStates(cc) ];
      end
    end

    % Now compute the steady-state probabilities of the fast recurrent
    % macro states

    % piMS will contain the steady-state probabilities of the fast recurrent
    % subsets: piMS_ij contains steady-state probability of state j in subset i
    piMS = [];
    % For every fast recurrent subset do...
    for c = ccs'
      % Get a shorter version of the row c without the trailing zeros
      cc = fastStates( c( find( c>0 ) ) );
      % Extract the (quasi-)generator matrix
      QI = Q(cc,cc);
      % Make a real generator matrix out of QI
      rowsum = sum(QI');
      QI = QI - diag(rowsum);
      % Prepare everthing to calculate the steady-state probability vector
      QI(:,end) = 1;
      b = zeros(1,length(QI));
      b(end) = 1;
      % Now calculate it and append it to piMS (with trailing zeros)
      piI = b * inv(QI);
      piMS = [ piMS; [ piI, zeros( 1, length(fastStates)-length(piI)) ] ];
    end

  end

  % === STEP 3 ===
  % m is the number of fast recurrent subsets
  [ m n ] = size(ccs);

  % Construction of the intermediate generator matrix
  Sigma_tilde=zeros(length(slowStates)+m,length(slowStates)+m);
  % Eq. (5.40): from slow states to slow states
  Sigma_tilde_00 = Q(slowStates,slowStates);
  Sigma_tilde(1:length(slowStates),1:length(slowStates)) = Sigma_tilde_00;

  % For all fast recurrent subsets calculate the transitions ...
  for k = (1:m)
    % Eq. (5.45): ... from fast subsets to slow states
    piI = piMS(k, find( piMS(k,:) > 0 ) );
    c = ccs(k,:);
    cc = fastStates( c ( find( c>0 ) ) );
    Sigma_tilde_I0  = piI * Q( cc, slowStates );

    % Eq. (5.46): ... from slow states to fast subsets
    Sigma_tilde_0I = Q( slowStates, cc ) * ones( length(cc), 1 );
    
    Sigma_tilde(length(slowStates)+k,(1:length(slowStates))) = Sigma_tilde_I0;
    Sigma_tilde(1:length(slowStates),length(slowStates)+k) = Sigma_tilde_0I;

    % Eq. (5.47): ... from fast subsets to fast subsets
    for j = (1:m)
      if j ~= i
        c2 = ccs(j,:);
        cc2 = fastStates( c2 ( find( c2>0  ) ) );
        Sigma_tilde_IJ = piI * Q(cc,cc2) * ones(length(cc2),1);

        Sigma_tilde(length(slowStates)+k,length(slowStates)+j) = Sigma_tilde_IJ;
      end
    end 
    
  end

  % Make a generator matrix out of the intermediate generator matrix
  rowsum = sum(Sigma_tilde');
  Sigma_tilde = Sigma_tilde - diag(rowsum);

  % === STEP 4 ===
  % S_F = empty =>
  Sigma= Sigma_tilde;


  % === STEP 5 ===
  % Aggregation of the initial state probability vector sigma: S_F = empty =>
  sigma = sigma_initial(slowStates);
  for c = ccs'
    cc = fastStates( c( find( c>0 ) ) );
    sigma = [ sigma, sum(sigma_initial(cc)) ];
  end

  % === STEP 6 ===
  % We use Jensen's method for the transient analysis of Sigma (Section
  % 5.1.4.1); this part can be replaced by any method for transient analysis

  % BEGIN: Jensen's method
  q = max(max(abs(Sigma)))+0.8;
  P = Sigma/q + eye(size(Sigma));

  nu = sigma;

  for t = (1:almostInfinity)
    nu = [ nu; nu(end,:) * P  ];
  end

  sigma_approx=zeros(size(sigma));
  for j = (1:almostInfinity)
    sigma_approx = sigma_approx + nu(j,:) * exp(-q*T) * ((q*T)^j / prod(1:j));
  end
  % END: Jensen's method

  % sigma_approx contains the results from the transient analysis

  % === STEP 7 & 8 ===
  % Disaggregations
  pi_transient = sigma_approx(1:length(slowStates));
  for k = (1:m)
    piI = piMS(k, find( piMS(k,:) > 0 ) );
    pi_transient = [ pi_transient, sigma_approx(length(slowStates)+k) * piI ];
  end

  % Rearrange the vector to its original organization by inverting the permutation
  % defined by Qperm
  invQperm = zeros(size(Qperm));
  for idx = (1:length(Qperm))
    invQperm(Qperm(idx)) = idx;
  end
  % Rearrange to its original organization
  pi_transient = pi_transient(invQperm);

  fprintf( '=========================INTERMEDIATE RESULTS===================================\n' )
  fprintf( '=== time instance ===\n' );
  T
  fprintf( '=== fast recurrent subsets: rows = subsets, columns = indices of states ===\n' );
  ccs
  fprintf( '=== Transient probabilities: columns = states ===\n' );
  pi_transient

  PI = [ PI; T, pi_transient ];

end
PI = [ PI; inf, pi ];
fprintf( '================================================================================\n' )
fprintf( '=== SUMMARY: first column = time instance, other columns = transient prob. ===\n' );
PI
