%   
% 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     = 4.; 
x_10    = 0.; % the initial conditions
x_20    = 0.;

x_1d    = 1.; % the target goals

q = 1; 
r = 1; 

omega_n = 6.28;
xi = 0.707;

% Part (a): find the optimal value for [c_1,c_2]: 
%  
cs = fminsearch( @(x) sect_3_prob_5_J_min_fn(x(1), x(2), t_0, t_f, x_1d, q, r, omega_n, xi), [ 0., 0. ] );
cs

c_1 = cs(1); c_2 = cs(2); 

% Evaluate J for these values of c_1 and c_2:
%
% First integate the given differential equations from t=0 to t=t_f: 
% 
sol = ode45( @(t,x) sect_3_prob_5_J_ode_fn(t,x, c_1,c_2, omega_n,xi,x_1d), [t_0,t_f], [0;0] );

t_t  = sol.x;
x_1t = sol.y(1,:); % extract x_1(t)
x_2t = sol.y(2,:); 
u_t = -c_1 * ( x_1t - x_1d ) - c_2 * x_2t; % compute u(t) 

% Second evaluate the expression for J: 
% 
J_part1 = q * ( x_1t(end) - x_1d )^2;
J_part2 = r * trapz( t_t, u_t.^2 );
J       = 0.5 * ( J_part1 + J_part2 )

J_a = J; 
t_t_a = t_t; 
u_t_a = u_t; 

ph_a = plot( t_t, u_t, '-' ); 
xlabel('time'); ylabel('u(t)'); 

% Part (b): find the optimal value for [c_1]: 
%  
cs = fminsearch( @(x) sect_3_prob_5_J_min_fn(x(1), 0., t_0, t_f, x_1d, q, r, omega_n, xi), [ 0. ] );
cs

c_1 = cs(1); c_2 = 0.;

% Evaluate J for these values of c_1 and c_2:
%
% First integate the given differential equations from t=0 to t=t_f: 
% 
sol = ode45( @(t,x) sect_3_prob_5_J_ode_fn(t,x, c_1,c_2, omega_n,xi,x_1d), [t_0,t_f], [0;0] );

t_t  = sol.x;
x_1t = sol.y(1,:); % extract x_1(t)
x_2t = sol.y(2,:); 
u_t = -c_1 * ( x_1t - x_1d ) - c_2 * x_2t; % compute u(t) 

% Second evaluate the expression for J: 
% 
J_part1 = q * ( x_1t(end) - x_1d )^2;
J_part2 = r * trapz( t_t, u_t.^2 );
J       = 0.5 * ( J_part1 + J_part2 )

J_b = J; 
t_t_b = t_t; 
u_t_b = u_t; 

hold on; ph_b = plot( t_t, u_t, '-r' ); 
xlabel('time'); ylabel('u(t)'); 
legend([ph_a,ph_b], {'u(t) Part (a)','u(t) Part (b)'} ); 

%saveas( gcf, '../../WriteUp/Graphics/Chapter3/sect_3_prob_5_ut_plots', 'epsc' );