% Algorithm for ONE FULL CYCLE AND ONE PARTIAL CYCLE
%
% From the paper:
% Solving for Equilibrium in the Basic Bathtub Model
% Arnott and Buli (2017)
%
% See README file at http://math.ucr.edu/~buli/Bathtub_Code/Bathtub_Code.html
%



close all;
clear all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Choice of Utility Function

tic

util_choice = 1;			% Choose 0 for exponential utility function; 1 for logarithmic utility function
cong_hyp = 1;				% Choose 0 for congested region; 1 for hypercongested region

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parameters

if util_choice == 0

A0 = 5;					% Exponential Utility Function Paramters
b1 = 1/2;
B0 = 10;
a1 = 1/3;
H = (A0*b1)/(a1*B0);

elseif util_choice == 1

a0 = 15;				% Logarithmic Utility Function Parameters
a1 = .5;
b0 = 18;

else

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Other Parameters

k = 6;                                  % Latest arrival time t = 6 => 11 AM 
L = 4;                                  % Distance traveled by cars

vf = 15;                                % free flow velocity
kj = 10^6;                              % jam density

fftt = L/vf;                            % Free Flow Travel Time

tzeros = 5;                             % Break point count 
t = zeros(1,tzeros);
v = zeros(1,tzeros);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Maximizing utility with respect to start time

tttt = linspace(0.1,6,8000);

if util_choice == 0

u_max = (A0/a1)*(1 - exp(-a1*tttt)) + (B0/b1)*(1 - exp( b1*(tttt + fftt - k)));
u_max_prime = A0*exp(-a1*tttt) - B0*exp(b1*(tttt+fftt-k));

elseif util_choice == 1

u_max = a0*log(a1*tttt) + b0*log(-(tttt + fftt)+k);
u_max_prime = a0*(1./tttt) - b0*(1./(-(tttt + fftt)+k));

else

end

% t chosen to maximize utility at free-flow velocity

temp_u = find(u_max_prime(1:end-1).*u_max_prime(2:end)<0);
closestValueL_u = u_max_prime(temp_u);
closestValueR_u = u_max_prime(temp_u+1);

slope_int_u = (closestValueR_u - closestValueL_u)/(tttt(temp_u+1) - tttt(temp_u) );
yint_u = closestValueL_u - slope_int_u*tttt(temp_u);

t_max_u = (-yint_u)/slope_int_u;				% time of maximum utility


if util_choice == 0

u_max_star = (A0/a1)*(1 - exp(-a1*t_max_u)) + (B0/b1)*(1 - exp( b1*(t_max_u + fftt - k)));

elseif util_choice == 1

u_max_star = a0*log(a1*t_max_u) + b0*log(-(t_max_u + fftt)+k);	% maximum utility

else

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Mesh grid for time

if util_choice == 0
    Nt = 30000; 
elseif util_choice == 1
	Nt = 400000;	
else

end

% Preallocation 

dens0 = zeros(1,Nt+1);
dens1 = zeros(1,Nt+1);
dens2 = zeros(1,Nt+1);

vel0 = zeros(1,Nt+1);
vel1 = zeros(1,Nt+1);
vel2 = zeros(1,Nt+1); 

ent0 = zeros(1,Nt+1);
ent1 = zeros(1,Nt+1);

exit1 = zeros(1,Nt+1);
exit2 = zeros(1,Nt+1);

t_mesh0 = zeros(1,Nt+1);
T_mesh0 = zeros(1,Nt+1);
Tp_mesh0 = zeros(1,Nt+1);
Tpp_mesh0 = zeros(1,Nt+1);
t_mesh1 = zeros(1,Nt+1);
T_mesh1 = zeros(1,Nt+1);
Tp_mesh1 = zeros(1,Nt+1);
Tpp_mesh1 = zeros(1,Nt+1);
t_mesh2 = zeros(1,Nt+1);
T_mesh2 = zeros(1,Nt+1);
Tp_mesh2 = zeros(1,Nt+1);
Tpp_mesh2 = zeros(1,Nt+1);

xx0 = zeros(1,Nt+1);
xx1 = zeros(1,Nt+1);
xx2 = zeros(1,Nt+1);

N_com = zeros(1,Nt+1);
N_com1 = zeros(1,Nt+1);

