%----------------------------------------------------%|
%--------------SECOND ORDER MULTIREGIME--------------%|
%---------------FEEDBACK FLUID QUEUES----------------%|
%--------------------SIMULATION----------------------%|
%----------------------------------------------------%|
%Author: H: Emre Kankaya                             %|
%----------------------------------------------------%|
%-----------------DESCRIPTION------------------------%|
%----------------------------------------------------%|

%----------------------------------------------------%|
clear;




%----------------------------------------------------%|
%---------------SYSTEM PARAMETERS--------------------%|
%----------------------------------------------------%|
%assume that emitting boundary points are left continuous in parameters
thresholds=[0,0.5,1];
driftsR{1}=[1,2];%drifts in regime 1
driftsR{2}=[-1,2];%drifts in regime 2
driftsT{1}=[1,2];%drifts in regime 1
driftsT{2}=[0,2];%drifts in regime 1
driftsT{3}=[-1,0];%drifts in regime 1

varR{1}=[0.1,0.1]*10;
varR{2}=[0.1,0.1]*10;
varT{1}=[0.1,0.1]*10;
varT{2}=[0,0.1]*10;
varT{3}=[0.1,0.1]*10;
RefvsAbs{1}=[1,1];%at T^(0)=0; 1 for reflecting, 0 for absorbing, 2 for first order 
RefvsAbs{2}=[1,1];%at T^(K)
QR{1}=[-1 1;1,-1];       %|
QR{2}=[-1 1;1,-1];       %|
QT{1}=[-1 1;1,-1];       %|
QT{2}=[-1 1;1,-1];       %|
QT{3}=[-1 1;1,-1];       %|
noofsamples=1000;%For Plotting                     %|
%thresholds=[0,1,2];
%driftsR{1}=[1,1,-1,0];%drifts in regime 1
%driftsR{2}=[2,-1,1,-1];%drifts in regime 2
%driftsT{1}=[0,1,0,0];%drifts in regime 1
%driftsT{2}=[1,0,-1,0];%drifts in regime 1
%driftsT{3}=[0,-1,0,-1];%drifts in regime 1
%varR{1}=[0.1,0,0,0.1];
%varR{2}=[0.1,0,0,0];
%varT{1}=[0.1,0,0,0];
%varT{2}=[0.1,0,0,0.1];
%varT{3}=[0.1,0,0,0];
%RefvsAbs{1}=[1,0,0,0];%at T^(0)=0; 1 for reflecting, 0 for absorbing, 2 for first order 
%RefvsAbs{2}=[1,0,0,0];%at T^(K)
%QR{1}=[-3,1,1,1;1,-3,1,1;1,1,-3,1;1,1,1,-3];       %|
%QR{2}=[-3,1,1,1;1,-2,1,0;1,1,-3,1;1,1,1,-3];
%QT{1}=[-3,1,1,1;1,-3,1,1;1,1,-3,1;1,1,1,-3];
%QT{2}=[-3,1,1,1;1,-2,1,0;1,1,-3,1;1,1,1,-3];
%QT{3}=[-3,1,1,1;1,-2,1,0;1,1,-3,1;1,1,1,-3];

simultime=20000;
deltat=0.000025;
deltaxstat=0.001;

%----------------------------------------------------%|

%----------------------------------------------------%|
%-----------------INITIALIZATIONS--------------------%|
%----------------------------------------------------%|
t=0;
currentstate=1;
currentcontent=0.001;
regimevsbound=1;%0 for boundary, 1 for regime
currentregime=1;%if regimevsbound=0, values 0,...,K,
%if regimevsbound=1, values 1,...,K,
r=length(driftsR);%number of regimes                 %|
s=length(driftsR{1});%number of states                %|
change=0;
deltax=sqrt(deltat);
m=floor(thresholds(r+1)/deltaxstat)+1;
contentx=zeros(s,m);

%----------------------------------------------------%|
%------------------BOUNDARY TYPES--------------------%|
%----------------------------------------------------%|
%Classify intermediate boundary points
it2no=zeros(1,r-1);
it5no=zeros(1,r-1);
itmatrix=zeros(r-1,s);
for i=1:r-1
    for j=1:s
        if varR{i}(j)>0%+
            if driftsR{i+1}(j)<0&varR{i+1}(j)==0
                it2no(i)=it2no(i)+1;                
                itmatrix(i,j)=2;                
            end
        elseif driftsR{i}(j)>0&varR{i}(j)==0%0+
            if varR{i+1}(j)>0
                it5no(i)=it5no(i)+1;                               
                itmatrix(i,j)=5;                
            end
        end
    end
end






