%Newell Model
dt=1/10; %time increment
L=20; %length of car
N=200; %number of cars
vo=50; %max speed
L1=25; %initial spacing
r1=1/L1; %initial density
rmax=5280/L+10; %max density
color=1; %1 is green 2 is yellow 3 is red
lastcolor=1;
relax=.05; %acceleration constant
x=-2*N*L1:L1:N*L1; %initial positions
M=max(size(x));
a1=0;
a2=0;
w=0;  %width of intersection
s=diff(x);
lastinitial=M+1;

TG =30; %green
TY =0; % yellow
TR =30; %red

last=lastinitial; %last car through light
last2=lastinitial; %last car through 2nd light

s=diff(x); %intial differences
u=[vo*(1-L./s),vo];
v=0*u;
t=0;
%W=(L+w)/5280;
l1=1; %location od first light
ll1=5280*l1;%(N+1)*L1;
l2=2; %location of 2nd light
ll2=l2*5280;
step=10;
a=v-u;
e1=exp(-dt/relax);
t=0;
P=7; %number of periods
g=5;
for m=1:P    %%/dt
  for k=1:floor(TG/dt)
   last=sum((x<ll1)); 
   last2=sum((x<ll2));
   color=1;
   x=x+v*dt;
   s=diff(x);
   r=1./s;
   u=[vo*(1-L./s),vo];
   a=a*e1;
   v1=u+a;
   ww=(v1-v)/dt;
   v=v1;
   a=v-u;
   t=t+dt; 
   j=k-step*floor(k/step);
if j==0

ff1=sprintf('Velocity (in miles/hour) vs Position (in miles), time (in seconds)   %3.0f\ncars past light1   %d\tcars past light2   %d', t, sum(x>=ll1), sum(x>=ll2));
ff=sprintf('Auto Density (in cars/mile) vs Position (in miles)\nmin deceleration (in feet/sec/sec) and max accelerations (in feet/sec/sec)   %0.2f   %0.2f\nminimum car spacing (in feet)   %0.2f', floor(min(ww)), floor(max(ww))+1, min(s));

subplot(211)
plot(x/5280, 3600*v/5280,'k.',[l1 l1],[0 280],'g',[l2 l2],[0 280],'g')
   axis([0,2.5,0,vo])
   title(ff1)

subplot(212)
  plot(x/5280, [r,0]*5280, 'k .',[l1 l1],[0 400],'g',[l2 l2],[0 400],'g')

  axis([0,2.5,0,rmax])
  title(ff)
  pause;
else
end
end

tg=t;
ty=tg+TY;
tp1=tg;
tp2=tg;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        last=sum((x<ll1));
        last2=sum((x<ll2));
        lt1 = last;
        lt2 = last2;
         if last>=1
           xlt1 = x(lt1);
           vlt1 = v(lt1);

         else
         end
         if last2>=1
           xlt2 = x(lt2);
           vlt2 = v(lt2);

        else
        end
        stp1 = lt1;
        stp2 = lt2;
       if stp1>=1
          xstp1=x(stp1);
          vstp1=v(stp1);
       else
          xstp1=x(1);
          vstp1=v(1); 
       end
       if stp2>=1
          xstp2=x(stp2);
          vstp2=v(stp2);
       else
	  xstp2=x(1);
          vstp2=v(1);

       end
      vv1=vstp1;
      xx1=xstp1;
      vv2=vstp2;
      xx2=xstp2;
if stp1==M
      rstp1=0;
elseif stp1>=1
     rstp1=r(stp1);
else
     rstp1=r(1);
end
if stp2==M
    rstp2=0;
elseif stp2>=1
    rstp2=r(stp2);
else
   rstp2=r(1);
end
[stp1 stp2]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(211)
plot(xstp1/5280,3600*vstp1/5280,'r o',xstp2/5280,3600*vstp2/5280,'r o',x/5280, 3600*v/5280,'k.',[l1 l1],[0 280],'r',[l2 l2],[0 280],'r')
   axis([0,2.5,0,vo])
   title(ff1)

subplot(212)
  plot(xstp1/5280,5280*rstp1,'r o',xstp2/5280,5280*rstp2,'r o',x/5280, [r,0]*5280, 'k .',[l1 l1],[0 400],'r',[l2 l2],[0 400],'r')
  axis([0,2.5,0,rmax])
  title(ff)
  pause;


%%%%%% Start of red cycle--time is (m-1)*(TG+TY+TR)+TG+TY --stp1 and stp2 are fixed  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:floor(TR/dt)
  color=3;
if stp1==M
  A1=(vo-vstp1)/relax;
else
end
  if stp1<=M-1&stp1>=1
         A1=(vo*L/((x(stp1+1)-xstp1)^2))*(v(stp1+1)-vstp1)+(vo*(1-L/(x(stp1+1)-xstp1))-vstp1)/relax;
   else
   end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if stp1>=1
   if xstp1>=ll1-g*L
       if xstp1+vstp1*(ty+TR-tp1)<=ll1
         xstp1=xstp1+vstp1*dt;
         vstp1=vstp1+dt*min([0, A1]);
       else
          if xstp1+vstp1*(ty+TR-tp1)/2>ll1
            wstp1=min([vv1*((ll1-xstp1)^.5)/((ll1-xx1)^.5),vstp1]);
            wstp1=wstp1*(xstp1<ll1)*(wstp1>0);
            xstp1=xstp1+dt*wstp1;
            vstp1=vstp1+dt*A1;
            xstp1=xstp1*(xstp1<=ll1)+ll1*(xstp1>ll1);
          else
             astp1=2*(xx1-ll1+vv1*(ty+TR-tp1))/((ty+TR-tp1)^2);
             wwstp1=vv1-astp1*(t-tp1);
             wwstp1=wwstp1*(wwstp1>0);
             wstp1=min([wwstp1,vstp1]);
             wstp1=wstp1*(wstp1>0);
             xstp1=xstp1+dt*wstp1;
             vstp1=vstp1+dt*A1;
             xstp1=xstp1*(xstp1<=ll1)+ll1*(xstp1>ll1);
          end
       end
   else
      xx1=xstp1;    %*(xstp1<ll1)+ll1*(xstp1>=ll1);
      vv1=vstp1;    %*(xstp1<ll1);

      AA1=A1;
      tp1=t;
      xstp1=xstp1+vstp1*dt;
      xstp1=xstp1;  %*(xstp1<ll1)+ll1*(xstp1>=ll1);
      vstp1=vstp1+A1*dt;