% Break Point Information

tzeros = 5;                             % Break point count 
t = zeros(1,tzeros);
v = zeros(1,tzeros);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initial Values for u and t_ 

if util_choice == 0				% Exponential Utility Function
	tol = 10^(-5)				% Tolerance for utility/L loops
	if cong_hyp == 0
		u = 24.468890000000002 		% Starting value of utility
		t_up = 2.457250999999993	% Starting value of time of the first entry
	elseif cong_hyp == 1
		u = 0.0000				% Starting value of utility
		t_up = 0.0000			% Starting value of time of the first entry
    else
        fprintf('ERROR: Choose congested or hypercongested region')
	end

elseif util_choice == 1				% Logarithmic Utility Function
	tol = 10^(-4);                  % Tolerance for utility/L loops
	if cong_hyp == 0
		%u = 23.3148                            % Starting value of utility
		%t_up = 1.9706                          % First point
		u = 24.37617 + 0.00001
		t_up = 2.46056
	elseif cong_hyp == 1
		%u = -4.4296	+ 0.01                  % Starting value of utility
		%t_up = .225593 + 0.000005              % Starting value of time of the first entry
        %u = -8.499599999999914	+ 0.01			% Starting value of utility
		%t_up = 0.194551041955935 + 0.000005	% Starting value of time of the first entry
        %u = -22.229600000000726 + 0.01         % Starting value of utility
        %t_up = 0.092135919146068 + 0.000005    % Starting value of time of the first entry
        u = -25 + 0.01                          % Starting value of utility
        t_up = 0.0788                           % Starting value of time of the first entry
    else
        fprintf('ERROR: Choose congested or hypercongested region')
	end

else

end

% Gradient Parameters

nn = 0;
nnn = 0;
nnnn = 0;

v(1) = vf;                  % Initial Velocity
num_cycles = 3;				% Total Cycles for FP


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MAIN METHOD