%----------------------------------------------------%|
%------------------CONSTRUCTIONS---------------------%|
%----------------------------------------------------%|
%TRANSITION PROBABILITIES AMONG STATES FOR BOUNDARY Pts
for i=1:r+1
    for j=1:s
        QT_probs{i}{j}=zeros(1,s+1);
        gecici=zeros(1,s+1);
        for k=2:s+1
            if ~(j==k-1)
                gecici(k)=-QT{i}(j,k-1)/QT{i}(j,j);               
            end
        end        
        QT_probs{i}{j}=gecici;
        for k=2:s+1
            QT_probs{i}{j}(k)=QT_probs{i}{j}(k)+QT_probs{i}{j}(k-1);
        end
    end
end

%TRANSITION PROBABILITIES AMONG STATES FOR REGIMES
for i=1:r
    for j=1:s
        QR_probs{i}{j}=zeros(1,s+1);
        gecici=zeros(1,s+1);
        for k=2:s+1
            if ~(j==k-1)
                gecici(k)=-QR{i}(j,k-1)/QR{i}(j,j);               
            end
        end        
        QR_probs{i}{j}=gecici;
        for k=2:s+1
            QR_probs{i}{j}(k)=QR_probs{i}{j}(k)+QR_probs{i}{j}(k-1);
        end
    end
end

inc=0;

