% Algorithm for TWO 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 = 0;			% 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;

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,5.5,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 = 40000; % First seg
    Nt = 185000; % Second seg
elseif util_choice == 1
	%Nt = 30000; % first segment
	Nt = 185000; % log	
else

end

% Preallocation 

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

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

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

exit1 = zeros(1,Nt+1);
exit2 = zeros(1,Nt+1);
exit3 = 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);

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

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

% Break Point Information

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


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

% Gradient Method Parameters

if util_choice == 0				% Exponential Utility Function
	tol = 10^(-3)				% Tolerance for utility/L loops (10^-4 for cong)
	if cong_hyp == 0
		u = 24.345060000004690 		% Starting value of utility
		t_up = 2.256291539708880	% First point
	elseif cong_hyp == 1
		%u = 16-0.001               % Starting value of utility; tol = -3, u 0.001, t 0.00001, 10000
		%t_up = -0.5040             % Starting value of time of the first entry
		%u = -22 - 0.0001           % Starting value of utility; tol = -3, u 0.001, t 0.00001
		%t_up = -3.9831             % Starting value of time of the first entry
        
		%u = 20.708160000001897
		%t_up = 0.529133444070187
        
        u = 12.930159999997844 + 0.001
        t_up = -1.009840893626025
	else
	end
	
elseif util_choice == 1                     % Logarithmic Utility Function
	tol = 10^(-3);                          % Tolerance for utility/L loops
	if cong_hyp == 0
		u = 24.10795 + 0.0001               % Starting value of utility
		t_up = 2.2789                       % Starting value of time of the first entry
	elseif cong_hyp == 1
		%u = -4.42 - 0.01                   % Starting value of utility
		%t_up = 0.225                       % Starting value of time of the first entry
		%u = 15.6815 + 0.001                % Starting value of utility
		%t_up = 0.91973 + 0.000001          % Starting value of time of the first entry
		%u = 13.685500000001106 + 0.001     % Starting value of utility
		%t_up = 0.801481611267895           % Starting value of time of the first entry
		%u = 4.112500000002958 + 0.01       % Starting value of utility
		%t_up = 0.408178934002925           % Starting value of time of the first entry
		u = 1.715500000003173 + 0.001       % Starting value of utility
		t_up = 0.345197889160610 + 0.0001   % Starting value of time of the first entry
		%u = -4.42 - 0.001                  % Starting value of utility
		%t_up = 0.225747010000002           % Starting value of time of the first entry
	else
	end

else

end


% Gradient Parameters

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

v(1) = vf;				% Initial Velocity
num_cycles = 4;				% Total Cycles for FFP



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

% MAIN METHOD

