%Newell Model
dt=1/10; %time increment
L=20;%length of car
N=200; %number of cars
vo=50; %max speed
L1=L; %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=5; %acceleration constant
x=-2*N*L1:L1:N*L1; %initial positions
M=max(size(x));
a1=0;
a2=0;
w=20;  %width of intersection
s=diff(x);
lastinitial=M+1;  %N+1;

TG = 25; %green
TY =  5; %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=50;  %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+L+w), sum(x>=ll2+L+w));

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',[l1+W, l1+W],[0 280],'g',[l2 l2],[0 280],'g',[l2+W, l2+W],[0 280],'g')
   axis([0,2.5,0,vo])
   title(ff1)

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

  axis([0,2.5,0,rmax])
  title(ff)
  pause(0);
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);

               while (xlt1+min(vlt1)*TY >=ll1+L+w) &lt1>=1
                  if lt1>=2
                   lt1 = lt1-1;
                   vlt1 =[vlt1, v(lt1)];
                   xlt1 = x(lt1);
                  elseif lt1==1
                   lt1=lt1-1;
                  else
                  end
              end
         else
         end
         if last2>=1
           xlt2 = x(lt2);
           vlt2 = v(lt2);

                while (xlt2+min(vlt2)*TY >=ll2+L+w) & lt2>=1
                    if lt2>=2
                      lt2 = lt2-1;
                      vlt2 = [vlt2,v(lt2)];
                      xlt2 = x(lt2);
                    elseif lt2==1
                      lt2 = lt2-1;
                    else
                    end
                end
        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],'y',[l1+W, l1+W],[0 280],'y',[l2 l2],[0 280],'y',[l2+W, l2+W],[0 280],'y')
   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 280],'y',[l1+W, l1+W],[0 280],'y',[l2 l2],[0 280],'y',[l2+W, l2+W],[0 280],'y')
  axis([0,2.5,0,rmax])
  title(ff)
if stp2>=1
  pause(.5);
else
pause(0);
end

%%%%%%%%%%%%%%%%%%%%%%   Start of yellow red loop---time is m*(TG+TY+TR)+TG.    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 for k=1:floor(TY/dt)
       color=2;
       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]);
            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);
            wstp1=min([wwstp1,vstp1]);
            xstp1=xstp1+dt*wstp1;

            vstp1=vstp1+dt*A1;
            xstp1=xstp1*(xstp1<ll1)+ll1*(xstp1>=ll1);
          end
       end
   else
      vv1=vstp1;
      xx1=xstp1;
      AA1=A1;
      tp1=t;
      xstp1=xstp1+vstp1*dt;
      vstp1=vstp1+A1*dt;
      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*(ty+TR-tp2)<=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]);

                 xstp2=xstp2+dt*wstp2;
                 vstp2=vstp2+dt*A2;
                 xstp2=xstp2*(xstp2<ll2)+ll2*(xstp2>=ll2);
               else
                 astp2=2*(xx2-ll2+vv2*(ty+TR-tp2))/((ty+TR-tp2)^2);
                 wwstp2=vv2-astp2*(t-tp2);
                 wstp2=min([wwstp2,vstp2]);
                 xstp2=xstp2+dt*wstp2;

                 vstp2=vstp2+dt*A2;
                 xstp2=xstp2*(xstp2<ll2)+ll2*(xstp2>=ll2);
               end
           end
       else
          vv2=vstp2;
          xx2=xstp2;
          AA2=A2;
          tp2=t;
          xstp2=xstp2+vstp2*dt;
          vstp2=vstp2+A2*dt;
          wstp2=vstp2;
       end
else
end

       x=x+v*dt;
         if stp1>=1
          x(stp1)=xstp1;
         else  
         end
    if stp2<=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
        xstp2=x(1);
        wstp2=v1(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+L+w), sum(x>=ll2+L+w));

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],'y',[l1+W, l1+W],[0 280],'y',[l2 l2],[0 280],'y',[l2+W, l2+W],[0 280],'y')
   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 280],'y',[l1+W, l1+W],[0 280],'y',[l2 l2],[0 280],'y',[l2+W, l2+W],[0 280],'y')


  axis([0,2.5,0,rmax])
  title(ff)
if stp2>=1
  pause(.5);
else
pause(0);
end
 else
 end
end 
%ty=t;

%%%%%% 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]);
            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);
             wstp1=min([wwstp1,vstp1]);
             xstp1=xstp1+dt*wstp1;

             vstp1=vstp1+dt*A1;
             xstp1=xstp1*(xstp1<ll1)+ll1*(xstp1>=ll1);
          end
       end
   else
      vv1=vstp1;
      xx1=xstp1;
      AA1=A1;
      tp1=t;
      xstp1=xstp1+vstp1*dt;
      vstp1=vstp1+A1*dt;
      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]);
                 xstp2=xstp2+dt*wstp2;
                 vstp2=vstp2+dt*A2;
                 xstp2=xstp2*(xstp2<ll2)+ll2*(xstp2>=ll2);
               else
                  astp2=2*(xx2-ll2+vv2*(ty+TR-tp2))/((ty+TR-tp2)^2);
                  wwstp2=vv2-astp2*(t-tp2);
                  wstp2=min([wwstp2,vstp2]);
                  xstp2=xstp2+dt*wstp2;
                  vstp2=vstp2+dt*A2;
                  xstp2=xstp2*(xstp2<ll2)+ll2*(xstp2>=ll2);
               end
           end
       else
          vv2=vstp2;
          xx2=xstp2;
          AA2=A2;
          tp2=t;
          xstp2=xstp2+vstp2*dt;
          vstp2=vstp2+A2*dt;
          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+L+w), sum(x>=ll2+L+w));

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',[l1+W, l1+W],[0 280],'r',[l2 l2],[0 280],'r',[l2+W, l2+W],[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 280],'r',[l1+W, l1+W],[0 280],'r',[l2 l2],[0 280],'r',[l2+W, l2+W],[0 280],'r')


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


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