%      vstp1=vstp1*(xstp1<ll1);
      wstp1=vstp1; 
   end
else
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if stp2==M
  A2=(vo-vstp2)/relax;
else
end


   if stp2<=M-1&stp2>=1
      A2=(vo*L/((x(stp2+1)-xstp2)^2))*(v(stp2+1)-vstp2)+(vo*(1-L/(x(stp2+1)-xstp2))-vstp2)/relax;
   else
   end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if stp1==M
   xstp2=xstp1;
   wstp2=wstp1;
elseif stp1<=M-1&stp2>=1
        if xstp2>=ll2-g*L
            if xstp2+vstp2*(tg+TY+TR-t)<=ll2
               xstp2=xstp2+vstp2*dt;
               vstp2=vstp2+dt*min([0, A2]);
            else
               if xstp2+vstp2*(ty+TR-tp2)/2>ll2
                 wstp2=min([vv2*((ll2-xstp2)^.5)/((ll2-xx2)^.5),vstp2]);
                 wstp2=wstp2*(xstp2<ll2)*(wstp2>0);
                 xstp2=xstp2+dt*wstp2;
                 xstp2=xstp2*(xstp2<=ll2)+ll2*(xstp2>ll2);
                 vstp2=vstp2+dt*A2;
               else
                  astp2=2*(xx2-ll2+vv2*(ty+TR-tp2))/((ty+TR-tp2)^2);
                  wwstp2=vv2-astp2*(t-tp2);
                  wwstp2=wwstp2*(wwstp2>0);
                  wstp2=min([wwstp2,vstp2]);
%                  wstp2=wstp2*(wstp2>0);

                  xstp2=xstp2+dt*wstp2;
                  vstp2=vstp2+dt*A2;
                  xstp2=xstp2*(xstp2<=ll2)+ll2*(xstp2>ll2);
               end
           end
       else
          vv2=vstp2;      %*(xstp2<ll2);
          xx2=xstp2;      %*(xstp2<ll2)+ll2*(xstp2>=ll2);
          AA2=A2;
          tp2=t;
          xstp2=xstp2+vstp2*dt;
          xstp2=xstp2*(xstp2<ll2)+ll2*(xstp2>=ll2);
          vstp2=vstp2+A2*dt;
          vstp2=vstp2;     %*(xstp2<ll2);
          wstp2=vstp2;
       end
else
end
%%%%%%%%%%%%%%%%%%%%%
   
   x=x+v*dt;
    if stp1>=1
      x(stp1)=xstp1;
    else
    end
    if stp1<=M-1 & stp2>=1
      x(stp2)=xstp2;
    else
    end
   s=diff(x);
   r=1./s;

if stp1==M
   rstp1=0;
else
end
    if stp1<=M-1
       if stp1>=2
        r(stp1)=r(stp1-1);
        rstp1=r(stp1);
       else
	rstp1=r(1);
       end
    else
    end

if stp2==M
       rstp2=0;
else
end
    if stp2<=M-1
      if stp2 >=2
        r(stp2)=r(stp2-1);
        rstp2=r(stp2);
      else
        rstp2=r(1);
      end
    else
    end
  
   u=[vo*(1-L./s),vo];
   a=a*e1;
   v1=u+a;
   if stp1>=1
     v1(stp1)=wstp1;
   else
     xstp1=x(1);
     wstp1=v1(1);
   end
   if stp1<=M-1
      if stp2>=1
        v1(stp2)=wstp2;
      else
       wstp2=v1(1);
       xstp2=x(1);
      end
   else
   end
   ww=(v1-v)/dt;

   v=v1;
   a=v-u;
   t=t+dt;
   j=k-step*floor(k/step);
   if j==0
ff1=sprintf('Velocity (in miles/hour) vs Position (in miles), time (in seconds)   %3.0f\ncars past light1   %d\tcars past light2   %d', t, sum(x>=ll1), sum(x>=ll2));
ff=sprintf('Auto Density (in cars/mile) vs Position (in miles)\nmin deceleration (in feet/sec/sec) and max accelerations (in feet/sec/sec)   %0.2f   %0.2f\nminimum car spacing (in feet)   %0.2f', floor(min(ww)), floor(max(ww))+1, min(s));

subplot(211)
plot(xstp1/5280,3600*wstp1/5280,'r o',xstp2/5280,3600*wstp2/5280,'r o',x/5280, 3600*v/5280,'k.',[l1 l1],[0 280],'r',[l2 l2],[0 280],'r')
   axis([0,2.5,0,vo])
   title(ff1)

subplot(212)
  plot(xstp1/5280,5280*rstp1,'r o',xstp2/5280,5280*rstp2,'r o',x/5280, [r,0]*5280, 'k .',[l1 l1],[0 400],'r',[l2 l2],[0 400],'r')


  axis([0,2.5,0,rmax])
  title(ff)
  pause;
 else
 end
end 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
