% close all
% clear
% clc
% 
% kd = 0.1;
% 
% kv = 0.2;
% 
% dt = 0.2;
% 
% P = 50;
% 
% Q = 300;
% 
% ds =1;
% 
% 
% X0 = zeros(P,1);
% 
% V0 = zeros(P,1);
% 
% % X0(3) = 0.1;
% 
% V0(1) = -1/15;
% 
% % X0 = X0+(rand(P,1)-0.5)*0.1;
% % 
% % V0 = V0+0.1*(rand(P,1)-0.5);
% 
% S0 = zeros(2*P,1);
% S1=S0;
% S = zeros(2*P,Q);
% 
% 
% 
% for i =1:P;
%     S0(i*2-1) = X0(i);
%     S0(i*2) = V0(i);
% end
%     
% 
% 
% % 
% %  dX = [X0(end)-X0(1),diff(X0)];
% % 
% %  dV = [V0(end)-V0(1),diff(V0)];
% 
% M = [0, 1; -kd, -kv];
% 
% N = [0, 0; kd kv];
% 
% A1 = zeros(P);
% 
% A1(1,P)=1;
% 
% A1 = kron(A1,N);
% 
% A = kron(eye(P),M)+kron(diag(ones(1,P-1),-1),N) + A1;
% 
% A=sparse(A);
% 
% A1 = eye(P*2);
% A1 = sparse(A1);
% 
% 
% % h1=figure(1);
% % hold on
% % h2=figure(2);
% for i =1:Q
% 
% S1 = (A1+A*dt)*S0;
% % figure(h1);
% 
% % figure(h2);
% % for j =1:100
% %     
% %     plot([S0(2*j),i-1],[S1(2*j), i],'k*-');
% % end
% 
% S0(2:2:end)=max(S0(2:2:end),-1);
% 
% S(:,i) = S0;
% 
% 
% S0 = S1;
% 
% 
% end
% 
% 
% figure
% axis([-ds*2,ds*P+ds*3,-1,Q+1]);
% hold on
% 
% Y = 1:Q;
%  for j =1:P
%     plot(S(2*j-1,:)+ds*j,Y,'r.-');
% %     axis([-ds*3,ds*P+ds*3,-1,Q+1]);hold on
%  end
%  title('The relative displacement of the cars in the car-following model system');
% xlabel('The index number of the car');
% ylabel('The iteration number ( \delta t =0.5)');
% 
%  figure
% axis([-ds*2,ds*P+ds*3,-1,Q+1]);
% hold on
% for j =1:P
%     plot(S(2*j,:)+ds*j,Y,'k.-');
% %     axis([-ds*3,ds*P+ds*3,-1,Q+1]);hold on
% end
%  
% title('The relative velocity of the cars in the car-following model system');
% xlabel('The index number of the car');
% ylabel('The iteration number ( \delta t =0.5)');



close all
clear
clc

kd = 0.1;

kv = 0.2;

dt = 0.1;

kd = kd*dt;

kv = kv*dt;

P = 50;

Q = 300;



M = [0, 1; -kd, -kv];

N = [0, 0; kd kv];

Y =zeros(P,Q);

V= zeros(P,Q);

t = 1:Q;

t = dt*t;

v_start = 25;

ds = 25;

a = 2;

Y0 = zeros(1,Q);
% V0 = (rand(size(Y0))-0.5)*2;
V0=Y0;

Y(:,1)=2.4*rand(size(V(:,1)))-1.2;

Y1 = zeros(P,1);
V1 = Y1;
% for i = 1:Q
%     v = a*t(i);
%     if v<=v_start
%         V0(i)=v;
%     else
%         V0(i)=v_start;
%     end
%     if i>1
%         Y0(i)=Y0(i-1)+V0(i)*dt;
%     end
% end
% 
% 
% for i = 1: Q
%    
%     v = V1(1) + kd*(Y0(i)-Y1(1))+kv*(V0(i)-V1(1));
%     if v<v_start
%      V1(1) =v;
%     else
%      V1(1) =v_start;
%     end
%     Y1(1) = Y1(1) + dt*V1(1);
%     for j = 2:P
%            
%             v = V1(j) + kd*(Y1(j-1)-Y1(j))+kv*(V1(j-1)-V1(j)); 
%      if v<v_start
%      V1(j) =v;
%     else
%      V1(j) =v_start;
%     end
%             Y1(j) = Y1(j) + dt*V1(j);
%     end
%     V(:,i)=V1;
%     Y(:,i)=Y1;
% end

% for i=2:Q
%         Y0(i)=Y0(i-1)+V0(i)*dt;
% end


for i = 2: Q
    V(1,i) = V(1,i-1)+kd*(V0(i)-V(1,i-1))+kd*(Y0(i)-Y(1,i-1));
    Y(1,i) = Y(1,i-1)+V(1,i-1)*dt;
    for j = 2:P
        V(j,i) = V(j,i-1) + kd*(V(j-1,i-1)-V(j,i-1))+kv*(Y(j-1,i-1)-Y(j,i-1));
        Y(j,i) = Y(j, i-1) + V(j,i-1)*dt;
    end
end


plot(Y0,t)

hold on
for k = 1:P
    plot(k*30+Y(k,:),t,'r')
end
xlabel('car index: from 0 to 50')
ylabel('time (second)')


figure
plot(V0,t)

hold on
for k = 1:P
    plot(k*30+V(k,:),t,'k')
end

xlabel('car index: from 0 to 50')
ylabel('time (second)')

