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

% specify the parameter for this problem 
I_y = 750;
I_z = 1000;
I_x = 500;
p_0 = 20;
tau = 0.1;

% Part (a) 
%
F = [ 0, 0, 0; 
      0, 0, p_0*(I_x - I_z)/I_y; 
      0, p_0*(I_y-I_x)/I_z, 0 ];
F      
% the elementwise exponential:
exp(F)
% the matrix exponential
expm(F)
      
G = diag( 1./[I_x,I_y,I_z] );

Phi_DT = expm(F*tau);

% Evaluate Gamma(\Delta t) using \Phi(\Delta t) * ( taylor_series ) * G * \Delta t 
%
exp_sum = eye(3); ii=1;
while 1 
  delta_sign = (-1)^(ii);
  delta_sum  = delta_sign*(tau^ii)*(F^ii)/factorial(ii+1);
  if( norm(delta_sum)<1.e-6 )
    break
  end
  exp_sum = exp_sum + delta_sum;
  ii=ii+1;
end

Gamma_DT = Phi_DT * exp_sum * G * tau;


% Part (b) the integraion based on the function for of f() in dxdt = f(x,t) :
%
nSteps = 5/tau;
x_0 = [0,0.1,0];% the initial conditions
t_0 = 0.0; % the initial time
n = 3;

t_history = zeros(nSteps+1,1);
x_history = zeros(nSteps+1,n);
t_history(1) = t_0; 
x_history(1,:) = x_0(:).';
for ii=1:nSteps,
  xim1 = x_history(ii,:)';
  tim1 = t_history(ii);
  
  xi = Phi_DT * xim1; % the integration step 
  
  t_history(ii+1) = tim1+tau;
  x_history(ii+1,:) = xi(:)';
end


fh = figure; 
ph_Dp = plot( t_history, x_history(:,1), '-r' ); hold on; 
ph_Dq = plot( t_history, x_history(:,2), '-g' ); 
ph_Dr = plot( t_history, x_history(:,3), '-b' ); 


legend( [ph_Dp,ph_Dq,ph_Dr], {'\Delta p', '\Delta q', '\Delta r'}, 'location', 'northwest' );
xlabel('time'); ylabel('\Delta x(t)');

saveas(gcf,'../../WriteUp/Graphics/Chapter2/sect_2_3_prob_5.eps','epsc');