clear
clc
close all

N_bcm = 16;
N_cfm = 16;

L = 4;
K = 4;

chain_CFM = N_cfm/K;
chain_BCM = N_bcm/L;

N_car = N_bcm + N_cfm;

tau = 1.5;

k_d = 0.3;
k_v = 0.2;
dt = 0.1;
T_react = 1.5;

N_T = 2001;
car_length = 5;

v_avg = 20;
desired_space = v_avg*T_react; 

delta_u = desired_space + car_length;

track_length = N_car*delta_u;

index_CFM = zeros(1,N_cfm);
index_BCM = zeros(1,N_bcm);

for n = 0:chain_CFM-1
    index_CFM(n*K+1:n*K+K) = [n*(K+L)+1:n*(K+L)+K]-1;
end
    index_CFM = [index_CFM(3:end),N_car];
    
for n = 0:chain_BCM-1
    index_BCM(n*L+1:n*L+L) = [n*(K+L)+K+1:n*(K+L)+K+L]-1;
end




u0 = 0;
v0 = 0;
a0 = -5;



U=zeros(N_T, N_car);
V = U;
U1=U;
V1 = U1;
U_pos = U;

U_shift = (0 : N_car - 1)*delta_u;
U_shift1 = U_shift;


u = U_shift+10;
u1 = u; 

U_pos(1,:) = u;

Time = [0:N_T-1]*dt;

Y = zeros(1, N_car);

a_min = -5;
a_max = 5;
v_max = v_avg;
v_min = -44.44 + v_avg;

Total_length = N_car*delta_u;
dx_hist = Total_length/N_car;


figure(1);
set(gcf, 'position',[60,150,1800,400])
subplot('position',[0.03, 0.65, 0.94, 0.20])

plot(u, Y,'sk', 'MarkerSize',12,'MarkerFaceColor','w', 'lineWidth',2)
hold on
plot(u(1), Y(1),'sk', 'MarkerSize',12,'MarkerFaceColor','k', 'lineWidth',2)
title(sprintf('Pure traffic under CFM (time: 0.0 sec.). k_d = %1.1f, k_v = %1.1f, T = %1.1f', k_d, k_v, T_react));

xlim([0,U_shift(end)+delta_u]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[], 'FontSize', 16)
set(gca,'xtick', [0:4:N_car]*dx_hist, 'FontSize',20);
hold off


subplot('position',[0.03, 0.12, 0.94, 0.20])
plot(u1(index_BCM), Y(index_BCM),'sr', 'MarkerSize',12,'MarkerFaceColor','w','lineWidth',2)
hold on
plot(u1(index_CFM), Y(index_CFM),'sk', 'MarkerSize',12,'MarkerFaceColor','w', 'lineWidth',2)
plot(u1(1), Y(1),'sk', 'MarkerSize',12,'MarkerFaceColor','k', 'lineWidth',2)
title(sprintf('Mixed traffic with BCM and CFM (time: 0.0 sec.). k_d = %1.1f, k_v = %1.1f, T = %1.1f, tau = %1.1f', k_d, k_v, T_react,tau))

xlim([0,U_shift1(end)+delta_u]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[], 'FontSize', 16)
set(gca,'xtick', [0:4:N_car]*dx_hist, 'FontSize',20);
hold off



M(1) = getframe(gcf);

a1 = zeros(1, N_bcm);