%while num_cycles == 3

	% Fineness scale for the utility level	

	if util_choice == 0
		if cong_hyp == 0
			u = u - 0.00001
		elseif cong_hyp == 1
			u = u + 0.00001
		else
		end
		
	elseif util_choice == 1
		if cong_hyp == 0
			u = u - 0.0001
		elseif cong_hyp == 1
			u = u - 0.01
		else
		end
	else

	end

	xx0(end) = 100;                     % Arbitrary value to start gradient method

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	while abs(xx0(end)-L) > tol         % start of inner start-time loop

		clearvars('N_com','N_com1')		

		tbar = t_up;                    % Earliest departure time t = 0 => 5 AM 

		t(1) = tbar;	

		% Computation for T(t), Tdot(t), ... derivatives, and Number of cycle
		 
		T = zeros(1,tzeros+2);
		Tp = zeros(1,tzeros+2);
        
        % Computation of primary breakpoints
		
		if util_choice == 0		
		
			p1 = A0*b1 + a1*(B0 - b1*u);
	    
			T(1) = k - t(1) + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t(1)) ) ));
			Tp(1) = -1 + (A0*a1)/(-A0*b1 + exp(a1*t(1))*p1);
			t(2) = t(1) + T(1);
			v(2) = v(1)/(1+Tp(1));

			for j = 1:tzeros-1
			 
				T(j+1) = k - t(j+1) + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t(j+1)) ) ));
				Tp(j+1) = -1 + (A0*a1)/(-A0*b1 + exp(a1*t(j+1))*p1);
				t(j+2) = t(j+1) + T(j+1);
				v(j+2) = v(j+1)/(1+Tp(j+1));

			end


		elseif util_choice == 1
		    
			T(1) = k - t(1) - exp((1/b0)*(u - a0*log(a1*t(1))));
			Tp(1) = -1 + ((a0)/(b0))*(1./t(1)).*exp((1/b0)*(u - a0*log(a1*t(1))));
			t(2) = t(1) + T(1);
			v(2) = v(1)/(1+Tp(1));

			for j = 1:tzeros-1
			 
				T(j+1) = k - t(j+1) - exp((1/b0)*(u - a0*log(a1*t(j+1))));
				Tp(j+1) = -1 + ((a0)/(b0))*(1./t(j+1)).*exp((1/b0)*(u - a0*log(a1*t(j+1))));
				t(j+2) = t(j+1) + T(j+1);
				v(j+2) = v(j+1)/(1+Tp(j+1));

			end

		else

        end

        % density breakpoints
        
		dens_bp = kj*(1-(v./vf));
        
        % number of cycles computation

		num_cycles = find(dens_bp(1:end-1).*dens_bp(2:end)<0);

		kbp = kj*(1-(v(2)/vf));


		% Break-point values

		dens0(1) = dens_bp(1);
		vel0(1) = vf;
		xx0(1) = 0;
		N_com(1) = 0;

		dens1(1) = dens_bp(2);
		vel1(1) = v(2);
		xx1(1) = 0;
		N_com1(1) = 0;

		dens2(1) = dens_bp(3);
		vel2(1) = v(3);
		xx2(1) = 0;
		N_com2(1) = 0;


		% Mesh Setup

		deltat = (t(2)-t(1))/Nt;

		if util_choice == 0

			t_mesh0 = [t(1):deltat:t(2)];
			T_mesh0 = k - t_mesh0 + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh0) ) ));
			Tp_mesh0 = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh0)*p1);
			Tpp_mesh0 = - ((a1^2)*A0*p1*exp(a1*t_mesh0))./((-A0*b1 + exp(a1*t_mesh0)*p1).^2);

			t_mesh1 = t_mesh0 + T_mesh0;
			T_mesh1 = k - t_mesh1 + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh1) ) ));
			Tp_mesh1 = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh1)*p1);
			Tpp_mesh1 = - ((a1^2)*A0*p1*exp(a1*t_mesh1))./((-A0*b1 + exp(a1*t_mesh1)*p1).^2);

			t_mesh2 = t_mesh1 + T_mesh1;
			T_mesh2 = k - t_mesh2 + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh2) ) ));
			Tp_mesh2 = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh2)*p1);
			Tpp_mesh2 = - ((a1^2)*A0*p1*exp(a1*t_mesh2))./((-A0*b1 + exp(a1*t_mesh2)*p1).^2);


		
		elseif util_choice == 1
		
			t_mesh0 = [t(1):deltat:t(2)];
			T_mesh0 = k - t_mesh0 - exp((1/b0)*(u - a0*log(a1*t_mesh0)));
			Tp_mesh0 = -1 + ((a0)/(b0))*(1./t_mesh0).*exp((1/b0)*(u - a0*log(a1*t_mesh0)));
			Tpp_mesh0 = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh0.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh0)));

			t_mesh1 = t_mesh0 + T_mesh0;
			T_mesh1 = k - t_mesh1 - exp((1/b0)*(u - a0*log(a1*t_mesh1)));
			Tp_mesh1 = -1 + ((a0)/(b0))*(1./t_mesh1).*exp((1/b0)*(u - a0*log(a1*t_mesh1)));
			Tpp_mesh1 = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh1.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh1)));

			t_mesh2 = t_mesh1 + T_mesh1;
			T_mesh2 = k - t_mesh2 - exp((1/b0)*(u - a0*log(a1*t_mesh2)));
			Tp_mesh2 = -1 + ((a0)/(b0))*(1./t_mesh2).*exp((1/b0)*(u - a0*log(a1*t_mesh2)));
			Tpp_mesh2 = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh2.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh2)));


		else

		end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

		for j = 1:Nt

		ENT_temp = zeros(2);
		ENT_temp(1,1) = (2+Tp_mesh0(j));
		ENT_temp(1,2) = -(1 + Tp_mesh0(j))^2; 

		ENT_temp(2,1) = (-1)/(1 + Tp_mesh0(j));
		ENT_temp(2,2) = (2+Tp_mesh1(j));
		
		b_mat = zeros(2,1);
		b_mat(1) = (vel0(j)*(-Tpp_mesh0(j)))/((1+Tp_mesh0(j)));
		b_mat(2) = (vel1(j)*(-Tpp_mesh1(j)))/((1+Tp_mesh1(j)));

		ENT = (vf/kj).*ENT_temp\b_mat; 	


		ent0(j) = ENT(1);
		ent1(j) = ENT(2);

		exit1(j) = ent0(j)/(1+Tp_mesh0(j));  

		dens0(j+1) = ent0(j)*(t_mesh0(j+1)-t_mesh0(j)) + dens0(j);
		vel0(j+1) = (t_mesh0(j+1)-t_mesh0(j))*(-vf/kj)*ent0(j) + vel0(j);
		xx0(j+1) = (t_mesh0(j+1)-t_mesh0(j))*vel0(j) + xx0(j);

		dens1(j+1) = (ent1(j)-exit1(j))*(t_mesh1(j+1)-t_mesh1(j)) + dens1(j);
		vel1(j+1) = (t_mesh1(j+1)-t_mesh1(j))*(-vf/kj)*(ent1(j)-exit1(j)) + vel1(j);
		xx1(j+1) = (t_mesh1(j+1)-t_mesh1(j))*vel1(j) + xx1(j);

		end


		% Last Cycle - Exits Only

		exit2(1) = ent1(1)/(1+Tp_mesh1(1));
		kdot2(1) = (-exit2(1));
		dens2(2) = kdot2(1)*(t_mesh2(2)-t_mesh2(1)) + dens2(1);
		vel2(2) = (t_mesh2(2)-t_mesh2(1))*(-vf/kj)*(kdot2(1)) + vel2(1);

		for j = 1:Nt

			exit2(j+1) = ent1(j+1)/(1+Tp_mesh1(j+1));
			kdot2(j+1) = (-exit2(j+1));
			dens2(j+1) = kdot2(j)*(t_mesh2(j+1)-t_mesh2(j)) + dens2(j);
			vel2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*(-vf/kj)*(kdot2(j)) + vel2(j);

		end


		t_index_temp = find(vf-vel2<0);
		t_index = t_index_temp(1)-1;


		% Matrix Solver for Second Subcycle of all Cycles

		for j = t_index+1:Nt

		ENT_temp1 = zeros(2-1);

		ENT_temp1(1,1) = (2+Tp_mesh0(j));

		b_mat1 = zeros(2-1,1);
		b_mat1(1) = (vel0(j)*(-Tpp_mesh0(j)))/((1+Tp_mesh0(j)));

		ENT = (vf/kj).*ENT_temp1\b_mat1;

		ent0(j) = ENT(1);
		ent1(j) = 0;
		
		exit1(j) = ent0(j)/(1+Tp_mesh0(j));  

		dens0(j+1) = ent0(j)*(t_mesh0(j+1)-t_mesh0(j)) + dens0(j);
		vel0(j+1) = (t_mesh0(j+1)-t_mesh0(j))*(-vf/kj)*ent0(j) + vel0(j);
		xx0(j+1) = (t_mesh0(j+1)-t_mesh0(j))*vel0(j) + xx0(j);

		dens1(j+1) = (ent1(j)-exit1(j))*(t_mesh1(j+1)-t_mesh1(j)) + dens1(j);
		vel1(j+1) = (t_mesh1(j+1)-t_mesh1(j))*(-vf/kj)*(ent1(j)-exit1(j)) + vel1(j);
		xx1(j+1) = (t_mesh1(j+1)-t_mesh1(j))*vel1(j) + xx1(j);

		end

		ENT_temp1 = zeros(2-1);

		ENT_temp1(1,1) = (2+Tp_mesh0(Nt+1));

		b_mat1 = zeros(2-1,1);
		b_mat1(1) = (vel0(Nt+1)*(-Tpp_mesh0(Nt+1)))/((1+Tp_mesh0(Nt+1)));
		
		ENT = (vf/kj).*ENT_temp1\b_mat1;

		ent0(Nt+1) = ENT(1);
		ent1(Nt+1) = 0;

		exit1(Nt+1) = ent0(Nt+1)/(1+Tp_mesh0(Nt+1)); 
		
		
		% Last Cycle - Exits Only

		exit2(1) = ent1(1)/(1+Tp_mesh1(1));
		kdot2(1) = (-exit2(1));
		dens2(2) = kdot2(1)*(t_mesh2(2)-t_mesh2(1)) + dens2(1);
		vel2(2) = (t_mesh2(2)-t_mesh2(1))*(-vf/kj)*(kdot2(1)) + vel2(1);

		for j = 1:Nt

			exit2(j+1) = ent1(j+1)/(1+Tp_mesh1(j+1));
			kdot2(j+1) = (-exit2(j+1));
			dens2(j+1) = kdot2(j)*(t_mesh2(j+1)-t_mesh2(j)) + dens2(j);
			vel2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*(-vf/kj)*(kdot2(j)) + vel2(j);

		end

		
		% Error computation at each time step

		error_count2(nnn+1) = abs(xx0(end) - L);
		nnn = nnn + 1;

		% Fineness scale for time of the first entry (fixed point method is on)

		if util_choice == 0

			if cong_hyp == 0
                gamma_save = 0.05*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up - 0.000001
			elseif cong_hyp == 1
				t_up = t_up + 0.000001
			else
			end

		elseif util_choice == 1

			if cong_hyp == 0
				gamma_save = 0.01*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up - 0.000001
			elseif cong_hyp == 1
				%t_up = t_up + 0.000001
                gamma_save = 0.0075*( xx0(end)-L );
				t_up = t_up - gamma_save
			else
			end
		
		else

		end


	end				% L loop end

	if cong_hyp == 0
		t_up = t_up + gamma_save
		%t_up = t_up + 0.000001
	elseif cong_hyp == 1
		%t_up = t_up - 0.000001
        t_up = t_up + gamma_save
	else
	end 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	% Number of Commuters (N) Computation
	
	% First Subcycle

	for j = 1:t_index
		N_com(j+1) = ent0(j)*(t_mesh0(j+1)-t_mesh0(j)) + N_com(j);
		N_com1(j+1) = ent1(j)*(t_mesh1(j+1)-t_mesh1(j)) + N_com1(j);
	end
	
	% Second Subcycle

	for j = t_index+1:Nt
		N_com(j+1) = ent0(j)*(t_mesh0(j+1)-t_mesh0(j)) + N_com(j);
	end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	% Recomputing number of cycles for outer loop
		 
	T = zeros(1,tzeros+2);
	Tp = zeros(1,tzeros+2);
		
		
	if util_choice == 0		
		
		p1 = A0*b1 + a1*(B0 - b1*u);
	    
		T(1) = k - t(1) + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t(1)) ) ));
		Tp(1) = -1 + (A0*a1)/(-A0*b1 + exp(a1*t(1))*p1);
		t(2) = t(1) + T(1);
		v(2) = v(1)/(1+Tp(1));

		for j = 1:tzeros-1
			 
			T(j+1) = k - t(j+1) + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t(j+1)) ) ));
			Tp(j+1) = -1 + (A0*a1)/(-A0*b1 + exp(a1*t(j+1))*p1);
			t(j+2) = t(j+1) + T(j+1);
			v(j+2) = v(j+1)/(1+Tp(j+1));

		end


	elseif util_choice == 1
		    
		T(1) = k - t(1) - exp((1/b0)*(u - a0*log(a1*t(1))));
		Tp(1) = -1 + ((a0)/(b0))*(1./t(1)).*exp((1/b0)*(u - a0*log(a1*t(1))));
		t(2) = t(1) + T(1);
		v(2) = v(1)/(1+Tp(1));

		for j = 1:tzeros-1
			 
			T(j+1) = k - t(j+1) - exp((1/b0)*(u - a0*log(a1*t(j+1))));
			Tp(j+1) = -1 + ((a0)/(b0))*(1./t(j+1)).*exp((1/b0)*(u - a0*log(a1*t(j+1))));
			t(j+2) = t(j+1) + T(j+1);
			v(j+2) = v(j+1)/(1+Tp(j+1));

		end

	else

	end

	dens_bp = kj*(1-(v./vf));

	num_cycles = find(dens_bp(1:end-1).*dens_bp(2:end)<0);

	% Storage of equilibium utility, start time t_, and N

	u_matrix(nnnn+1) = u;
	t_matrix(nnnn+1) = t_up;
	T_matrix(nnnn+1) = T(1);
	Tp_matrix(nnnn+1) = Tp(1);
	N_com_matrix(nnnn+1) = N_com(end) + N_com1(end);

	nnnn = nnnn+1

