% Algorithm for THREE FULL CYCLES, 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 = 0;				% 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

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

% 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;


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);

else

end

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


% Mesh grid for time

if util_choice == 0
	%Nt = 10000;  % log
    Nt = 80000;
elseif util_choice == 1
	Nt = 45000;	
else

end

% Preallocation

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

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

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

exit1 = zeros(1,Nt+1);
exit2 = zeros(1,Nt+1);
exit3 = zeros(1,Nt+1);
exit4 = 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);
t_mesh3 = zeros(1,Nt+1);
T_mesh3 = zeros(1,Nt+1);
Tp_mesh3 = zeros(1,Nt+1);
Tpp_mesh3 = zeros(1,Nt+1);
t_mesh4 = zeros(1,Nt+1);
T_mesh4 = zeros(1,Nt+1);
Tp_mesh4 = zeros(1,Nt+1);
Tpp_mesh4 = zeros(1,Nt+1);

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

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

% Break Point Information

tzeros = 6;                             % Break point count 
t = zeros(1,tzeros);
v = zeros(1,tzeros);
xx0 = zeros(1,Nt+1);


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

% Initial Values for u and t_ 


if util_choice == 0                 % Exponential Utility Function
	tol = 10^(-3)                   % Tolerance for utility/L loops 10^-4, u 0.0001, t 0.000001
	if cong_hyp == 0
		u = 23.880160000005773      % Starting value of utility    
		t_up = 1.832697631807088	% Refinement
	elseif cong_hyp == 1
		u = 16                      % Starting value of utility
		t_up = -0.52			% Second point
	else
	end
	
elseif util_choice == 1                         % Logarithmic
	tol = 10^(-4);                              % Tolerance for utility/L loops
	if cong_hyp == 0
		%u = 23.3174 + 0.001                    % Starting value of utility
		%t_up = 1.97125                         % Starting value of time of the first entry
		%u = 17.393399999992759 + 0.001         % Starting value of utility
		%t_up = 1.065421061749116 + 0.000001	% Starting value of time of the first entry
% 		u = 16.880399999992132 + 0.001          % Starting value of utility
% 		t_up = 1.018865282924472 + 0.000001     % Starting value of time of the first entry
        u = 20 + 0.001                          % Starting value of utility
        t_up = 1.353 + 0.000005                 % Starting value of time of the first entry
	elseif cong_hyp == 1
		u = 15.7 - 0.00001                      % Starting value of utility
		t_up = 0.92125 - 0.00001                % Starting value of time of the first entry
	else
	end

else
    fprintf('ERROR: Choose congested or hypercongested region')
end


% Gradient Parameters

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

v(1) = vf;				% Initial Velocity
num_cycles = 5;				% Total Cycles for FFFP

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

% MAIN METHOD

