clear
%---------------------------------------------------|
%----MULTI REGIME - FIRST ORDER - FINITE BUFFER-----|
%---------WITH ORDERED SCHUR DECOMPOSITION----------|
%-----------NONE OF THE RATES IS ZERO---------------|
%------  NONE OF THE STATES IS REPULSIVE------------|
%---------------------------------------------------|

%---------Output: Statewise PDF Plot----------------|
%---------Output: Total PDF Plot--------------------|
%---------Output: Statewise PDF Plot----------------|
%---------Output: Total CDF Plot--------------------|


%------------------PARAMETERS-----------------------|                                                   
Thresholds=[0,1,3]; %terminal and intermediate boundary points
QR{1}=[-2,1,1;1,-2,1;1,1,-2];%infinitesimal generator in region 1          
QR{2}=2*QR{1};%[-5,2,1,1,1;0,-3,1,1,1;2,1,-5,0,2;0,1,1,-3,1;1,1,1,1,-4];%infinitesimal generator in region 1                    
QT{1}=QR{1};%infinitesimal generator in region 1          
QT{2}=QR{1};%infinitesimal generator in region 2          
QT{3}=QR{2};%infinitesimal generator in region 1          
ratesR{1}=[-0.99,-1,2];%rate in region 1: states 1 and 2 respectively     
ratesR{2}=[-0.5,-1,1];%rate in 
RT{1}=[0,0,2];%rate in region 1: states 1 and 2 respectively     
RT{2}=[-0.99,-1,2];%rate in 
RT{3}=[-0.5,-1,0];%rate in region 1: states 1 and 2 respectively     
noofsamples=1000;%For Plotting                     %|
%---------------------------------------------------|


%-------------------PREPARE-------------------------|
r=length(ratesR);                                  %|
s=length(ratesR{1});                               %|
accsstate=zeros(1,s);%takes values in 1->Nstates   %|
accsbound=zeros(1,s);%takes values in 1->Nregions  %|
%A)Find Accumulations:                             %|
%---------------------                             %|
k=1;                                               %|
%Accumulations at zero                             %|
for i=1:s                                          %|
    if ratesR{1}(i)<0                              %|
        accsstate(k)=i;                            %|
        accsbound(k)=0;                            %|
        k=k+1;                                     %|
    end                                            %|
end                                                %|
%Accumulation at Intermediate Boundaries           %|
for j=1:r-1                                        %|
    for i=1:s                                      %|
        if ratesR{j}(i)>0&ratesR{j+1}(i)<0         %|
            accsstate(k)=i;                        %|
            accsbound(k)=j;                        %|
            k=k+1;                                 %|
        end                                        %|
    end                                            %|
end                                                %|
%Accumulations at Buffer Length                    %|
for i=1:s                                          %|
    if ratesR{r}(i)>0                              %|
        accsstate(k)=i;                            %|
        accsbound(k)=r;                            %|
        k=k+1;                                     %|
    end                                            %|
end                                                %|
%-   -   -   -   -   -   -   -   -   -   -   -   - %|
%B)Ordered Schur Decomposition:                    %|
%------------------------------                    %|
for i=1:r                                          %|    
   %[U,U_inv,A,Aplus,Aminus,Nminus,pik,lambda]=additive2_decompose_schur2(diag(ratesR{i}),QR{i});
   [U_inv,Aplus,Aminus,Nminus] = additive2_decompose_schur(diag(ratesR{i}),QR{i});
   Nplus=s-Nminus-1;                               %|
   Lzero=U_inv(Nminus+1,1:s);                      %|
   Lminus=U_inv(1:Nminus,1:s);                     %|
   Lplus=U_inv(Nminus+2:s,1:s);                    %|
   Uinvc{i}=U_inv;                                 %|
   Aplusc{i}=Aplus;                                %|
   Aminusc{i}=Aminus;                              %|
   Nplusc{i}=Nplus;                                %|
   Nminusc{i}=Nminus;                              %|
   Lzeroc{i}=Lzero;                                %|
   Lplusc{i}=Lplus;                                %|
   Lminusc{i}=Lminus;                              %|
end                                                %|
%---------------------------------------------------|

disp('solving')
%---------------------SOLUTION----------------------|
totalmatrix=zeros(r*s+s,r*s+s+1);                  %|
%A) Boundary Conditions:                           %|
%-----------------------                           %|
%Boundary C. at Zero                               %| 
totalmatrix(1:s,1:s)=[Lzeroc{1}*diag(ratesR{1});...%|
 Lminusc{1}*diag(ratesR{1});...                    %|
 expm(-Aplusc{1}*Thresholds(2))*Lplusc{1}*diag(ratesR{1})]; 