%end 					% Outer utility loop end


toc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculation of the trip duration function T(t;u), and its derivatives for
% plotting

if util_choice == 0
	p1 = A0*b1 + a1*(B0 - b1*u);
	TTTT = k - tttt + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*tttt) ) ));
	TTTTp = -1 + (A0*a1)./(-A0*b1 + exp(a1*tttt)*p1);
	TTTTpp = - ((a1^2)*A0*p1*exp(a1*tttt))./((-A0*b1 + exp(a1*tttt)*p1).^2);

elseif util_choice == 1

	TTTT = k - tttt - exp((1/b0)*(u - a0*log(a1*tttt)));
	TTTTp = -1 + ((a0)/(b0))*(1./tttt).*exp((1/b0)*(u - a0*log(a1*tttt)));
	TTTTpp = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(tttt.^2)).*exp((1/b0)*(u - a0*log(a1*tttt)));

else

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Plotting of T(t), v(t), k(t), e(t), x(t), d(t), etc. 

figure(1)
T_plt = plot(tttt,TTTT,'-k','LineWidth',3), hold on
plot(tttt(1:500:end),TTTT(1:500:end),'ko','MarkerSize',20)
pT = plot(tttt(1),TTTT(1),'-ko','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tp_plt = plot(tttt,TTTTp,'-r','LineWidth',3), hold on
plot(tttt(1:500:end),TTTTp(1:500:end),'rs','MarkerSize',20)
pTp = plot(tttt(1),TTTTp(1),'-rs','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TTp_plt = plot(tttt,TTTTpp,'-b','LineWidth',3), hold on
plot(tttt(1:500:end),TTTTpp(1:500:end),'bd','MarkerSize',20)
pTTp = plot(tttt(1),TTTTpp(1),'-bd','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%plot(tttt,TTTTppp,'-m','LineWidth',3), hold on
%tT_plt = plot(tttt,tttt+TTTT,'b-'), hold off
l1 = legend([pT,pTp,pTTp],'$T(t)$','$\ddot{T}(t)$','$\ddot{T}(t)$');
set(l1,'Interpreter','latex');
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
title('Travel Time $T(t)$ and Derivatives','Interpreter','latex')
%ttl1 = sprintf('Travel Time $T(t)$ and Derivatives');
%title(ttl1,'FontWeight','bold','Interpreter','latex';
xlim([t(1) t_mesh2(end)]);
ylim([-3 5]);
xlabel('Clock Time','Interpreter','latex');
ylabel('$T(t)$, $\dot{T}(t)$, $\ddot{T}(t)$','Interpreter','latex');
grid on

% Velocity Plot

figure(2)
bpv = plot(t(1:5),v(1:5),'ko','MarkerSize',20), hold on
v0p = plot(t_mesh0(1),vel0(1),'-bd','MarkerSize',20), hold on
v1p = plot(t_mesh1(1),vel1(1),'-rs','MarkerSize',20), hold on
v2p = plot(t_mesh2(1),vel2(1),'-gx','MarkerSize',20), hold on
bpv = plot(t(1:5),v(1:5),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v0plt = plot(t_mesh0,vel0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:4000:t_index+1),vel0(1:4000:t_index+1),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:t_index+1),vel0(1:4000:t_index+1),'bd','MarkerSize',22), hold on
plot(t_mesh0(t_index+1:40000:end-1),vel0(t_index+1:40000:end-1),'bd','MarkerSize',20), hold on
plot(t_mesh0(t_index+1:40000:end-1),vel0(t_index+1:40000:end-1),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v1plt = plot(t_mesh1,vel1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:3000:t_index+1),vel1(1:3000:t_index+1),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:3000:t_index+1),vel1(1:3000:t_index+1),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),vel1(t_index+1:10000:t_index+40000),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),vel1(t_index+1:10000:t_index+40000),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+40000:70000:end-1),vel1(t_index+40000:70000:end-1),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+40000:70000:end-1),vel1(t_index+40000:70000:end-1),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v2plt = plot(t_mesh2,vel2,'g-','LineWidth',3), hold on
plot(t_mesh2(1:3000:t_index+1),vel2(1:3000:t_index+1),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:3000:t_index+1),vel2(1:3000:t_index+1),'gx','MarkerSize',22), hold on
plot(t_mesh2(1:20000:end),vel2(1:20000:end),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:20000:end),vel2(1:20000:end),'gx','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
leg1 = legend([bpv,v0p,v1p,v2p],'Break points','$v(t)$ - Cycle 1','$v(t)$ - Cycle 2','$v(t)$ - Cycle 3')
set(leg1,'Interpreter','latex');
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
title('Velocity $v(t)$','Interpreter','latex');
xlabel('Clock Time','Interpreter','latex');
ylabel('Velocity (mph)','Interpreter','latex');
ylim([-6 16])
xlim([t(1) t_mesh2(end)])
%ylim([0 6])
grid on