%while num_cycles == 5

	% Fineness scale for the utility level		

	if util_choice == 0
		if cong_hyp == 0
			u = u - 0.001
		elseif cong_hyp == 1
			u = u + 0.001
        else
            fprintf('ERROR: Choose congested or hypercongested region')
		end
		
	elseif util_choice == 1
		if cong_hyp == 0
			u = u - 0.001
		elseif cong_hyp == 1
			u = u + 0.00001
        else
            fprintf('ERROR: Choose congested or hypercongested region')
		end
    else
        fprintf('ERROR: Choose congested or hypercongested region')
	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','N_com2','N_com3')
		
		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);
		
		
		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);

		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;

		dens3(1) = dens_bp(4);
		vel3(1) = v(4);
		xx3(1) = 0;
		N_com3(1) = 0;

		dens4(1) = dens_bp(5);
		vel4(1) = v(5);
		xx4(1) = 0;

		% Mesh Setup

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

		if util_choice == 0
			
			p1 = A0*b1 + a1*(B0 - b1*u);
			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);

			t_mesh3 = t_mesh2 + T_mesh2;
			T_mesh3 = k - t_mesh3 + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh3) ) ));
			Tp_mesh3 = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh3)*p1);
			Tpp_mesh3 = - ((a1^2)*A0*p1*exp(a1*t_mesh3))./((-A0*b1 + exp(a1*t_mesh3)*p1).^2);

			t_mesh4 = t_mesh3 + T_mesh3;
			T_mesh4 = k - t_mesh4 + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh4) ) ));
			Tp_mesh4 = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh4)*p1);
			Tpp_mesh4 = - ((a1^2)*A0*p1*exp(a1*t_mesh4))./((-A0*b1 + exp(a1*t_mesh4)*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)));

			t_mesh3 = t_mesh2 + T_mesh2;
			T_mesh3 = k - t_mesh3 - exp((1/b0)*(u - a0*log(a1*t_mesh3)));
			Tp_mesh3 = -1 + ((a0)/(b0))*(1./t_mesh3).*exp((1/b0)*(u - a0*log(a1*t_mesh3)));
			Tpp_mesh3 = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh3.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh3)));

			t_mesh4 = t_mesh3 + T_mesh3;
			T_mesh4 = k - t_mesh4 - exp((1/b0)*(u - a0*log(a1*t_mesh4)));
			Tp_mesh4 = -1 + ((a0)/(b0))*(1./t_mesh4).*exp((1/b0)*(u - a0*log(a1*t_mesh4)));
			Tpp_mesh4 = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh4.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh4)));

		else

		end

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

		for j = 1:Nt

		ENT_temp = zeros(4);
		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));
		ENT_temp(2,3) = -(1 + Tp_mesh1(j))^2;

		ENT_temp(3,2) = (-1)/(1 + Tp_mesh1(j));
		ENT_temp(3,3) = (2+Tp_mesh2(j));
		ENT_temp(3,4) = -(1 + Tp_mesh2(j))^2;

		ENT_temp(4,3) = (-1)/(1 + Tp_mesh2(j));
		ENT_temp(4,4) = (2+Tp_mesh3(j));

		b_mat = zeros(4,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)));
		b_mat(3) = (vel2(j)*(-Tpp_mesh2(j)))/((1+Tp_mesh2(j)));
		b_mat(4) = (vel3(j)*(-Tpp_mesh3(j)))/((1+Tp_mesh3(j)));


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


		ent0(j) = ENT(1);
		ent1(j) = ENT(2);
		ent2(j) = ENT(3);
		ent3(j) = ENT(4);

		exit1(j) = ent0(j)/(1+Tp_mesh0(j)); 
		exit2(j) = ent1(j)/(1+Tp_mesh1(j)); 
		exit3(j) = ent2(j)/(1+Tp_mesh2(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);

		dens2(j+1) = (ent2(j)-exit2(j))*(t_mesh2(j+1)-t_mesh2(j)) + dens2(j);
		vel2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*(-vf/kj)*(ent2(j)-exit2(j)) + vel2(j);
		xx2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*vel2(j) + xx2(j);

		dens3(j+1) = (ent3(j)-exit3(j))*(t_mesh3(j+1)-t_mesh3(j)) + dens3(j);
		vel3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*(-vf/kj)*(ent3(j)-exit3(j)) + vel3(j);
		xx3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*vel3(j) + xx3(j);

		end


		% Last Cycle - Exits Only

		exit4(1) = ent3(1)/(1+Tp_mesh3(1));
		kdot4(1) = (-exit4(1));
		dens4(2) = kdot4(1)*(t_mesh4(2)-t_mesh4(1)) + dens4(1);
		vel4(2) = (t_mesh4(2)-t_mesh4(1))*(-vf/kj)*(kdot4(1)) + vel4(1);

		for j = 1:Nt

			exit4(j+1) = ent3(j+1)/(1+Tp_mesh3(j+1));
			kdot4(j+1) = (-exit4(j+1));
			dens4(j+1) = kdot4(j)*(t_mesh4(j+1)-t_mesh4(j)) + dens4(j);
			vel4(j+1) = (t_mesh4(j+1)-t_mesh4(j))*(-vf/kj)*(kdot4(j)) + vel4(j);

		end


		t_index_temp = find(vf-vel4<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(4-1);
		ENT_temp1(1,1) = (2+Tp_mesh0(j));
		ENT_temp1(1,2) = -(1 + Tp_mesh0(j))^2; 

		ENT_temp1(2,1) = (-1)/(1 + Tp_mesh0(j));
		ENT_temp1(2,2) = (2+Tp_mesh1(j));
		ENT_temp1(2,3) = -(1 + Tp_mesh1(j))^2;

		ENT_temp1(3,2) = (-1)/(1 + Tp_mesh1(j));
		ENT_temp1(3,3) = (2+Tp_mesh2(j));

		b_mat1 = zeros(4-1,1);
		b_mat1(1) = (vel0(j)*(-Tpp_mesh0(j)))/((1+Tp_mesh0(j)));
		b_mat1(2) = (vel1(j)*(-Tpp_mesh1(j)))/((1+Tp_mesh1(j)));
		b_mat1(3) = (vel2(j)*(-Tpp_mesh2(j)))/((1+Tp_mesh2(j)));

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

		ent0(j) = ENT(1);
		ent1(j) = ENT(2);
		ent2(j) = ENT(3);
		ent3(j) = 0;

		exit1(j) = ent0(j)/(1+Tp_mesh0(j)); 
		exit2(j) = ent1(j)/(1+Tp_mesh1(j));
		exit3(j) = ent2(j)/(1+Tp_mesh2(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);

		dens2(j+1) = (ent2(j)-exit2(j))*(t_mesh2(j+1)-t_mesh2(j)) + dens2(j);
		vel2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*(-vf/kj)*(ent2(j)-exit2(j)) + vel2(j);
		xx2(j+1) = (t_mesh2(j+1)-t_mesh2(j))*vel2(j) + xx2(j);

		dens3(j+1) = (ent3(j)-exit3(j))*(t_mesh3(j+1)-t_mesh3(j)) + dens3(j);
		vel3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*(-vf/kj)*(ent3(j)-exit3(j)) + vel3(j);
		xx3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*vel3(j) + xx3(j);


		end

		ENT_temp1 = zeros(4-1);
		ENT_temp1(1,1) = (2+Tp_mesh0(Nt+1));
		ENT_temp1(1,2) = -(1 + Tp_mesh0(Nt+1))^2; 

		ENT_temp1(2,1) = (-1)/(1 + Tp_mesh0(Nt+1));
		ENT_temp1(2,2) = (2+Tp_mesh1(Nt+1));
		ENT_temp1(2,3) = -(1 + Tp_mesh1(Nt+1))^2;

		ENT_temp1(3,2) = (-1)/(1 + Tp_mesh1(Nt+1));
		ENT_temp1(3,3) = (2+Tp_mesh2(Nt+1));

		b_mat1 = zeros(4-1,1);
		b_mat1(1) = (vel0(Nt+1)*(-Tpp_mesh0(Nt+1)))/((1+Tp_mesh0(Nt+1)));
		b_mat1(2) = (vel1(Nt+1)*(-Tpp_mesh1(Nt+1)))/((1+Tp_mesh1(Nt+1)));
		b_mat1(3) = (vel2(Nt+1)*(-Tpp_mesh2(Nt+1)))/((1+Tp_mesh2(Nt+1)));

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

		ent0(Nt+1) = ENT(1);
		ent1(Nt+1) = ENT(2);
		ent2(Nt+1) = ENT(3);
		ent3(Nt+1) = 0;

		exit1(Nt+1) = ent0(Nt+1)/(1+Tp_mesh0(Nt+1)); 
		exit2(Nt+1) = ent1(Nt+1)/(1+Tp_mesh1(Nt+1));
		exit3(Nt+1) = ent2(Nt+1)/(1+Tp_mesh2(Nt+1)); 

		
		% Last Cycle - Exits Only

		exit4(1) = ent3(1)/(1+Tp_mesh3(1));
		kdot4(1) = (-exit4(1));
		dens4(2) = kdot4(1)*(t_mesh4(2)-t_mesh4(1)) + dens4(1);
		vel4(2) = (t_mesh4(2)-t_mesh4(1))*(-vf/kj)*(kdot4(1)) + vel4(1);

		for j = 1:Nt

			exit4(j+1) = ent3(j+1)/(1+Tp_mesh3(j+1));
			kdot4(j+1) = (-exit4(j+1));
			dens4(j+1) = kdot4(j)*(t_mesh4(j+1)-t_mesh4(j)) + dens4(j);
			vel4(j+1) = (t_mesh4(j+1)-t_mesh4(j))*(-vf/kj)*(kdot4(j)) + vel4(j);

		end

		% Error computation at each time step

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

		% Fineness scale in time for G and H plots

		if util_choice == 0

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

		elseif util_choice == 1

			if cong_hyp == 0
				gamma_save = 0.0075*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up - 0.00001
			elseif cong_hyp == 1
				gamma_save = 0.001*( 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.00001
	elseif cong_hyp == 1
		t_up = t_up + gamma_save
		%t_up = t_up - 0.000001
	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);
		N_com2(j+1) = ent2(j)*(t_mesh2(j+1)-t_mesh2(j)) + N_com2(j);
		N_com3(j+1) = ent3(j)*(t_mesh3(j+1)-t_mesh3(j)) + N_com3(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);
		N_com1(j+1) = ent1(j)*(t_mesh1(j+1)-t_mesh1(j)) + N_com1(j);
		N_com2(j+1) = ent2(j)*(t_mesh2(j+1)-t_mesh2(j)) + N_com2(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) + N_com2(end) + N_com3(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)));
	TTTTppp = (a0/b0)*((-a0/b0)-1)*((-a0/b0)-2)*exp(u/b0)*(a1^(-a0/b0))*(tttt.^((-a0/b0)-3));
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
%ax = axes;
%set(ax,'fontsize',50,'TickLabelInterpreter', 'latex');
l1 = legend([pT,pTp,pTTp],'$T(t)$','$\dot{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_mesh4(end)]);
ylim([-3 2]);
%xlim([2.4 3.2]);
xlabel('Clock Time','Interpreter','latex');
ylabel('$T(t)$, $\dot{T}(t)$, $\ddot{T}(t)$','Interpreter','latex');
grid on


figure(2)
%plot(tttt,((v(2)/(1+Tp(2)))+1)*(((a0)/(b0))*(1./tttt).*exp((1/b0)*(u - a0*log(a1*tttt)))),'-k','LineWidth',3), hold on
%plot(tttt,(-5.919285397155480)*tttt + (v(2)-(-5.919285397155480)*t(2)),'c','LineWidth',3 ), hold on
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
v3p = plot(t_mesh3(1),vel3(1),'-mv','MarkerSize',20), hold on
v4p = plot(t_mesh4(1),vel4(1),'-c^','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:end),vel0(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:end),vel0(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v1plt = plot(t_mesh1,vel1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:4000:end),vel1(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:4000:end),vel1(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v2plt = plot(t_mesh2,vel2,'g-','LineWidth',3), hold on
plot(t_mesh2(1:4000:end),vel2(1:4000:end),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:4000:end),vel2(1:4000:end),'gx','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v3plt = plot(t_mesh3,vel3,'m-','LineWidth',3), hold on
plot(t_mesh3(1:4000:end),vel3(1:4000:end),'mv','MarkerSize',20), hold on
plot(t_mesh3(1:4000:end),vel3(1:4000:end),'mv','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v4plt = plot(t_mesh4,vel4,'c-','LineWidth',3), hold on
plot(t_mesh4(1:4000:end),vel4(1:4000:end),'c^','MarkerSize',20), hold on
plot(t_mesh4(1:4000:end),vel4(1:4000:end),'c^','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
title('Velocity $v(t)$','Interpreter','latex')
xlabel('Clock Time','Interpreter','latex')
ylabel('Velocity (mph)','Interpreter','latex')
ylim([-6 16])
xlim([t(1) t_mesh4(end)])
%ylim([0 6])
grid on
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex');
% Block 1
% Axes handle 1 (this is the visible axes)
ah1 = gca;
% Legend at axes 1
led1 = legend(ah1,[bpv,v0p,v1p],'Break points','$v(t)$ - Cycle 1','$v(t)$ - Cycle 2',[0 0 0 0])
led1.FontSize = 40;
set(led1,'Interpreter','latex');
% Block 2
% Axes handle 2 (unvisible, only for place the second legend)
ah2=axes('position',get(gca,'position'), 'visible','off');
% Legend at axes 2
led2 = legend(ah2,[v2p,v3p,v4p],'$v(t)$ - Cycle 3','$v(t)$ - Cycle 4','$v(t)$ - Cycle 5',[1 1 1 1])
led2.FontSize = 40;
set(led2,'Interpreter','latex');
%led1 = legend([bpv,v0p,v1p],'Break points','v(t) - Cycle 1','v(t) - Cycle 2');
%set(led1,'Interpreter','latex');
%led2 = legend([v2p,v3p],'Break points','v(t) - Cycle 1','v(t) - Cycle 2');
%set(led2,'Interpreter','latex');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%




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
k3p = plot(t_mesh3(1),dens3(1),'-mv','MarkerSize',20), hold on
k4p = plot(t_mesh4(1),dens4(1),'-c^','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:end),dens0(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:end),dens0(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k1plt = plot(t_mesh1,dens1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:4000:end),dens1(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:4000:end),dens1(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k2plt = plot(t_mesh2,dens2,'g-','LineWidth',3), hold on
plot(t_mesh2(1:4000:end),dens2(1:4000:end),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:4000:end),dens2(1:4000:end),'gx','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k3plt = plot(t_mesh3,dens3,'m-','LineWidth',3), hold on
plot(t_mesh3(1:4000:end),dens3(1:4000:end),'mv','MarkerSize',20), hold on
plot(t_mesh3(1:4000:end),dens3(1:4000:end),'mv','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k4plt = plot(t_mesh4,dens4,'c-','LineWidth',3), hold on
plot(t_mesh4(1:4000:end),dens4(1:4000:end),'c^','MarkerSize',20), hold on
plot(t_mesh4(1:4000:end),dens4(1:4000:end),'c^','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
title('Density $k(t)$','Interpreter','latex')
xlabel('Clock Time','Interpreter','latex')
ylabel('Density','Interpreter','latex')
xlim([t(1) t_mesh4(end)])
%ylim([0 6])
grid on
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex')
% Block 1
% Axes handle 1 (this is the visible axes)
ah1 = gca;
% Legend at axes 1
ledk1 = legend(ah1,[bpk,k0p,k1p],'Break points','$k(t)$ - Cycle 1','$k(t)$ - Cycle 2',[0 0 0 0])
ledk1.FontSize = 40;
set(ledk1,'Interpreter','latex');
% Block 2
% Axes handle 2 (unvisible, only for place the second legend)
ah2=axes('position',get(gca,'position'), 'visible','off');
% Legend at axes 2
ledk2 = legend(ah2,[k2p,k3p,k4p],'$k(t)$ - Cycle 3','$k(t)$ - Cycle 4','$k(t)$ - Cycle 5',[1 1 1 1])
ledk2.FontSize = 40;
set(ledk2,'Interpreter','latex');


figure(4)
%{
bpe = plot(t(1),ent0(1),'ko','MarkerSize',10), hold on
plot(t(2),ent1(1),'ko','MarkerSize',10), hold on
plot(t(3),ent2(1),'ko','MarkerSize',10), hold on
plot(t(4),ent3(1),'ko','MarkerSize',10), hold on
plot(t(5),exit4(1),'ko','MarkerSize',10), hold on
%}
e0p = plot(t_mesh0(1),ent0(1),'-bd','MarkerSize',20), hold on
e1p = plot(t_mesh1(1),ent1(1),'-bd','MarkerSize',20), hold on
e2p = plot(t_mesh2(1),ent2(1),'-bd','MarkerSize',20), hold on
e3p = plot(t_mesh3(1),ent3(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
x3p = plot(t_mesh3(1),exit3(1),'-rs','MarkerSize',20), hold on
x4p = plot(t_mesh4(1),exit4(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
e2p = plot(t_mesh2(1),ent2(1),'-bd','MarkerSize',22), hold on
e3p = plot(t_mesh3(1),ent3(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
x3p = plot(t_mesh3(1),exit3(1),'-rs','MarkerSize',22), hold on
x4p = plot(t_mesh4(1),exit4(1),'-rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e0plt = plot(t_mesh0,ent0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:4000:end),ent0(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:end),ent0(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e1plt = plot(t_mesh1,ent1,'b-','LineWidth',3), hold on
plot(t_mesh1(1:4000:end),ent1(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh1(1:4000:end),ent1(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e2plt = plot(t_mesh2,ent2,'b-','LineWidth',3), hold on
plot(t_mesh2(1:4000:end),ent2(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh2(1:4000:end),ent2(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e3plt = plot(t_mesh3,ent3,'b-','LineWidth',3), hold on
plot(t_mesh3(1:4000:end),ent3(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh3(1:4000:end),ent3(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1plt = plot(t_mesh1,exit1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:4000:end),exit1(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:4000:end),exit1(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x2plt = plot(t_mesh2,exit2,'r-','LineWidth',3), hold on
plot(t_mesh2(1:4000:end),exit2(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh2(1:4000:end),exit2(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x3plt = plot(t_mesh3,exit3,'r-','LineWidth',3), hold on
plot(t_mesh3(1:4000:end),exit3(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh3(1:4000:end),exit3(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x4plt = plot(t_mesh4,exit4,'r-','LineWidth',3), hold on
plot(t_mesh4(1:4000:end),exit4(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh4(1:4000:end),exit4(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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([-.25e06 4e06])
xlim([t(1) t_mesh4(end)])
grid on
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex')
lede = legend([e0p,x1p],'$e(t)$ - Entry Rate','$x(t)$ - Exit Rate')
lede.FontSize = 40;
set(lede,'Interpreter','latex');


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
x2p = plot(t_mesh2(1),xx2(1),'-gx','MarkerSize',20), hold on
x3p = plot(t_mesh3(1),xx3(1),'-mv','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(3),xx2(1),'ko','MarkerSize',20), hold on
plot(t(4),xx3(1),'ko','MarkerSize',20), hold on
plot(t(2),xx1(1),'ko','MarkerSize',22), hold on
plot(t(3),xx2(1),'ko','MarkerSize',22), hold on
plot(t(4),xx3(1),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0plt = plot(t_mesh0,xx0,'b-','LineWidth',3), hold on
plot(t_mesh0(1:4000:end),xx0(1:4000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0(1:4000:end),xx0(1:4000:end),'bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1plt = plot(t_mesh1,xx1,'r-','LineWidth',3), hold on
plot(t_mesh1(1:4000:end),xx1(1:4000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1(1:4000:end),xx1(1:4000:end),'rs','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x2plt = plot(t_mesh2,xx2,'g-','LineWidth',3), hold on
plot(t_mesh2(1:4000:end),xx2(1:4000:end),'gx','MarkerSize',20), hold on
plot(t_mesh2(1:4000:end),xx2(1:4000:end),'gx','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x3plt = plot(t_mesh3,xx3,'m-','LineWidth',3), hold on
plot(t_mesh3(1:4000:end),xx3(1:4000:end),'mv','MarkerSize',20), hold on
plot(t_mesh3(1:4000:end),xx3(1:4000:end),'mv','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
title('Trip Length','Interpreter','latex')
ylabel('Distance (miles)','Interpreter','latex')
xlabel('Clock Time','Interpreter','latex')
xlim([t(1) t_mesh4(end)])
ylim([-.2 6])
grid on
set(gca,'fontsize',50,'TickLabelInterpreter', 'latex')
% Block 1
% Axes handle 1 (this is the visible axes)
ah1 = gca;
% Legend at axes 1
ledx1 = legend(ah1,[bpx,x0p,x1p],'Break Points','Distance - Cycle 1','Distance - Cycle 2',[0 0 0 0])
ledx1.FontSize = 40;
set(ledx1,'Interpreter','latex');
% Block 2
% Axes handle 2 (unvisible, only for place the second legend)
ah2=axes('position',get(gca,'position'), 'visible','off');
% Legend at axes 2
ledx2 = legend(ah2,[x2p,x3p],'Distance - Cycle 3','Distance - Cycle 4',[1 1 1 1])
ledx2.FontSize = 40;
set(ledx2,'Interpreter','latex');


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

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