
% The partitions
S0=[1,2,3];
S1=[4,5];
S2=[6];


% Parameters from Table 4.1 (service rates of the service stations)
mu = ...
[ 0.00 4.50 0.30;
  2.40 0.00 0.15;
  0.20 0.20 0.00 ];


Q = ...
[       0 mu(1,2)       0 mu(1,3)       0       0 ;  % (200)
  mu(2,1)       0 mu(1,2) mu(2,3) mu(1,3)       0 ;  % (110)
        0 mu(2,1)       0       0 mu(2,3)       0 ;  % (020)
  mu(3,1) mu(3,2)       0       0 mu(1,2) mu(1,3) ;  % (101)
        0 mu(3,1) mu(3,2) mu(2,1)       0 mu(2,3) ;  % (011)
        0       0       0 mu(3,1) mu(3,2)       0 ]; % (002)
%   (200)   (110)   (020)   (101)   (011)   (002)

% Now calculate the negative row-sums
Q = Q - diag(sum(Q'));

% For the initial probability vector we choose a uniform distribution:
n=0;
pi=ones(1,6)/6;
q=10;
epsilon=0.00001;
c=1/2;

% As a norm we choose the sum of the absolute values of the probability vector,
% as well as for the residual error 
last_residualerror = 0
residualerror = norm( zeros(size(pi(end,:))) - pi(end,:) * Q, 1 )
while residualerror >= epsilon

  % Geometric convergence
  if n >= 1

    if residualerror > c * last_residualerror
      pi(end,:) = pi(end,:) + pi(end,:) * (Q/q);
    end
     
  end

  n = n + 1;

  % Now calculate the macro-state generator matrix according
  % to Eq. (4.34):
  Sigma = zeros(3,3);
  Sigma_Ij = zeros( 3, 6 );

  for I = (0:2)
    for J = (0:2)

      % Determine the partitions
      switch I
        case 0
          SI=S0;
        case 1
          SI=S1;
        case 2
          SI=S2;
      end
      switch J
        case 0
          SJ=S0;
        case 1
          SJ=S1;
        case 2
          SJ=S2;
      end

      QIJ = Q(SI,SJ);
      [ k, l ] = size( QIJ );
      row_sums = QIJ * ones( l, 1 );
      pi_macro = pi(end,SI)/sum(pi(end,SI));
      Sigma(I+1,J+1) = pi_macro * row_sums;

       % Now the transition rates from macro-state I to micro-states j in S can easily
       % be derived in this iteration (Eq. (4.35))
       Sigma_Ij(I+1,:) = pi_macro * Q(SI,:);
       Sigma_Ij(I+1,SI) = 0;

    end
  % Now the transition probabilities from macro-state I 
  end

  Sigma
  Sigma_Ij

  % Now calculate the equilibrium of Sigma by its left-sided eigenvector with the largest
  % eigenvalue (=0)
  [ evec, eval ] = eig(Sigma')
  sigma = evec(:, find( max(diag(eval)) == diag(eval) ) );
  sigma = sigma' / sum(sigma)

  % Add a new approximation vector
  pi = [ pi; zeros(1,6) ];

  % Now for every subset S_I (0 <= I < 3) the linear equations from Eq.~(4.23) need to be solved:
  for I = (0:2)

    % Determine the partition
    switch I
      case 0
        SI=S0;
      case 1
        SI=S1;
      case 2
        SI=S2;
    end

    % We have to solve the linear equations system x*A = b <=> x = b * inv(A), where
    A = Q(SI,SI)
    K = find( (0:2) != I);
    b = -sigma(K) * Sigma_Ij( K, SI )

    pi(end,SI) = b * inv(A)

  end

  % Normalize pi
  pi(end,:) = pi(end,:) / sum(pi(end,:))


  % Calculate the residual error
  last_residualerror = residualerror;
  residualerror = norm( zeros(size(pi(end,:))) - pi(end,:) * Q, 1 );

end

pi