%while num_cycles == 4

	% Fineness scale for the G(t,u) and H(t,u,L) Plots	

	if util_choice == 0
		if cong_hyp == 0
			u = u - 0.0001
		elseif cong_hyp == 1
			u = u - 0.001
		else
		end
		
	elseif util_choice == 1
		if cong_hyp == 0
			u = u - 0.0001
		elseif cong_hyp == 1
			u = u - 0.001
		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','N_com2')
		
		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

        % density breakpoints

		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;

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


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

		else

		end

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

		for j = 1:Nt

		ENT_temp = zeros(3);
		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));

		b_mat = zeros(3,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)));
            
		ENT = (vf/kj).*ENT_temp\b_mat; 


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

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

		end


		% Last Cycle - Exits Only

		exit3(1) = ent2(1)/(1+Tp_mesh2(1));
		kdot3(1) = (-exit3(1));
		dens3(2) = kdot3(1)*(t_mesh3(2)-t_mesh3(1)) + dens3(1);
		vel3(2) = (t_mesh3(2)-t_mesh3(1))*(-vf/kj)*(kdot3(1)) + vel3(1);

		for j = 1:Nt

			exit3(j+1) = ent2(j+1)/(1+Tp_mesh2(j+1));
			kdot3(j+1) = (-exit3(j+1));
			dens3(j+1) = kdot3(j)*(t_mesh3(j+1)-t_mesh3(j)) + dens3(j);
			vel3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*(-vf/kj)*(kdot3(j)) + vel3(j);

		end


		t_index_temp = find(vf-vel3<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(3-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));

		b_mat1 = zeros(3-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)));

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

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

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

		end

		ENT_temp1 = zeros(3-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));


		b_mat1 = zeros(3-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)));

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

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

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

		exit3(1) = ent2(1)/(1+Tp_mesh2(1));
		kdot3(1) = (-exit3(1));
		dens3(2) = kdot3(1)*(t_mesh3(2)-t_mesh3(1)) + dens3(1);
		vel3(2) = (t_mesh3(2)-t_mesh3(1))*(-vf/kj)*(kdot3(1)) + vel3(1);

		for j = 1:Nt

			exit3(j+1) = ent2(j+1)/(1+Tp_mesh2(j+1));
			kdot3(j+1) = (-exit3(j+1));
			dens3(j+1) = kdot3(j)*(t_mesh3(j+1)-t_mesh3(j)) + dens3(j);
			vel3(j+1) = (t_mesh3(j+1)-t_mesh3(j))*(-vf/kj)*(kdot3(j)) + vel3(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.025*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up - 0.00001
			elseif cong_hyp == 1
				gamma_save = 0.0075*( xx0(end)-L );
				t_up = t_up - gamma_save
			else
			end

		elseif util_choice == 1

			if cong_hyp == 0
				gamma_save = 0.025*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up - 0.00001
			elseif cong_hyp == 1
				gamma_save = 0.0075*( xx0(end)-L );
				t_up = t_up - gamma_save
				%t_up = t_up + 0.000001
			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);
	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);
	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;
	N_com_matrix(nnnn+1) = N_com(end) + N_com1(end) + N_com2(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-'), hold on
Tp_plt = plot(tttt,TTTTp,'r-'), hold on
TTp_plt = plot(tttt,TTTTpp,'g-'), hold on
tT_plt = plot(tttt,tttt+TTTT,'b-'), hold off
legend([T_plt,Tp_plt,TTp_plt,tT_plt],'T(t)','T`(t)','T``(t)','t+T(t)')
title('Travel Time T(t) and Derivatives')
xlim([-.1 5])
ylim([-20 20])
grid on


figure(2)
plot(t(1:5),v(1:5),'ko'), hold on
v0 = plot(t_mesh0,vel0,'k-'), hold on
v1 = plot(t_mesh1,vel1,'r-'), hold on
v2 = plot(t_mesh2,vel2,'g-'), hold on
v3 = plot(t_mesh3,vel3,'m-'), hold on
legend([v0,v1,v2,v3],'Cycle 1','Cycle 2','Cycle 3','Cycle 4')
title('Velocity')
ylim([-12 16])
grid on

figure(3)
plot(t(1:4),dens_bp(1:4),'ko'), hold on
k0 = plot(t_mesh0,dens0,'k-'), hold on
k1 = plot(t_mesh1,dens1,'r-'), hold on
k2 = plot(t_mesh2,dens2,'g-'), hold on
k3 = plot(t_mesh3,dens3,'m-'), hold on
legend([k0,k1,k2,k3],'Cycle 1','Cycle 2','Cycle 3','Cycle 4')
title('Density')
%ylim([-.2*10^5 6*10^4])
%xlim([2.45 2.9])
grid on

figure(4)
% Entrt Rate
e0 = plot(t_mesh0,ent0,'b-'), hold on
plot(t_mesh1,ent1,'b-'), hold on
plot(t_mesh2,ent2,'b-'), hold on
% Exit Rate
x0 = plot(t_mesh1,exit1,'r-'), hold on
plot(t_mesh2,exit2,'r-'), hold on
plot(t_mesh3,exit3,'r-'), hold on
legend([e0,x0],'Entry Rate e(t)','Exit Rate x(t)')
title('Entry Rate e(t), Exit Rate x(t)')
%ylim([-.5*10^5 10^6])
grid on


figure(5)
d0 = plot(t_mesh0,xx0,'k-'), hold on
d1 = plot(t_mesh1,xx1,'r-'), hold on
d2 = plot(t_mesh2,xx2,'g-'), hold on
d3 = plot(t_mesh3,xx3,'m-'), hold on
legend([d0,d1,d2,d3],'Cycle 1','Cycle 2','Cycle 3','Cycle 4')
title('Distance traveled x(t)')
%ylim([-.5*10^5 10^6])
grid on


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