% Density Plot

figure(3)
bpk = plot(t(1:5),dens_bp(1:5),'ko','MarkerSize',20), hold on
k0p = plot(t_mesh0(1),dens0(1),'-bd','MarkerSize',20), hold on
k1p = plot(t_mesh1(1),dens1(1),'-rs','MarkerSize',20), hold on
k2p = plot(t_mesh2(1),dens2(1),'-gx','MarkerSize',20), hold on
bpk = plot(t(1:5),dens_bp(1:5),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k0plt = plot(t_mesh0,dens0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:4000:t_index+1),dens0(1:4000:t_index+1),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:t_index+1),dens0(1:4000:t_index+1),'bd','MarkerSize',22), hold on
plot(t_mesh0(t_index+1:40000:end-1),dens0(t_index+1:40000:end-1),'bd','MarkerSize',20), hold on
plot(t_mesh0(t_index+1:40000:end-1),dens0(t_index+1:40000:end-1),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k1plt = plot(t_mesh1,dens1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:3000:t_index+1),dens1(1:3000:t_index+1),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:3000:t_index+1),dens1(1:3000:t_index+1),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),dens1(t_index+1:10000:t_index+40000),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),dens1(t_index+1:10000:t_index+40000),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+40000:70000:end-1),dens1(t_index+40000:70000:end-1),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+40000:70000:end-1),dens1(t_index+40000:70000:end-1),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k2plt = plot(t_mesh2,dens2,'g-','LineWidth',3), hold on
plot(t_mesh2(1:3000:t_index+1),dens2(1:3000:t_index+1),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:3000:t_index+1),dens2(1:3000:t_index+1),'gx','MarkerSize',22), hold on
plot(t_mesh2(1:20000:end),dens2(1:20000:end),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:20000:end),dens2(1:20000:end),'gx','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
legk = legend([bpk,k0p,k1p,k2p],'Break points','$k(t)$ - Cycle 1','$k(t)$ - Cycle 2','$k(t)$ - Cycle 3')
set(legk,'Interpreter','latex');
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
title('Density $k(t)$','Interpreter','latex');
ylabel('Density','Interpreter','latex');
xlabel('Clock Time','Interpreter','latex');
xlim([t(1) t_mesh2(end)])
ylim([-1.2e05 12e05])
grid on

