clear
clc
% close all


% N_car = 200;
% N_T = 2000;
% N_bar =200;
% 
% 
% k_d = 0.1;
% k_v = 0.1;
% 
% dt = 0.1;
% Time = [1:N_T]*dt;
% 
% v_avg = 25;
% car_length = 5;
% u_avg = 25;
% du = u_avg + car_length;
% T_react = 1;
% Total_length = N_car*du;
% dx_hist = Total_length/N_bar;
% 
% v_osci = 2;
% u_osci = 5;
% v_0 = 2*v_osci*rand(1,N_car)-v_osci;
% v_0 = v_0-mean(v_0);
% u_0 = 2*u_osci*rand(1,N_car)-u_osci;
% u_0 = u_0-mean(u_0);
% U=zeros(N_T, N_car );
% 
% U_hist =zeros(N_T, N_bar);
% x_value = [1:N_bar]*dx_hist-dx_hist/2;
% 
% U(1,:) = u_0;
% U_shift = (1:N_car)*du;
% u = mod(u_0+U_shift,Total_length);
% U_hist(1,:) = hist(u,x_value);
% 
% d = [diff(u_0) , u_0(1)-u_0(end)];
% b = find(d+u_avg<0);
% 
% rho = 1609.334./(d+du);
% 
% Y = zeros(1, N_car);
% 
% 
% a_min = -5;
% a_max = 5;
% v_min = -v_avg;
% v_max = 19.444;

load ex_multinode_BC_initial.mat

% load ex_multinode_BC_initial_80cars_cf_40_sec.mat

% load ex_multinode_BC_initial_200cars_cf_40_sec.mat

figure;
set(gcf, 'position',[160,150,1000,400])
subplot('position',[0.03, 0.84, 0.96, 0.08])
plot(u(1:2:end-1), Y(1:end/2),'sr', 'MarkerSize',5,'MarkerFaceColor','k')
hold on
plot(u(2:2:end), Y(1:end/2),'sk', 'MarkerSize',5,'MarkerFaceColor','r')
title('The initial condition for simulation (space / meter)')
xlim([-du,U_shift(end)+du]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[])
set(gca,'xtick', [0:4:N_bar]*dx_hist);
hold off

%plot(X_cf(i,stop_Index), Y(stop_Index),'sk', 'MarkerSize',10,'MarkerFaceColor','r')
subplot('position',[0.03, 0.05, 0.96, 0.68])
stem(u,rho,'*r')
xlim([-du,U_shift(end)+du]);
ylim([-10, 330]);
set(gca,'xtick', [0:4:N_bar]*dx_hist);
title('The density of traffic flow (number/mile) at time 0 (sec)')
M(1) = getframe(gcf);

v=v_0;


for i = 2:N_T/5
   
    
    a = max(a_min,min(a_max,k_d*(d-v*T_react)));
    v = max(v_min,min(v_max,v+a*dt));
    if ~isempty(b)
        v(b) = v(mod(b,N_car)+1);
    end
    
    U(i,:) = U(i-1,:) + v*dt + a*dt^2/2;
    U(i,:)=min(U(i,:),[U(i,2:end),U(i,1)]+u_avg);
    d = [diff(U(i,:)) , U(i,1)-U(i,end)];
    b = find(d+u_avg<0);
    rho = 1609.334./max(car_length,(d+du));
    u = U(i,:)+U_shift + (i-1)*dt*v_avg;
    u = mod(u,Total_length);
    U_hist(i,:) = hist(u,x_value);
    
subplot('position',[0.03, 0.84, 0.96, 0.08])
plot(u(1:2:end-1), Y(1:end/2),'sr', 'MarkerSize',5,'MarkerFaceColor','k')
hold on
plot(u(2:2:end), Y(1:end/2),'sk', 'MarkerSize',5,'MarkerFaceColor','r')
title('The simulation of car-following (space / meter)')
xlim([-du,U_shift(end)+du]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[])
set(gca,'xtick', [0:4:N_bar]*dx_hist);
hold off

%plot(X_cf(i,stop_Index), Y(stop_Index),'sk', 'MarkerSize',10,'MarkerFaceColor','r')
subplot('position',[0.03, 0.05, 0.96, 0.68])
stem(u,rho,'*r')
xlim([-du,U_shift(end)+du]);
ylim([-10, 330]);
set(gca,'xtick', [0:4:N_bar]*dx_hist);
title(sprintf('The density of traffic flow (car-following) at time %3.2f (sec)',i*dt))
    
    M(i) = getframe(gcf);
   
end



k =7;
N = 2*k+1;



