%   
% Written by:
% -- 
% John L. Weatherwax                2006-08-28
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

close all; clc; clear; 

% set the constants of this problem: 
t_0     = 0.; 
t_f     = 12.; 
xair    = 45;
xd      = 70; 
x0      = 45; % the initial conditions on x_1(t) 
T       = 4;
k1      = 1.0; 
c1      = 5.0; c2 = 1.0; % the ratio needs to be 5 (c1 cost of errors in (x_1(t)-x_d); c2 cost of u^2)

% find the optimal initial value for lambda(t) so that we posses the known value at lambda(t_f):
%  
lambda_0 = fminsearch( @(x) sect_5_prob_5_J_min_fn(x, t_0, t_f, x0,T,xair,xd,k1,c1,c2), [ -100. ] ) % initial guess at the optimization routine was found via trial an error 

% with this initial value for lambda(t) lets get the lagrangian multiplier function lambda(t) and the state x_1(t): 
% 
sol = ode23s( @(t,x) sect_5_prob_5_ode_fn(t,x, T,xair,xd,k1,c1,c2), [t_0,t_f], [x0;lambda_0] );

figure; plot( sol.x, sol.y(1,:), '-bo' ); hold on; xlabel('t'); ylabel('x_1(t)');
%saveas(gcf,'../../WriteUp/Graphics/Chapter3/sect_5_prob_5_x1.eps','epsc')

figure; plot( sol.x, sol.y(2,:), '-bo' ); hold on; xlabel('t'); ylabel('lambda(t)');

u_t = -( k1/(2*c2) ) * sol.y(2,:);
figure; plot( sol.x, u_t, '-k.' ); hold on; xlabel('t'); ylabel('u(t)');
%saveas(gcf,'../../WriteUp/Graphics/Chapter3/sect_5_prob_5_u_t.eps','epsc')