for i = 2:N_T
    if i<32
    v0 = v0 - a0*dt;
    u0 = u0 + v0*dt - a0*dt^2/2;
    U(i,1) = u0;
    V(i,1) = v0;
    U1(i,1) = u0;
    V1(i,1) = v0;

    else
        
    du = U(i-1, 1) - U(i-1, end);
    dv = V(i-1, 1) - V(i-1, end); 
    du = du + V(i-1, 1)*T_react ; 
    du = k_d*du;
    dv = k_v*dv;
    a = max(a_min,min(a_max,du+dv));
    v = max(v_min,min(v_max,V(i-1,1) - a*dt)); 
    u = v*dt - a*dt^2/2;
   
        if  U(i-1,  1) + u  <  U(i-1,  end)  - desired_space
            U(i,  1)  =   U(i-1,  end)  - desired_space;
            v =  v_max;
        else
            U(i,1) = U(i-1,1) + u;
        end
            V(i,1) = v;
    
    
    du = U1(i-1, 1) - U1(i-1, end);
    dv = V1(i-1, 1) - V1(i-1, end); 
    du = du + V1(i-1, 1)*T_react ; 
    du = k_d*du;
    dv = k_v*dv;
    a = max(a_min,min(a_max,du+dv));
    v = max(v_min,min(v_max,V1(i-1,1) - a*dt)); 
    u = v*dt - a*dt^2/2;
   
        if  U1(i-1,  1) + u  <  U1(i-1,  end)  - desired_space
            U1(i,  1) =   U1(i-1,  end)  - desired_space;
            v =  v_max;
        else
            U1(i,1) = U1(i-1,1) + u;
        end
            V1(i,1) = v;
    
    
    end
    
    
    du = diff(U(i-1, :));
    dv = diff(V(i-1, :));
    
    du = du + V(i-1, 2:end)*T_react ; 
    du = k_d*du;
    
    dv = k_v*dv;
   
    
    a = max(a_min,min(a_max,du+dv));
    v = max(v_min,min(v_max,V(i-1,2:end) - a*dt)); 
    u = v*dt - a*dt^2/2;
    
        
     for n =  2: N_car
        if  U(i-1,  n) + u(n-1)  <  U(i-1,  n-1)  - desired_space
            U(i,  n) =   U(i-1,  n-1)  - desired_space;
            v(n-1) =  v_max;
        else
            U(i,n) = U(i-1,n) + u(n-1);
        end
     end
 
     V(i, 2:end) = v;


    du = U1(i-1, index_CFM) - U1(i-1, index_CFM-1);
    dv = V1(i-1, index_CFM) - V1(i-1, index_CFM-1);
    
    du = du + V1(i-1, index_CFM)*T_react ; 
    du = k_d*du;
    
    dv = k_v*dv;
   
    
    a = max(a_min,min(a_max,du+dv));
    v = max(v_min,min(v_max,V1(i-1,index_CFM) - a*dt)); 
    u = v*dt - a*dt^2/2;
    
        
     for n =  1: N_cfm-1
        if  U1(i-1,  index_CFM(n)) + u(n)  <  U1(i-1,  (index_CFM(n)-1))  - desired_space
            U1(i,  index_CFM(n)) =   U1(i-1,  (index_CFM(n)-1))  - desired_space;
            v(n) =  v_max;
        else
            U1(i,index_CFM(n)) = U1(i-1,index_CFM(n)) + u(n);
        end
     end
 
     V1(i, index_CFM) = v;

   
    d2u =  U1(i-1, index_BCM-1) + U1(i-1,index_BCM +1) - 2*U1(i-1, index_BCM);
    d2u =  k_d * d2u;
    
    d2v =  V1(i-1, index_BCM-1) + V1(i-1, index_BCM+1) - 2*V1(i-1, index_BCM);
    d2v =  k_v * d2v;
    
    a = d2u+d2v;
    a = a*tau;
    a = max(a_min,min(a_max,a));
    
    v = max(v_min,min(v_max,V1(i-1,index_BCM) + a*dt)); 
    u = v*dt + a*dt^2/2;
    
    for n = 1:N_bcm
        if  U1(i-1, index_BCM(n)) + u(n)  <  U1(i-1, index_BCM(n)-1)  - desired_space
            U1(i, index_BCM(n)) =   U1(i-1, index_BCM(n) -1)  - desired_space;
            v(n) =  v_max;
        else
            U1(i,index_BCM(n)) = U1(i-1, index_BCM(n)) + u(n);
        end
    end
    
    

    V1(i,index_BCM) = v;
    
    u1 = U_pos(i-1,:) - v_avg*dt;
    
    for m =  1: N_car
        
        if u1(m)<0
            u1(m) = u1(m) + track_length;
        elseif u1(m) > track_length
            u1(m) = u1(m) - track_length;
        end
            
    end
    
    U_pos(i,:) = u1;
    
    u = U(i,:) + U_pos(i,:) ; 
    u1 = U1(i,:) + U_pos(i,:) ;
    
    for m =  1: N_car
        
        if u(m)<0
            u(m) = u(m) + track_length;
        elseif u(m) > track_length
            u(m) = u(m) - track_length;
        end
        
        if u1(m)<0
            u1(m) = u1(m) + track_length;
        elseif u1(m) > track_length
            u1(m) = u1(m) - track_length;
        end
            
    end
   

set(gcf, 'position',[60,150,1800,400])
subplot('position',[0.03, 0.65, 0.94, 0.2])
plot(u, Y,'sk', 'MarkerSize',12,'MarkerFaceColor','w', 'lineWidth',2)
hold on
plot(u(1), Y(1),'sk', 'MarkerSize',12,'MarkerFaceColor','k', 'lineWidth',2)
title(sprintf('Mixed traffic with BCM and CFM (time: %3.1f sec.).k_d = %1.1f, k_v = %1.1f, T = %1.1f.', (i-1)*dt, k_d, k_v, T_react))

xlim([0,U_shift(end)+delta_u]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[], 'FontSize', 16)
set(gca,'xtick', [0:4:N_car]*dx_hist, 'FontSize',20);
hold off


subplot('position',[0.03, 0.12, 0.94, 0.20])
plot(u1(index_BCM), Y(index_BCM),'sr', 'MarkerSize',12,'MarkerFaceColor','w','lineWidth',2)
hold on
plot(u1(index_CFM), Y(index_CFM),'sk', 'MarkerSize',12,'MarkerFaceColor','w', 'lineWidth',2)
plot(u1(1), Y(1),'sk', 'MarkerSize',12,'MarkerFaceColor','k', 'lineWidth',2)
title(sprintf('Mixed traffic with BCM and CFM (time: %3.1f sec.). k_d = %1.1f, k_v = %1.1f, T = %1.1f, tau = %1.1f.', (i-1)*dt, k_d, k_v, T_react,tau))

xlim([0,U_shift1(end)+delta_u]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[], 'FontSize', 16)
set(gca,'xtick', [0:4:N_car]*dx_hist, 'FontSize',20);
hold off

M(i) = getframe(gcf);
   
end