% Entry and Exit Rate Plot

figure(4)
e0p = plot(t_mesh0(1),ent0(1),'-bd','MarkerSize',20), hold on
e1p = plot(t_mesh1(1),ent1(1),'-bd','MarkerSize',20), hold on
x1p = plot(t_mesh1(1),exit1(1),'-rs','MarkerSize',20), hold on
x2p = plot(t_mesh2(1),exit2(1),'-rs','MarkerSize',20), hold on
e0p = plot(t_mesh0(1),ent0(1),'-bd','MarkerSize',22), hold on
e1p = plot(t_mesh1(1),ent1(1),'-bd','MarkerSize',22), hold on
x1p = plot(t_mesh1(1),exit1(1),'-rs','MarkerSize',22), hold on
x2p = plot(t_mesh2(1),exit2(1),'-rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e0plt = plot(t_mesh0,ent0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:4000:t_index+1),ent0(1:4000:t_index+1),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:t_index+1),ent0(1:4000:t_index+1),'bd','MarkerSize',22), hold on
plot(t_mesh0(t_index+1:40000:end-1),ent0(t_index+1:40000:end-1),'bd','MarkerSize',20), hold on
plot(t_mesh0(t_index+1:40000:end-1),ent0(t_index+1:40000:end-1),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e1plt = plot(t_mesh1,ent1,'b-','LineWidth',3), hold on
plot(t_mesh1(1:3000:t_index+1),ent1(1:3000:t_index+1),'bd','MarkerSize',20), hold on
plot(t_mesh1(1:3000:t_index+1),ent1(1:3000:t_index+1),'bd','MarkerSize',22), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),ent1(t_index+1:10000:t_index+40000),'bd','MarkerSize',20), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),ent1(t_index+1:10000:t_index+40000),'bd','MarkerSize',22), hold on
plot(t_mesh1(t_index+40000:70000:end-1),ent1(t_index+40000:70000:end-1),'bd','MarkerSize',20), hold on
plot(t_mesh1(t_index+40000:70000:end-1),ent1(t_index+40000:70000:end-1),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1plt = plot(t_mesh1,exit1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:3000:t_index+1),exit1(1:3000:t_index+1),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:3000:t_index+1),exit1(1:3000:t_index+1),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),exit1(t_index+1:10000:t_index+40000),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+1:10000:t_index+40000),exit1(t_index+1:10000:t_index+40000),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+40000:70000:end-1),exit1(t_index+40000:70000:end-1),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+40000:70000:end-1),exit1(t_index+40000:70000:end-1),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x2plt = plot(t_mesh2,exit2,'r-','LineWidth',3), hold on
plot(t_mesh2(1:3000:t_index+1),exit2(1:3000:t_index+1),'rs','MarkerSize',20), hold on
plot(t_mesh2(1:3000:t_index+1),exit2(1:3000:t_index+1),'rs','MarkerSize',22), hold on
plot(t_mesh2(1:20000:end),exit2(1:20000:end),'rs','MarkerSize',20), hold on
plot(t_mesh2(1:20000:end),exit2(1:20000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lege = legend([e0p,x1p],'$e(t)$ - Entry Rate','$x(t)$ - Exit Rate')
set(lege,'Interpreter','latex');
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
title('Entry Rate $e(t)$ and Exit Rate $x(t)$','Interpreter','latex')
ylabel('e(t), x(t)','Interpreter','latex')
xlabel('Clock Time','Interpreter','latex')
ylim([-.5e06 1.4e07])
xlim([t(1) t_mesh2(end)])
grid on

% Travel Distance Plot

figure(5)
bpx = plot(t(1),xx0(1),'ko','MarkerSize',20), hold on
x0p = plot(t_mesh0(1),xx0(1),'-bd','MarkerSize',20), hold on
x1p = plot(t_mesh1(1),xx1(1),'-rs','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bpx = plot(t(1),xx0(1),'ko','MarkerSize',22), hold on
plot(t(2),xx1(1),'ko','MarkerSize',20), hold on
plot(t(2),xx1(1),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0plt = plot(t_mesh0,xx0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:40000:end),xx0(1:40000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:40000:end),xx0(1:40000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1plt = plot(t_mesh1,xx1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:3000:t_index+1),xx1(1:3000:t_index+1),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:3000:t_index+1),xx1(1:3000:t_index+1),'rs','MarkerSize',22), hold on
plot(t_mesh1(t_index+1:70000:end),xx1(t_index+1:70000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1(t_index+1:70000:end),xx1(t_index+1:70000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
legx = legend([bpx,x0p,x1p],'Break Points','Distance - Cycle 1','Distance - Cycle 2')
set(legx,'Interpreter','latex');
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
title('Trip Length','Interpreter','latex')
ylabel('Distance (miles)','Interpreter','latex')
xlabel('Clock Time','Interpreter','latex')
xlim([t(1) t_mesh2(end)])
ylim([-.2 5])
grid on


% % Utility Function
% 
% 
% figure(6)
% um = plot(tttt,u_max,'b-'), hold on
% ump = plot(tttt,u_max_prime,'r-'), hold on 
% legend([um,ump],'U(t)','U`(t)')
% title('Utility Function')
% %ylim([-.5*10^5 10^6])
% grid on
% 
% % Error Plot
% 
% figure(7)
% err2 = plot([1:1:nnn],error_count2,'r-'), hold on
% title('Error for utility and break point')
% legend([err2],'Distance (L) Error')
% xlabel('Number of Iterations')
% ylabel('Error (L^c - L)')
% %ylim([-.5*10^5 10^6])
% grid on