function Traffic_Flow

% This will simulate TRAFFIC FLOW of n cars on a straight freeway with L
% lanes. Changing lanes is allowed. P is the position and speed vector for
% all n cars of the form P = [x1 y1 s1 k1; x2 y2 s2 k2; ... ; xn yn sn kn]
% [The extent of the freeway (length) is FrEx and k is a parameter for
% each car describing how different they are from the average driver in
% terms of top speed and frequency of lane-changing (driving style)

clf
clear

global n L dt dfmin avgS Frwy

n = 40;                         % number of cars
L = 4;                          % number of lanes
dt = 0.15;                      % time step
dfmin = 2;                      % minimum distance allowed
Frwy = dfmin*(n/L);             % length of freeway
avgS = 0.35;                    % average speed of cars



P = zeros(n,4);             % initialize P
P = initial_positions(P);
P = initial_k(P);
Pprev = P;                  % P a single time step dt ago

iter = 1;
t = 0;
while t >= 0
    
    
    dPS = PdotS(P);
    PSnew = P + dPS;        % update P w.r.t. speed and vertical changes dy
    
    cy = PSnew(1:n,2); cy = mod(cy,Frwy); PSnew(1:n,2) = cy; %vertical PBC 
    
    dPL = PdotL(PSnew,Pprev);
    Pnew = PSnew + dPL;     % update P w.r.t lane changes dx
    
    
    
    figure(1)
    % plot current car arrangement with vertical PBC
    xx = Pnew(1:n,1); yy = Pnew(1:n,2); yy = mod(yy,Frwy);
    subplot(1,3,1); plot(xx,yy,'bs','MarkerSize',8); 
        axis([0 L+1 0 Frwy/3]); title('1/3rd Frwy')
    subplot(1,3,2); plot(xx,yy,'bs','MarkerSize',8); 
        axis([0 L+1 Frwy/3 2*Frwy/3]); title('2/3rd Frwy')
    subplot(1,3,3); plot(xx,yy,'bs','MarkerSize',8); 
        axis([0 L+1 2*Frwy/3 Frwy]); title('3/3rd Frwy')

    drawnow
    
    % Prep for another iteration
    t = t + dt; iter = iter + 1;
  
    Pprev = P;
    P = Pnew;
    
end



function dPS = PdotS(P)

% This will find how P changes with respect to speed dS and movement along the
% highway dy

dPS = zeros(n,4);

for i = 1:n
    
    Si = P(i,3);
    
    dyi = Si * dt;
    
    dPS(i,2) = dyi;
    
    dSi = speed_change(i,P);
    
    dPS(i,3) = dSi;
    
end

end



function dSi = speed_change(i,P)

% This will determine the speed Si of the ith car Ci based on the speed of 
% the car in front of it and how their distance between has changed

ki = P(i,4);
Si = P(i,3);
smax = ki * avgS;
df = dfront(i,P);
sif = speedinfront(i,df,P);

% If the ith car Ci is within the minimum distance to the car in front, 
% then set the speed of Ci to the speed of the car in front or the max
% speed of Ci, whichever is less.
if df < dfmin/2
    Sinew = min(smax , 0.8 * sif);
    dSi = Sinew - Si;
    return
end

% Give two accelerations, a spring acceleration with equilibrium dfmin
% behind the car ahead and a regular acceleration which brings a lonely car
% to full speed smax in 10 seconds
if df < dfmin
    k = 3;
end
if df < 2*dfmin && df >= dfmin
    k = 2;
end
if df >= 2*dfmin
    k = 3;
end

springAccel = -(k * ki) * (dfmin - df);
regAccel = smax/10 * dt;

dSi = springAccel;
if dSi > regAccel
    dSi = regAccel;
end
if dSi < -2*regAccel
    dSi = -2*regAccel;
end

if Si + dSi > smax
    dSi = smax - Si;
end

if Si + dSi < 0
    dSi = -Si + smax/2;
end
    
end




function sif = speedinfront(i,df,P)

% This finds the speed of the car in front of ith car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

sif = inf; % For cars in the front of the pack

% Locate car in front
for j = 1:n
    if j ~= i
        if P(j,1) == cx
            if P(j,2) == cy + df
                sif = P(j,3);    % Speed of car in front
            end
        end
    end
end

end




function [sifL sifR] = speedinfrontLR(i,P)

% This finds speed of cars in front and to left and right of ith car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

[dfL dfR] = dfrontLR(i,P);

% If ith car Ci is at either edge of the freeway and one of the below
% 'if' loops to find sifL or sifR isn't enacted
sifL = 0;
sifR = 0;

for j = 1:n
    
    % Locate car in front LEFT
    if P(j,1) == cx - 1
        if P(j,2) == cy + dfL
            sifL = P(j,3); % Speed of car in front LEFT
        end
    end
    
    % Locate car in front RIGHT
    if P(j,1) == cx + 1
        if P(j,2) == cy + dfR
            sifR = P(j,3); % Speed of car in front RIGHT
        end
    end
    
end

end





function [sibL sibR] = speedinbackLR(i,P)

% This finds speed of cars in front and to left and right of ith car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

[dbL dbR] = dbackLR(i,P);

% If ith car Ci is at either edge of the freeway and one of the below
% 'if' loops to find sibL or sibR isn't enacted
sibL = 0;
sibR = 0;

for j = 1:n
    
    if j ~= i
    
        % Speed of car in front LEFT
        if P(j,1) == cx - 1
            if P(j,2) == cy - dbL
                sibL = P(j,3);
            end
        end
    
        % Speed of car in front RIGHT
        if P(j,1) == cx + 1
            if P(j,2) == cy - dbR
                sibR = P(j,3);
            end
        end
        
    end

end

end





function db = dback(i,P)

% This finds the closest car in back of car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

close = [];

for j = 1:n
    if j ~= i
        if P(j,1) == cx
            if P(j,2) <= cy
                close = [P(j,2) close];
            end
        end
    end
end

if isempty(close) == 1  % if there are no cars in back of Ci make
    db = 1e6;           % dback very large
    return
end

db = cy - min(close);

end






function df = dfront(i,P)

% This finds the closest car in front of ith car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

close = [];

for j = 1:n
    if j ~= i
        if P(j,1) == cx
            if P(j,2) >= cy
                close = [P(j,2) close];
            end
        end
    end
end

if isempty(close) == 1  % if there are no cars in front of Ci make
    df = 1e6;           % dfront very large
    return
end

df = min(close) - cy;

end




function [dbL dbR] = dbackLR(i,P)

% This finds the closest cars which are behind the car Ci, in the lanes 
% left and right to the car Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

closeleft = [];
closeright = [];

% Distance from Ci to car to left and back

if cx == 1       %in case Ci is at the edge of the freeway   
    dbL = 0;
end

if cx ~= 1
    for j = 1:n
        if j ~= i
            if P(j,1) == cx - 1       %car to the left and back
                if P(j,2) <= cy
                    closeleft = [P(j,2) closeleft];
                end
            end
        end
    end

    dbL = cy - min(closeleft);

    if isempty(closeleft) == 1 % if there are no cars in back left of Ci 
        dbL = 1e6;             % make dbleft very large
    end
    
end

% Distance from Ci to car to right and back

if cx == L       %in case Ci is at the edge of the freeway
    dbR = 0;
end

if cx ~= L
    for j = 1:n
        if j ~= i
            if P(j,1) == cx + 1      %car to the right and back
                if P(j,2) <= cy
                    closeright = [P(j,2) closeright];
                end
            end
        end
    end

    dbR = cy - min(closeright);

    if isempty(closeright) == 1 % if there are no cars in back right of Ci
        dbR = 1e6;              % make dbright very large
    end
    
end

end
        




function [dfL dfR] = dfrontLR(i,P)

% Now find distance between car Ci and cars to the right and left and in 
% front of Ci

Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);