%----------------------------------------------------%|
%-----------------SIMULATION-------------------------%|
%----------------------------------------------------%|
while t<simultime 
    
    r1=rand;
    if regimevsbound==0%at threshold        
        if r1<-QT{currentregime+1}(currentstate,currentstate)*deltat
            %A1-1/2-Yes
            %Q2-to which state it transits?
            change=1;
            r2=rand;
            for i=1:s
                if r2>QT_probs{currentregime+1}{currentstate}(i)&r2<QT_probs{currentregime+1}{currentstate}(i+1)&change==1
                    currentstate=i;
                    change=0;
                end
            end
        end                
    else%regimevsbound=1;in regime 
        if r1<-QR{currentregime}(currentstate,currentstate)*deltat
            %A1-1/2-Yes
            %Q2-to which state it transits?
            change=1;
            r2=rand;
            for i=1:s
                if r2>QR_probs{currentregime}{currentstate}(i)&r2<QR_probs{currentregime}{currentstate}(i+1)&change==1
                    change=0;
                    currentstate=i;
                end
            end
        end                
    end
    %Q2- What's the increment in deltat?
    if regimevsbound==0%at threshold   
        
            if driftsT{currentregime+1}(currentstate)==0&varT{currentregime+1}(currentstate)==0            
                currentcontent=currentcontent;        
            else
                r3=sign(rand-0.5);  
                 if currentregime==0
                    r3=1;
                elseif currentregime==r
                    r3=0;
                end

                inc=r3*deltax*sqrt(varT{currentregime+1}(currentstate))+driftsT{currentregime+1}(currentstate)*deltat; 
                currentcontent=currentcontent+inc;
            end
          
    else%in regime
        r3=sign(rand-0.5);        
        inc=r3*deltax*sqrt(varR{currentregime}(currentstate))+driftsR{currentregime}(currentstate)*deltat; 
        if currentregime>1&currentregime<r                       
            if currentcontent+inc>thresholds(currentregime+1)
                if itmatrix(currentregime,currentstate)==2
                    currentcontent=2*thresholds(currentregime+1)-currentcontent-inc;
                    if currentcontent==thresholds(currentregime+1)
                      	currentcontent=currentcontent-0.001;
                    end                    	
                elseif driftsT{currentregime+1}(currentstate)==0&varT{currentregime+1}(currentstate)==0
                  	currentcontent=thresholds(currentregime+1);
                else%special increment                          
                x1=(-sqrt(varR{currentregime}(currentstate))*r3+sqrt(varR{currentregime}(currentstate)+4*(-currentcontent+thresholds(currentregime+1))*driftsR{currentregime}(currentstate)))/driftsR{currentregime}(currentstate)/2;
                x2=(-sqrt(varR{currentregime}(currentstate))*r3-sqrt(varR{currentregime}(currentstate)+4*(-currentcontent+thresholds(currentregime+1))*driftsR{currentregime}(currentstate)))/driftsR{currentregime}(currentstate)/2;                                    
                    if x1>=0&x1<sqrt(deltat)+10^-10
                        x=x1;
                    elseif x2>=0&x2<sqrt(deltat)+10^-10
                        x=x2;
                    else
                        disp('error');
                        x1
                        x2
                    end
                    tt=x^2;
                    r3=sign(rand-0.5);                        
                    xx=abs(sqrt(deltat-tt));
                    inc=r3*xx*sqrt(varR{currentregime+1}(currentstate))+driftsR{currentregime+1}(currentstate)*(deltat-tt);         
                    if varR{currentregime+1}(currentstate)>0&inc<0
                        r3=1;
                        xx=abs(sqrt(deltat-tt));
                        inc=r3*xx*sqrt(varR{currentregime+1}(currentstate))+driftsR{currentregime+1}(currentstate)*(deltat-tt);         
                    end
                    currentcontent=thresholds(currentregime+1)+inc;                                                     
                end                
            elseif currentcontent+inc<thresholds(currentregime)                
                if itmatrix(currentregime-1,currentstate)==5
                    currentcontent=2*thresholds(currentregime)-currentcontent-inc;
                    if currentcontent==thresholds(currentregime)
                        currentcontent=currentcontent+0.001;
                    end
                elseif driftsT{currentregime}(currentstate)==0&varT{currentregime}(currentstate)==0
                    currentcontent=thresholds(currentregime);
                else
                    x1=(-(varR{currentregime}(currentstate))^0.5*r3+(varR{currentregime}(currentstate)+4*(thresholds(currentregime)-(currentcontent))*driftsR{currentregime}(currentstate))^0.5)/driftsR{currentregime}(currentstate)/2;
                    x2=(-(varR{currentregime}(currentstate))^0.5*r3-(varR{currentregime}(currentstate)+4*(thresholds(currentregime)-(currentcontent))*driftsR{currentregime}(currentstate))^0.5)/driftsR{currentregime}(currentstate)/2;
                    if x1>=0&x1<sqrt(deltat)+10^-10
                        x=x1;%                 
                    elseif x2>=0&x2<sqrt(deltat)+10^-10
                        x=x2;
                    else
                        x1
                        x2
                        disp('error');
                    end
                    tt=x^2;
                    r3=sign(rand-0.5);
                    xx=abs(sqrt(deltat-tt));
                    inc=r3*xx*sqrt(varR{currentregime-1}(currentstate))+driftsR{currentregime-1}(currentstate)*(deltat-tt);         
                    if varR{currentregime-1}(currentstate)>0&inc>0
                        r3=-1;
                        xx=abs(sqrt(deltat-tt));
                        inc=r3*xx*sqrt(varR{currentregime-1}(currentstate))+driftsR{currentregime-1}(currentstate)*(deltat-tt);         
                    end
                    currentcontent=thresholds(currentregime)+inc;                                                             
                end
            else
                currentcontent=currentcontent+inc;                                 
            end            
        elseif currentregime==1
            if currentcontent+inc>0
				if currentcontent+inc>thresholds(currentregime+1)  
                	if itmatrix(currentregime,currentstate)==2
                    	currentcontent=2*thresholds(currentregime+1)-currentcontent-inc;
                    	if currentcontent==thresholds(currentregime+1)
                        	currentcontent=currentcontent-0.001;
                        end                    	
                	elseif driftsT{currentregime+1}(currentstate)==0&varT{currentregime+1}(currentstate)==0
                    	currentcontent=thresholds(currentregime+1);
                    else%special increment                          
                        x1=(-sqrt(varR{1}(currentstate))*r3+sqrt(varR{1}(currentstate)+4*(-currentcontent+thresholds(2))*driftsR{1}(currentstate)))/driftsR{1}(currentstate)/2;
                        x2=(-sqrt(varR{1}(currentstate))*r3-sqrt(varR{1}(currentstate)+4*(-currentcontent+thresholds(2))*driftsR{1}(currentstate)))/driftsR{1}(currentstate)/2;                                    
                        if x1>=0&x1<sqrt(deltat)+10^-10
                            x=x1;
                        elseif x2>=0&x2<sqrt(deltat)+10^-10
                            x=x2;
                        else
                            disp('error');
                            x1
                            x2
                        end
                        tt=x^2;
                        r3=sign(rand-0.5);                        
                        xx=abs(sqrt(deltat-tt));
                        inc=r3*xx*sqrt(varR{2}(currentstate))+driftsR{2}(currentstate)*(deltat-tt);         
                        if varR{2}(currentstate)>0&inc<0
                            r3=1;
                            xx=abs(sqrt(deltat-tt));
                            inc=r3*xx*sqrt(varR{2}(currentstate))+driftsR{2}(currentstate)*(deltat-tt);         
                        end
                        currentcontent=thresholds(2)+inc;                                                     
                    end
                else
                    currentcontent=currentcontent+inc;             
                end
            else                
                if RefvsAbs{1}(currentstate)==0%threshold 0 is absorbing
                    currentcontent=0;  
                else%threshold 0 is reflecting
                    currentcontent=-(currentcontent+inc);             
                    if currentcontent==0
                        currentcontent=0.000001;
                    end
                end
            end
        else%currentregime==r
            if currentcontent+inc<thresholds(r+1)
                if currentcontent+inc<thresholds(r)
                    if itmatrix(currentregime-1,currentstate)==5
                        currentcontent=2*thresholds(currentregime)-currentcontent-inc;
                        if currentcontent==thresholds(currentregime)
                            currentcontent=currentcontent+0.001;
                        end
                    elseif driftsT{currentregime}(currentstate)==0&varT{currentregime}(currentstate)==0
                        currentcontent=thresholds(currentregime);
                    else
                        x1=(-(varR{r}(currentstate))^0.5*r3+(varR{r}(currentstate)+4*(thresholds(r)-(currentcontent))*driftsR{r}(currentstate))^0.5)/driftsR{r}(currentstate)/2;
                        x2=(-(varR{r}(currentstate))^0.5*r3-(varR{r}(currentstate)+4*(thresholds(r)-(currentcontent))*driftsR{r}(currentstate))^0.5)/driftsR{r}(currentstate)/2;
                        if x1>=0&x1<sqrt(deltat)+10^-10
                            x=x1;%                 
                        elseif x2>=0&x2<sqrt(deltat)+10^-10
                            x=x2;
                        else
                            x1
                            x2
                            disp('error');
                        end
                        tt=x^2;
                        r3=sign(rand-0.5);
                        xx=abs(sqrt(deltat-tt));
                        inc=r3*xx*sqrt(varR{r-1}(currentstate))+driftsR{r-1}(currentstate)*(deltat-tt);         
                        if varR{r-1}(currentstate)>0&inc>0
                            r3=-1;
                            xx=abs(sqrt(deltat-tt));
                            inc=r3*xx*sqrt(varR{r-1}(currentstate))+driftsR{r-1}(currentstate)*(deltat-tt);         
                        end
                        currentcontent=thresholds(r)+inc;                                                             
                    end
                else
                        currentcontent=currentcontent+inc;             
                    
                end
            else                
                if RefvsAbs{2}(currentstate)==0%threshold T^{K} is absorbing
                    currentcontent=thresholds(r+1);             
                else%threshold T^{K} is reflecting
                    currentcontent=2*thresholds(r+1)-(currentcontent+inc);             
                    if currentcontent==thresholds(r+1)
                        currentcontent=thresholds(r+1)-0.000001;
                    end
                end
            end
        end
    end    
    %Q3-What's is the new regime?
    for i=1:r+1
        if currentcontent==thresholds(i)
            currentregime=i-1;
            regimevsbound=0;
        end        
    end
    for i=1:r
        if currentcontent>thresholds(i)&currentcontent<thresholds(i+1)
            currentregime=i;
            regimevsbound=1;
        end
    end
    
    %Statistics
    mm=floor(currentcontent/deltaxstat)+1;
    contentx(currentstate,mm)=contentx(currentstate,mm)+1;
    t=t+deltat;
    