% q0 = -pi^2/3;
% 
%     q = zeros(1,k);
%     N = 2*k+1;
%     for i= 1:k
%         q(i) =- 2*(-1)^(i)/(i^2);
%     end
%     lambda = (q0+2*sum(q))/N;
%     
%     g = zeros(1,N);
%     g(k+1)=q0 - lambda;
%     
%     for i = 1:k
%         g(k+1+i)=q(i)-lambda;
%         g(k+1-i)=q(i)-lambda;
%     end

% q0 = -pi^2/3-1/6/pi;
% 
%     q = zeros(1,k);
%     N = 2*k+1;
%     for i= 1:k
%         q(i) =(1+ cos(i)) /(pi*i^2)- 2*sin(i)/(pi*i^3) - 2*(-1)^(i)/(i^2);
%     end
%     lambda = (q0+2*sum(q))/N;
%     
%     g = zeros(1,N);
%     g(k+1)=q0 - lambda;
%     
%     for i = 1:k
%         g(k+1+i)=q(i)-lambda;
%         g(k+1-i)=q(i)-lambda;
%     end


q0 = -pi/2;

    q = zeros(1,k);
    N = 2*k+1;
    for i= 1:k
        q(i) =(1- (-1)^(i))/i^2/pi;
    end
    lambda = (q0+2*sum(q))/N;
    
    g = zeros(1,N);
    g(k+1)=q0 - lambda;
    
    for i = 1:k
        g(k+1+i)=q(i)-lambda;
        g(k+1-i)=q(i)-lambda;
    end

% fprintf('The number cars used is : %d. The coefficients are: ', N)
% A = ones(N,N);
% m= -k:k;
% 
% hold on
% for i = 2:N
%     A(i,:)= m.^(i-1);
% end
% f = inv(A);
% g = f(:,3)*2



%% 2k+1 pints BC control

d2u=zeros(size(u_0));
d2v = d2u;

for i = N_T/5+1:500
    
    for ii = 1:N_car
        du_temp = 0;
        dv_temp = 0;
        
        for iii = -k:k
            if ii+iii<1
                du_temp = du_temp + U(i-1, ii+iii+N_car)*g(iii+k+1);
                dv_temp = dv_temp + v(ii+iii+N_car)*g(iii+k+1);
            elseif ii+iii>N_car
                du_temp = du_temp + U(i-1, ii+iii-N_car)*g(iii+k+1);
                dv_temp = dv_temp + v(ii+iii-N_car)*g(iii+k+1);
            else
                du_temp = du_temp + U(i-1, ii+iii)*g(iii+k+1);
                dv_temp = dv_temp + v(ii+iii)*g(iii+k+1);
            end
        end
        
        d2u(ii)=k_d*du_temp;
        d2v(ii)=k_v*dv_temp;
        
    end
    
    
    a = max(a_min,min(a_max,d2u+d2v));
    v = max(v_min,min(v_max,v+a*dt));
    U(i,:) = U(i-1,:) + v*dt + a*dt^2/2;
    U(i,:)=min(U(i,:),[U(i,2:end),U(i,1)]+u_avg);
    d = [diff(U(i,:)) , U(i,1)-U(i,end)];
    rho = 1609.334./(d+du);
 %         rho = 3*(d+du);
    u = U(i,:)+U_shift + (i-1)*dt*v_avg;
    u = mod(u,Total_length);
    U_hist(i,:) = hist(u,x_value);
    
subplot('position',[0.03, 0.84, 0.96, 0.08])
plot(u(1:2:end-1), Y(1:end/2),'sr', 'MarkerSize',5,'MarkerFaceColor','k')
hold on
plot(u(2:2:end), Y(1:end/2),'sk', 'MarkerSize',5,'MarkerFaceColor','r')
title('The simulation of 15 point (LSA-7) bilateral control (space / meter)')
xlim([-du,U_shift(end)+du]);
ylim([-0.5, 0.5]);
set(gca,'ytick',[])
set(gca,'xtick', [0:4:N_bar]*dx_hist);
hold off

% plot(X_cf(i,stop_Index), Y(stop_Index),'sk', 'MarkerSize',10,'MarkerFaceColor','r')
subplot('position',[0.03, 0.05, 0.96, 0.68])
hold off
stem(u,rho,'*r')
xlim([-du,U_shift(end)+du]);
ylim([-10, 330]);
set(gca,'xtick', [0:4:N_bar]*dx_hist);
title(sprintf('The density of traffic flow (bilateral control) at time %3.2f (sec)',(i-N_T/5)*dt))
    
    M(i) = getframe(gcf);
   
end

%% 3 pints BC control