closeleft = [];
closeright = [];

% Distance from Ci to car to left and front

if cx == 1       %in case Ci is at the edge of the freeway   
    dfL = 0;
end

if cx ~= 1
    for j = 1:n
        if j ~= i
            if P(j,1) == cx - 1      %car to the left and front
                if P(j,2) >= cy
                    closeleft = [P(j,2) closeleft];
                end
            end
        end
    end

    dfL = min(closeleft) - cy;
    
    if isempty(closeleft) == 1 % if there are no cars in front left of Ci 
        dfL = 1e6;             % make dfL very large
    end

end

% Distance from Ci to car to right and front

if cx == L       %in case Ci is at the edge of the freeway   
    dfR = 0;
end

if cx ~= L
    for j = 1:n
        if j ~= i
            if P(j,1) == cx + 1      %car to the right and front
                if P(j,2) >= cy
                    closeright = [P(j,2) closeright];
                end
            end
        end
    end

    dfR = min(closeright) - cy;
    
    if isempty(closeright) == 1 % if there are no cars in front right of Ci
        dfR = 1e6;              % make dfR very large
    end
    
end

end





function dPL = PdotL(P,Pprev)

% This will find how P changes with respect to lane changes dx. We follow 
% the convention that the speed dS and position dy adjustments have been 
% made already

