% Algorithm for 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                         % Starts clock to time the algorithm

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 for the Utility Function

if util_choice == 0         % Exponential Utility Function Paramters

A0 = 5;					
b1 = 1/2;
B0 = 10;
a1 = 1/3;

elseif util_choice == 1

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

else

end

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

% Other parameters

k = 6;                                  % "latest feasible" arrival time t = 6 => 11 AM 
L = 4;                                  % exogenous trip length

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);                    % preallocation
v = zeros(1,tzeros);


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

% Maximizing utility with respect to start time

tttt = linspace(0.1,5.66666,8000);                                                  % vector of times between t = 0.1 and 5.6666

if util_choice == 0

u_max = (A0/a1)*(1 - exp(-a1*tttt)) + (B0/b1)*(1 - exp( b1*(tttt + fftt - k)));     % creates U(t,fftt), with a vector of t's
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);

% linear interpolation

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;

% Maximum utility value computation

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;
elseif util_choice == 1
	Nt = 20000;	
else

end

% Preallocation

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

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

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

xx0 = zeros(1,Nt+1);

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



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


% Gradient Method Parameters


if util_choice == 0                             % Exponential Utility Function
	tol = 10^(-5);                              % Tolerance for utility/L loops 
	if cong_hyp == 0
		u = u_max_star                          % Starting value of utility 
		t_up = t_max_u - 0.00001                % Starting value of time of the first entry
	elseif cong_hyp == 1
		%u = u_max_star - 0.00001               % Starting value of utility
		%t_up = t_max_u - 0.00001               % Starting value of time of the first entry
    else
        fprintf('ERROR: Choose congested or hypercongested region')
	end
	
elseif util_choice == 1                         % Logarithmic
	tol = 10^(-5);                              % Tolerance for utility/L loops
	if cong_hyp == 0
		u = u_max_star                         % Starting value of utility
		t_up = t_max_u - 0.00001               % Starting value of time of the first entry
% 		u = 24.434731561866130 + 0.00001		% Utility used for Figure 5.1
% 		t_up = 2.542435608285697 + 0.000005 	% Time of the first departure for Figure 5.1
	elseif cong_hyp == 1
		u = u_max_star - 0.00001               % Starting value of utility
		t_up = t_max_u - 0.00001               % Starting value of time of the first entry
    else
        fprintf('ERROR: Choose congested or hypercongested region')
	end

else

end


% Gradient Parameters

nn = 0;                                         % Index start values
nnn = 0;
nnnn = 0;

v(1) = vf;                                      % Initial Velocity
num_cycles = 2;                                 % Total Cycles for Partial Cycles 

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

% MAIN ALGORITHM


