%   
% 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; 
x10     = xair; x20 = xair; % the initial conditions on x_1(t) and x_2(t)
T1      = 40; T2=T1;
k1      = 1.0; 
k2      = 1.0;
c1      = 1.0; 
c2      = 1.0;

% find the optimal value for [lambda_0]: 
%  
lambda_0 = [ -100., -100.]; % the initial guess at the values of lambda_1(0) and lambda_2(0)
lambda = fminsearch( @(x) sect_5_prob_6_J_min_fn(x, t_0, t_f, x10,x20,T1,T2,xair,xd,k1,k2,c1,c2), lambda_0 ) % initial guess found via trial an error 

% with this initial value lets get the lagrangian multiplier function lambda(t) and x_1(t): 
% 
sol = ode23s( @(t,x) sect_5_prob_6_ode_fn(t,x, T1,T2,xair,xd,k1,k2,c1,c2), [t_0,t_f], [x10;x20;lambda(1);lambda(2)] );

figure; 
h1 = plot( sol.x, sol.y(1,:), '-bo' ); hold on; 
h2 = plot( sol.x, sol.y(2,:), '-ro' ); 
xlabel('t'); ylabel('x(t)'); legend( [h1,h2], {'x_1(t)','x_2(t)'}, 'location', 'best' );
%saveas(gcf,'../../WriteUp/Graphics/Chapter3/sect_5_prob_6_xs.eps','epsc')

figure; 
h1 = plot( sol.x, sol.y(3,:), '-bo' ); hold on; 
h2 = plot( sol.x, sol.y(4,:), '-ro' ); 
xlabel('t'); ylabel('lambda(t)'); legend( [h1,h2], {'lambda_1(t)','lambda_2(t)'}, 'location', 'best' );
%saveas(gcf,'../../WriteUp/Graphics/Chapter3/sect_5_prob_6_lambdas.eps','epsc')

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