dPL = zeros(n,4);

for i = 1:n
    
    Si = P(i,3);
  
    dxi = lane_change(i,Si,P,Pprev);
    
    dPL(i,1) = dxi;
    
end

end





function dxi = lane_change(i,Si,P,Pprev)

% This will determine whether the ith car Ci will change lanes based on
% the speed of cars around and their distances from Ci

dxi = 0; % Default is not to change lanes

ki = P(i,4);        % Recall ki is from 0.50 to 1.50 and determines how
                    % quick a driver is
                    
smax = ki * avgS;   % Desired speed of ith car


% If the left and right cars in back are/were too close, Ci shouldn't 
% change lanes
[dbL dbR] = dbackLR(i,P);
[dbLp dbRp] = dbackLR(i,Pprev);
if dbL < dfmin && dbR < dfmin || dbLp < dfmin && dbRp < dfmin
    return
end 

% If the left and right cars in front are/were too close, Ci shouldn't 
% change lanes
[dfL dfR] = dfrontLR(i,P);
[dfLp dfRp] = dfrontLR(i,Pprev);
if dfL < dfmin && dfR < dfmin || dfLp < dfmin && dfRp < dfmin
    return
end 

% If the speed of the car in front is/was faster than the right and left 
% cars in front, Ci shouldn't change lanes
df = dfront(i,P);
sif = speedinfront(i,df,P);
[sifL sifR] = speedinfrontLR(i,P);
f = [sifL sifR sif];
speedF = sort(f);
if sif == speedF(3);
    return
end

% If the left and right cars in back are going too fast, Ci shouldn't 
% change lanes
[sibL sibR] = speedinbackLR(i,P);
if 1.2 * Si < sibL && 1.2 * Si < sibR 
    return
end 

% For the case where the above isn't the case and Ci wants to change lanes
% because it is getting close to the car in front, ie it will reach the car
% in front within 10 seconds, or because it is being tailgated
db = dback(i,P);
sifon = inf;              % anything > smax
if df < dfmin
    sifon = sif;          % declare speed of 'nearby' cars in front
end
if sifon < smax  ||  df < dfmin  ||  db < dfmin/2 
    r = rand();
    if r < 0.95                 % preference to not change lanes, notice
        return                  % higher ki means more lane changes
    end
    
    Ldxi = 0;
    if sibL <= 1.2 * Si
        Ldxi = -1;              % lane change to the left
    end
    if dbL < dfmin || dfL < dfmin || dbLp < dfmin || dfLp < dfmin
        Ldxi = 0;               % in case there's no room, cancel it
    end
    
    Rdxi = 0;
    if sibR <= 1.2 * Si
        Rdxi = +1;              % lane change to the right
    end
    if dbR < dfmin || dfR < dfmin || dbRp < dfmin || dfRp < dfmin
        Rdxi = 0;               % in case there's no room, cancel it
    end
    
    if sifL > sifR              % decide which lane change is better
        dxi = Ldxi;
    end
    if sifR > sifL
        dxi = Rdxi;
    end
end

end





function P = initial_k(P)

% This fills in ki (driving style) of ith car Ci with ki in interval 
% [0.5,1.5]. Most cars will have the average 1, less will have 0.50 and 
% 1.50, describing more timid and more aggressive drivers. About 1/2 of 
% cars have k ~ 1 , 1/4 have k < 1 and 1/4 have k > 1 Basically, k will 
% determine the top speed and frequency of lane-changing for a car

% Set up distribution

kS = linspace(-(0.25)^(1/5),+(0.25)^(1/5),n);
ki = 1 + (kS).^5;

r = randperm(n); 

for i = 1:n
    
    j = r(i);               %randomly distribute k's among n cars
    P(i,4) = ki(j);
    
end

end





function P = initial_positions(P)

% This will place the n cars in the freeway with L lanes in checkerboard
% fashion

j = 1;
for i = 1:2:n-1
    rx = j;
    ry = dfmin * ceil(i/L);
    P(i,1) = rx;
    P(i,2) = ry;
    P(i+1,1) = rx + 1;
    P(i+1,2) = ry + dfmin/2;
    j = 1 + mod (j + 1 , L);
end

end

end