integ=[Lzeroc{1}*Thresholds(2);...                 %|
 Aminusc{1}^-1*(expm(Aminusc{1}*Thresholds(2))-eye(Nminusc{1}))*Lminusc{1};...
 Aplusc{1}^-1*(eye(Nplusc{1})-expm(-Aplusc{1}*Thresholds(2)))*Lplusc{1}]*ones(s,1);
totalmatrix(1:s,r*s+s+1)=integ;                    %|
%Boundary C. at Thresholds                         %| 
if r>1                                             %|
    for i=1:r-1                                    %|
        totalmatrix((i-1)*s+1:i*s,i*s+1:(i+1)*s)=[-Lzeroc{i}*diag(ratesR{i});...
         -expm(Aminusc{i}*(Thresholds(i+1)-Thresholds(i)))*Lminusc{i}*diag(ratesR{i});...
         -Lplusc{i}*diag(ratesR{i})];              %|
        totalmatrix(i*s+1:(i+1)*s,i*s+1:(i+1)*s)=[Lzeroc{i+1}*diag(ratesR{i+1});...
         Lminusc{i+1}*diag(ratesR{i+1});...        %|
         expm(-Aplusc{i+1}*(Thresholds(i+2)-Thresholds(i+1)))*Lplusc{i+1}*diag(ratesR{i+1})];
         integ=[Lzeroc{i+1}*(Thresholds(i+2)-Thresholds(i+1));...
         Aminusc{i+1}^-1*(expm(Aminusc{i+1}*(Thresholds(i+2)-Thresholds(i+1)))-eye(Nminusc{i+1}))*Lminusc{i+1};...
         Aplusc{i+1}^-1*(eye(Nplusc{i+1})-expm(-Aplusc{i+1}*(Thresholds(i+2)-Thresholds(i+1))))*Lplusc{i+1}]*ones(s,1);
        totalmatrix(i*s+1:(i+1)*s,r*s+s+1)=integ;  %|
    end                                            %|
else                                               %|
    i=0;                                           %|
end                                                %|               
%Boundary C. at Buffer Length                      %| 
totalmatrix(i*s+1:(i+1)*s,(i+1)*s+1:(i+2)*s)=[Lzeroc{i+1}*diag(ratesR{i+1});...
 expm(Aminusc{i+1}*(Thresholds(i+2)-Thresholds(i+1)))*Lminusc{i+1}*diag(ratesR{i+1});...
 Lplusc{i+1}*diag(ratesR{i+1})];                   %|
totalmatrix(r*s+1:(r+1)*s,r*s+s+1)=ones(s,1);      %|
for i=1:s                                          %|
    if accsbound(i)==0                             %|
        totalmatrix(r*s+i,1:s)=-QT{1}(accsstate(i),:);
    elseif accsbound(i)==r                         %|
        totalmatrix(r*s+i,r*s+1:(r+1)*s)=QT{r+1}(accsstate(i),:);
    else                                           %|
        totalmatrix(r*s+i,accsbound(i)*s+1:(accsbound(i)+1)*s)=-QT{accsbound(i)+1}(accsstate(i),:);
    end                                            %|
end                                                %|
%-   -   -   -   -   -   -   -   -   -   -   -   - %|
disp('solving')
%B)Solution:                                       %|
%-----------                                       %|
totalvector=[zeros(1,r*s+s) 1];                    %|
solution=totalvector/totalmatrix;                  %|
%---------------------------------------------------|

disp('plotting')
%---------------------PLOTS-------------------------|
samples=9000+1;                                    %|
deltax=Thresholds(r+1)/9000;                       %|
%pdfs=zeros(s,samples);                             %|
cdfs=zeros(s,samples);                             %|
%totalpdfs=zeros(1,samples);                        %|
totalcdfs=zeros(1,samples);                        %|
for i=1:r                                          %|
    azeroc{i}=solution((i-1)*s+1);                 %|
    aminusc{i}=solution((i-1)*s+2:(i-1)*s+1+Nminusc{i});
    aplusc{i}=solution((i-1)*s+2+Nminusc{i}:i*s);  %|