end
    
%----------------------------------------------------%|
%---------------------PLOTS--------------------------%|
%----------------------------------------------------%|
t=0:deltaxstat:deltaxstat*(m-1);
t2=0:deltaxstat*100:deltaxstat*(m-1);
%PDFs
S=['b' 'g' 'r' 'c' 'm'];
contentx=contentx./sum(sum(contentx))/deltaxstat;
%contentx2=zeros(1,21);
%for i=0:20
%    contentx2(i+1)=contentx((i)*100+1)
%end
figure(1);
hold on;
for i=1:s
plot(t,contentx(i,:),S(mod(i,5)+1));
end
totalpdfs=contentx(1,:);
for i=2:s
    totalpdfs=totalpdfs+contentx(i,:);
end
figure(3);
hold on;
plot(t,totalpdfs,S(mod(i,5)+1));

contentxcdf=contentx;
for i=2:m
    contentxcdf(:,i)=contentxcdf(:,i)+contentxcdf(:,i-1);
end
contentxcdf=contentxcdf*deltaxstat;
%contentx2=zeros(s,21);
%t2=0:deltaxstat*100:deltaxstat*(m-1);
%for i=0:20
%    contentx2(:,i+1)=contentxcdf(:,(i)*100+1)
%end

figure(2);
hold on;
for i=1:s
plot(t,contentxcdf(i,:),S(mod(i,5)+1));
end
totalcdfs=contentxcdf(1,:);
for i=2:s
    totalcdfs=totalcdfs+contentxcdf(i,:);
end

%contentx2=zeros(1,21);
%t2=0:deltaxstat*100:deltaxstat*(m-1);
for i=0:20
    contentx2(:,i+1)=contentxcdf(:,(i)*100+1)
end

figure(4);
hold on;
plot(t,totalcdfs,S(mod(i,5)+1));