while num_cycles == 2                          % Comment out this line to create Figure 5.1 

	% 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.00001
		elseif cong_hyp == 1
			u = u + 0.00001
		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')
		
		tbar = t_up;                            % Earliest departure time t = 0 => 5 AM 

		t(1) = tbar;	

		% Computation for T(t), Tdot(t), ... derivatives, and Number of cycles
		 
		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
            fprintf('ERROR: Choose congested or hypercongested region')
        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);

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

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


		else

		end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
		% Entry Rate Computation

		ent0(1) = -(kj/vf)*((vel0(1)*Tpp_mesh0(1))/((1+Tp_mesh0(1))*(2+Tp_mesh0(1))));

		dens0(2) = ent0(1)*deltat + dens0(1);
		vel0(2) = deltat*(-vf/kj)*ent0(1) + vel0(1);


		for j = 1:Nt-1			
			ent0(j+1) = -(kj/vf)*((vel0(j+1)*Tpp_mesh0(j+1))/((1+Tp_mesh0(j+1))*(2+Tp_mesh0(j+1))));
			dens0(j+2) = ent0(j+1)*deltat + dens0(j+1);
			vel0(j+2) = deltat*(-vf/kj)*ent0(j+1) + vel0(j+1);
		end
			ent0(Nt+1) = -(kj/vf)*((vel0(Nt+1)*Tpp_mesh0(Nt+1))/((1+Tp_mesh0(Nt+1))*(2+Tp_mesh0(Nt+1))));

		% Linear Interpolation for Density

		temp_ent = find(kbp-dens0<0);

		if length(temp_ent) < 1
			break
		end

		closestValueL_ent = dens0(temp_ent(1)-1);
		closestValueR_ent = dens0(temp_ent(1));

		slope_int_ent = (closestValueR_ent - closestValueL_ent)/(t_mesh0(temp_ent(1)) - t_mesh0(temp_ent(1)-1) );
		yint_ent = closestValueL_ent - slope_int_ent*t_mesh0(temp_ent(1)-1);

		t_lent = (kbp-yint_ent)/slope_int_ent;

		% Linear Interpolation for Velocity

		closestValueL_vel = vel0(temp_ent(1)-1);
		closestValueR_vel = vel0(temp_ent(1));

		slope_int_vel = (closestValueR_vel - closestValueL_vel)/(t_mesh0(temp_ent(1)) - t_mesh0(temp_ent(1)-1) );
		yint_vel = closestValueL_vel - slope_int_vel*t_mesh0(temp_ent(1)-1);

		vel_lent = slope_int_vel*t_lent + yint_vel;

		% New mesh vectors 

		t_mesh0_update = [t_mesh0(1:temp_ent(1)-1) t_lent t_mesh0(temp_ent(1):end)];

		if util_choice == 0

			T_mesh0_update = k - t_mesh0_update + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh0_update) ) ));
			Tp_mesh0_update = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh0_update)*p1);
			Tpp_mesh0_update = - ((a1^2)*A0*p1*exp(a1*t_mesh0_update))./((-A0*b1 + exp(a1*t_mesh0_update)*p1).^2);

			t_mesh1_update = t_mesh0_update + T_mesh0_update;
			T_mesh1_update = k - t_mesh1_update + (1/b1)*log(1 - (b1/B0)*(u - (A0/a1)*(1-exp(-a1*t_mesh1_update) ) ));
			Tp_mesh1_update = -1 + (A0*a1)./(-A0*b1 + exp(a1*t_mesh1_update)*p1);
			Tpp_mesh1_update = - ((a1^2)*A0*p1*exp(a1*t_mesh1_update))./((-A0*b1 + exp(a1*t_mesh1_update)*p1).^2);


		elseif util_choice == 1

			T_mesh0_update = k - t_mesh0_update - exp((1/b0)*(u - a0*log(a1*t_mesh0_update)));
			Tp_mesh0_update = -1 + ((a0)/(b0))*(1./t_mesh0_update).*exp((1/b0)*(u - a0*log(a1*t_mesh0_update)));
			Tpp_mesh0_update = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh0_update.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh0_update)));

			t_mesh1_update = t_mesh0_update + T_mesh0_update;
			T_mesh1_update = k - t_mesh1_update - exp((1/b0)*(u - a0*log(a1*t_mesh1_update)));
			Tp_mesh1_update = -1 + ((a0)/(b0))*(1./t_mesh1_update).*exp((1/b0)*(u - a0*log(a1*t_mesh1_update)));
			Tpp_mesh1_update = (-(a0^2)/(b0^2) - (a0)/(b0) )*(1./(t_mesh1_update.^2)).*exp((1/b0)*(u - a0*log(a1*t_mesh1_update)));


		else

		end


		% First Cylce updates

		dens0_update = [dens0(1:temp_ent(1)-1) kbp dens0(temp_ent(1):end)];
		vel0_update = [vel0(1:temp_ent(1)-1) vel_lent vel0(temp_ent(1):end)];
		ent0_update = [ent0(1:temp_ent(1)-1) 0 ent0(temp_ent(1):end)];

		vel0_update(temp_ent(1):end) = vel_lent;
		dens0_update(temp_ent(1):end) = kbp;
		ent0_update(temp_ent(1):end) = 0;

		% Distance computation

		xx0(2) = (t_mesh0_update(2)-t_mesh0_update(1))*vel0(1) + xx0(1);

		for j = 1:Nt		
			xx0(j+2) = (t_mesh0_update(j+2)-t_mesh0_update(j+1))*vel0_update(j+1) + xx0(j+1);
		end

		% Second Cycle Computation

		dens1(1) = dens0_update(end);
		vel1(1) = vel0_update(end);

		exit1(1) = ent0_update(1)/(1+Tp_mesh0_update(1));
		kdot1(1) = (-exit1(1));
		dens1(2) = kdot1(1)*(t_mesh1_update(2)-t_mesh1_update(1)) + dens1(1);
		vel1(2) = (t_mesh1_update(2)-t_mesh1_update(1))*(-vf/kj)*kdot1(1) + vel1(1);

		for j = 1:Nt

			exit1(j+1) = ent0_update(j+1)/(1+Tp_mesh0_update(j+1));
			kdot1(j+1) = (-exit1(j+1));
			dens1(j+2) = kdot1(j+1)*(t_mesh1_update(j+2)-t_mesh1_update(j+1)) + dens1(j+1);
			vel1(j+2) = (t_mesh1_update(j+2)-t_mesh1_update(j+1))*(-vf/kj)*(kdot1(j+1)) + vel1(j+1);

		end

		exit1(Nt+2) = ent0_update(Nt+2)/(1+Tp_mesh0_update(Nt+2));


		% Error computation at each time step

		error_count1(nnn+1) = abs(xx0(end) - L);
		%gamman = 0.0000001; % 0.00001  Alternative time refinement 
		nnn = nnn + 1;

		% Fineness scale for time of the first entry

		if util_choice == 0

			if cong_hyp == 0
				%t_up = t_up - gamman*vf*(-(- ((a1^2)*A0*p1*exp(a1*t_up))/((-A0*b1 + exp(a1*t_up)*p1)^2)))/((1 -1 + (A0*a1)/(-A0*b1 + exp(a1*t_up)*p1))^3)  
				t_up = t_up - 0.000001
			elseif cong_hyp == 1
				%t_up = t_up + gamman*vf*(-(- ((a1^2)*A0*p1*exp(a1*t_up))/((-A0*b1 + exp(a1*t_up)*p1)^2)))/((1 -1 + (A0*a1)/(-A0*b1 + exp(a1*t_up)*p1))^3)
				t_up = t_up + 0.000001
			else
			end

		elseif util_choice == 1

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

		end

	end   % end of while loop for L loop
		
    % Save correct value of time of first entry

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

	% Number of Commuters (N) Computation
	
	% First Subcycle

	for j = 1:temp_ent(1)
		N_com(j+1) = ent0_update(j)*(t_mesh0_update(j+1)-t_mesh0_update(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);

	nnnn = nnnn + 1


end 			% end of outer utility loop, comment out for one value of utility


toc             % end of computing algorithm time

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

% 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:100:end),TTTT(1:100:end),'ko','MarkerSize',20), hold on
pT = plot(tttt(1),TTTT(1),'-ko','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tp_plt = plot(tttt,TTTTp,'-r','LineWidth',3), hold on
plot(tttt(1:100:end),TTTTp(1:100:end),'rs','MarkerSize',20), hold on
pTp = plot(tttt(1),TTTTp(1),'-rs','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TTp_plt = plot(tttt,TTTTpp,'-b','LineWidth',3), hold on
plot(tttt(1:100:end),TTTTpp(1:100:end),'bd','MarkerSize',20), hold on
pTTp = plot(tttt(1),TTTTpp(1),'-bd','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%tT_plt = plot(tttt,tttt+TTTT,'b-'), hold off
legend([pT,pTp,pTTp],'T(t)','T`(t)','T``(t)');
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_mesh1(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


% Velocity Plot

figure(2)
bpv = plot(t(1:3),v(1:3),'ko','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v0plt = plot(t_mesh0_update,vel0_update,'b-','LineWidth',3), hold on
plot(t_mesh0_update(1:2000:end),vel0_update(1:2000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0_update(1:2000:end),vel0_update(1:2000:end),'bd','MarkerSize',22), hold on
v0p = plot(t_mesh0_update(1),vel0_update(1),'-bd','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v1plt = plot(t_mesh1_update,vel1,'r-','LineWidth',3), hold on
plot(t_mesh1_update(1:2000:end),vel1(1:2000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1_update(1:2000:end),vel1(1:2000:end),'rs','MarkerSize',22), hold on
v1p = plot(t_mesh1_update(1),vel1(1),'-rs','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
leg1 = legend([bpv,v0p,v1p],'Break points','$v(t)$ - Cycle 1','$v(t)$ - Cycle 2');
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([14.2 15.4])
xlim([t(1) t_mesh1(end)]);
grid on

% Density Plot

figure(3)
bpk = plot(t(1:3),dens_bp(1:3),'ko','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k0plt = plot(t_mesh0_update,dens0_update,'b-','LineWidth',3), hold on
plot(t_mesh0_update(1:2000:end),dens0_update(1:2000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0_update(1:2000:end),dens0_update(1:2000:end),'bd','MarkerSize',22), hold on
k0p = plot(t_mesh0_update(1),dens0_update(1),'-bd','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k1plt = plot(t_mesh1_update,dens1,'r-','LineWidth',3), hold on
plot(t_mesh1_update(1:2000:end),dens1(1:2000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1_update(1:2000:end),dens1(1:2000:end),'rs','MarkerSize',22), hold on
k1p = plot(t_mesh1_update(1),dens1(1),'-rs','MarkerSize',20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
legk = legend([bpk,k0p,k1p],'Break points','$k(t)$ - Cycle 1','$k(t)$ - Cycle 2');
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');
ylim([-0.5e04 5e04]);
xlim([t(1) t_mesh1(end)]);
grid on

% Entry and Exit Rate Plot

figure(4)
bpe = plot(t(1),ent0_update(1),'ko','MarkerSize',20), hold on
bpe = plot(t(1),ent0_update(1),'ko','MarkerSize',22), hold on
plot(t(2),exit1(1),'ko','MarkerSize',20), hold on
plot(t(2),exit1(1),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e0plt = plot(t_mesh0_update,ent0_update,'b-','LineWidth',3), hold on
plot(t_mesh0_update(1:2000:end),ent0_update(1:2000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0_update(1:2000:end),ent0_update(1:2000:end),'bd','MarkerSize',22), hold on
e0p = plot(t_mesh0_update(1),ent0_update(1),'-bd','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e1plt = plot(t_mesh1_update,exit1,'r-','LineWidth',3), hold on
plot(t_mesh1_update(1:2000:end),exit1(1:2000:end),'rs','MarkerSize',20), hold on
plot(t_mesh1_update(1:2000:end),exit1(1:2000:end),'rs','MarkerSize',22), hold on
e1p = plot(t_mesh1_update(1),exit1(1),'-rs','MarkerSize',20), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lege = legend([bpe,e0p,e1p],'Break points','$e(t)$ - Cycle 1','$x(t)$ - Cycle 2')
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([0 4.3*10^5])
xlim([t(1) t_mesh1(end)]);
grid on

% Travel Distance Plot

figure(5)
bpx = plot(t(1),xx0(1),'ko','MarkerSize',20), hold on
bpx = plot(t(1),xx0(1),'ko','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0plt = plot(t_mesh0_update,xx0,'b-','LineWidth',3), hold on
plot(t_mesh0_update(1:2000:end),xx0(1:2000:end),'bd','MarkerSize',20), hold on
plot(t_mesh0_update(1:2000:end),xx0(1:2000:end),'bd','MarkerSize',22), hold on
x0p = plot(t_mesh0_update(1),xx0(1),'-bd','MarkerSize',20), hold on
x0p = plot(t_mesh0_update(1),xx0(1),'-bd','MarkerSize',22), hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
legx = legend([bpx,x0p],'Break points','Distance Traveled')
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_mesh1(end)]);
grid on

% % Utility Function
% 
% figure(6)
% plot(tttt,u_max,'b-'), hold on
% plot(tttt,u_max_prime,'r-'), hold on 
% title('Utility Function')
% %ylim([-.5*10^5 10^6])
% grid on
% 
% % Error Plot
% 
% figure(7)
% err1 = plot([0:1:nnn-1],error_count1,'b-'), hold on
% %err2 = plot([0:1:nnn],error_count2,'r-'), hold on
% title('Error for utility and break point')
% %legend([err1,err2],'Breakpoint Error','Utility Error')
% xlabel('Number of Iterations')
% %ylabel('Velocity, v(t)')
% %ylim([-.5*10^5 10^6])
% grid on
% 
% % N vs. Utility
% 
% figure(8)
% plot(N_com_matrix,u_matrix,'k'), hold on 
% title('Utility vs Number of commuters')
% %ylim([-.5*10^5 10^6])
% grid on
% 
% % t_underbar
% 
% figure(9)
% plot(t_matrix,u_matrix,'k'), hold on 
% title('Utility vs time (G(t,u) function)')
% %ylim([-.5*10^5 10^6])
% grid on