end                                                %|
i=1;                                               %|
%for j=1:samples                                    %|
%    if j*deltax<=Thresholds(i+1)                   %|
%        pdfs(:,j)=(azeroc{i}*Lzeroc{i}+...         %|
%         aminusc{i}*expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))*Lminusc{i}+...
%         aplusc{i}*expm(-Aplusc{i}*(Thresholds(i+1)-(j-1)*deltax))*Lplusc{i})';
%    elseif i<r                                     %|
%        i=i+1;                                     %|
%        pdfs(:,j)=(azeroc{i}*Lzeroc{i}+...         %|
%         aminusc{i}*expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))*Lminusc{i}+...
%         aplusc{i}*expm(-Aplusc{i}*(Thresholds(i+1)-(j-1)*deltax))*Lplusc{i})';
%    else                                           %|
%        pdfs(:,j)=(azeroc{i}*Lzeroc{i}+...         %|
%         aminusc{i}*expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))*Lminusc{i}+...
%         aplusc{i}*Lplusc{i})';                    %|
%    end                                            %|
%end                                                %|
i=1;                                               %|
m=0;                                               %|
for j=1:samples                                    %|
    j
    if j*deltax<=Thresholds(i+1)                   %|
        cdfs(:,j)= m+(azeroc{i}*Lzeroc{i}*((j-1)*deltax-Thresholds(i))+...
         aminusc{i}*(Aminusc{i}^-1)*(expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))-eye(Nminusc{i}))*Lminusc{i}+...
         aplusc{i}*Aplusc{i}^-1*(expm(-Aplusc{i}*(Thresholds(i+1)-(j-1)*deltax))-expm(-Aplusc{i}*(Thresholds(i+1)-Thresholds(i))))*Lplusc{i})';  
    elseif i<r                                     %|
        m=cdfs(:,j-1);                             %|
        i=i+1;                                     %|
        cdfs(:,j)= m+(azeroc{i}*Lzeroc{i}*((j-1)*deltax-Thresholds(i))+...
         aminusc{i}*(Aminusc{i}^-1)*(expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))-eye(Nminusc{i}))*Lminusc{i}+...
         aplusc{i}*Aplusc{i}^-1*(expm(-Aplusc{i}*(Thresholds(i+1)-(j-1)*deltax))-expm(-Aplusc{i}*(Thresholds(i+1)-Thresholds(i))))*Lplusc{i})';
    else
        cdfs(:,j)= m+(azeroc{i}*Lzeroc{i}*((j-1)*deltax-Thresholds(i))+...
         aminusc{i}*(Aminusc{i}^-1)*(expm(Aminusc{i}*((j-1)*deltax-Thresholds(i)))-eye(Nminusc{i}))*Lminusc{i}+...
         aplusc{i}*Aplusc{i}^-1*(expm(-Aplusc{i}*(Thresholds(i+1)-(j-1)*deltax))-expm(-Aplusc{i}*(Thresholds(i+1)-Thresholds(i))))*Lplusc{i})';    
    end                                            %|
end                                                %|   
accbef=0;                                          %|
for k=1:s                                          %|
   if accsbound(k)==0                              %|
        accbef=solution(s*r+k);                    %|
        cdfs(accsstate(k),1:samples)=cdfs(accsstate(k),1:samples)+accbef;
   end                                             %| 
end                                                %|
for i=1:r-1                                        %|
    for k=1:s                                      %|
        if accsbound(k)==i                         %|
            accbef=solution(s*r+k);                %|
            cdfs(accsstate(k),floor(Thresholds(i+1)/deltax):samples)=cdfs(accsstate(k),floor(Thresholds(i+1)/deltax):samples)+accbef;    
        end                                        %|
    end                                            %|
end                                                %|
for k=1:s                                          %|
    if accsbound(k)==r                             %|
        accbef=solution(s*r+k);                    %|
        cdfs(accsstate(k),samples)=cdfs(accsstate(k),samples-1)+accbef;
    end                                            %|
end                                                %|
t=0:deltax:deltax*(samples-1);                     %|
%figure(1);                                         %|
%hold on                                            %|
%for i=1:s                                          %|
%    plot(t,pdfs(i,:),'y');                             %|
%end                                                %|
t=0:deltax:deltax*(samples-1);                     %|
figure(3);                                         %|
hold on                                            %|
for i=1:s                                          %|
    plot(t,cdfs(i,:),'y');                             %|
end                                                %|
totalcdfs=ones(1,s)*cdfs;                          %|
%totalpdfs=ones(1,s)*pdfs;                          %|
t=0:deltax:deltax*(samples-1);                     %|
%figure(2);                                         %|
%hold on                                            %|
%plot(t,totalpdfs,'y');                                 %|
t=0:deltax:deltax*(samples-1);                     %|
hold on                                            %|
figure(4);hold on;plot(t,totalcdfs,'r');                       %|
%---------------------------------------------------|
solution