% for i = N_T/5+1:N_T/5*2
%     
%     d2u=k_d*( [diff(U(i-1,:)) , U(i-1,1)-U(i-1,end)]- [U(i-1,1)-U(i-1,end) ,diff(U(i-1,:))]);
%     d2v=k_v*( [diff(v) , v(1)-v(end)]- [v(1)-v(end) ,diff(v)]);
%     a = max(a_min,min(a_max,d2u+d2v));
%     v = max(v_min,min(v_max,v+a*dt));
%     U(i,:) = U(i-1,:) + v*dt + a*dt^2/2;
%     U(i,:)=min(U(i,:),[U(i,2:end),U(i,1)]+u_avg);
%     d = [diff(U(i,:)) , U(i,1)-U(i,end)];
%     rho = 1609.334./(d+du);
%     u = U(i,:)+U_shift + (i-1)*dt*v_avg;
%     u = mod(u,Total_length);
%     U_hist(i,:) = hist(u,x_value);
%     
% subplot('position',[0.03, 0.84, 0.96, 0.08])
% plot(u(1:2:end-1), Y(1:end/2),'sr', 'MarkerSize',5,'MarkerFaceColor','k')
% hold on
% plot(u(2:2:end), Y(1:end/2),'sk', 'MarkerSize',5,'MarkerFaceColor','r')
% title('The simulation of bilateral control (space / meter)')
% xlim([-du,U_shift(end)+du]);
% ylim([-0.5, 0.5]);
% set(gca,'ytick',[])
% set(gca,'xtick', [0:4:N_bar]*dx_hist);
% hold off
% 
% % plot(X_cf(i,stop_Index), Y(stop_Index),'sk', 'MarkerSize',10,'MarkerFaceColor','r')
% subplot('position',[0.03, 0.05, 0.96, 0.68])
% stem(u,rho,'*r')
% xlim([-du,U_shift(end)+du]);
% ylim([-10, 330]);
% set(gca,'xtick', [0:4:N_bar]*dx_hist);
% title(sprintf('The density of traffic flow (bilateral control) at time %3.2f (sec)',(i-N_T/5)*dt))
%     
%     M(i) = getframe(gcf);
%    
% end

% writerObj = VideoWriter('Three_point_Bilateral_control_demo');
% open(writerObj);
% vidObj.FrameRate=10;
% writeVideo(writerObj,M);
% close(writerObj);

% subplot('position',[0.03, 0.84, 0.96, 0.08])
% plot(u(1:2:end-1), Y(1:end/2),'sr', 'MarkerSize',5,'MarkerFaceColor','k')
% hold on
% plot(u(2:2:end), Y(1:end/2),'sk', 'MarkerSize',5,'MarkerFaceColor','r')
% title('The simulation of bilateral control (space / meter)')
% xlim([-du,U_shift(end)+du]);
% ylim([-0.5, 0.5]);
% set(gca,'ytick',[])
% set(gca,'xtick', [0:4:N_bar]*dx_hist);
% hold off
% 
% % plot(X_cf(i,stop_Index), Y(stop_Index),'sk', 'MarkerSize',10,'MarkerFaceColor','r')
% subplot('position',[0.03, 0.05, 0.96, 0.68])
% hold off
% stem(u,rho,'*r')
% xlim([-du,U_shift(end)+du]);
% ylim([-10, 330]);
% set(gca,'xtick', [0:4:N_bar]*dx_hist);
% title(sprintf('The density of traffic flow (bilateral control) at time %3.2f (sec)',(i-N_T/5)*dt))

% writegif('CF_followed_by_BCM(LSZ-7)_demo.gif',M,0.1);
% 
% V=Time'*v_avg;
% figure
% for i = 1:N_car
%     hold on
%     plot(mod(U(:,i)+V+i*du,Total_length), Time, '.','color', [0.6, 0.6, 0.6])
% end
% % axis([-du,(N_car+1)*du, 0, Time(end)])
% % % set(gca,'xtick',[])
% % set(gca,'xtick',[0:100:du*N_car])
% xlabel('Position (meters)')
% ylabel('Time (seconds)')
% title('the density of cars')
% print(gcf,'-depsc2','density_car_following_followed_by_bilateral_control.eps')
% % 
% % U_hist1 = zeros(N_T/5, N_bar);
% % for i = 1: N_T/5
% %     U_hist1(i,:) = sum(U_hist(5*i-4:5*i,:)) ;
% % end
% % figure
% % imagesc(U_hist1)
% % colormap gray
% % % axis equal
% % colorbar
% % axis tight
% % xlabel(sprintf('Position Interval Index (%d meters for each inteval)', dx_hist))
% % ylabel('Time Interval Index (1second for each inteval)')
% % title('the density of cars')
% % 
