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

clear; 
close all; drawnow; 
clc; 

addpath('../Chapter1');
addpath('../Chapter3'); % holds the continious genetic algorithm code

% Parameters used in the genetic algorithm
% 
N_pop  = 500; % the population size
X_rate = 0.1; % selection rate (the percentage of the population to "keep" on each iteration)
mu     = 0.8; % the mutation rate (larger values favor a more global exploration)
max_number_of_iterations = 10000; 

% method to select mates:
%   1=> pairing from top to bottom
%   2=> random pairing
%   3=> random pairing (rank weighting)
%   4=> weighted random pairing (cost weighting)
%   5=> tournament selection 
%
parent_selection_method = 1; 

% method used to do crossover:
%   1=> single point crossover
%   2=> double point crossover 
%   3=> uniform crossover 
%   4=> blending method (3.9)
%   5=> blending method (3.13) or the books suggested method
%
crossover_method = 5;

% the linear dynamical system we want to match the output of:
%
x0 = [ 0; 1; 0; 1 ];
A_truth = [ 0, 1, 0, 0; -1, 0, 0, 0; 0, 0, 0, 1; 0, 0, 0, 0 ]; 
x_dim = size(A_truth,1); % the size of the vector {\bf X} 
N_vars = x_dim^2; % the number of variables the genetic algorithm will process
t_span = 0 : (pi/50.) : 10*pi ;
[t_truth,x_truth] = ode23s( @(t,x) linear_ode_function( x, A_truth ), t_span, x0);
xdot_truth = ( A_truth * x_truth.' ).';

% Method 1: our function handle should be able to evaluate an entire population input as a matrix of dimension [ N_pop x N_vars ] 
wfn = @(x) genetic_ode_function(x,t_truth,x_truth,x0);
bounds = repmat( [-2,+2], [N_vars,1] ); % some simple bounds on all our variables

% Method 2: our function handle should be able to evaluate an entire population input as a matrix of dimension [ N_pop x N_vars ] 
wfn = @(x) genetic_ode_function_xt_minus_Ax(x,x_truth,xdot_truth);
bounds = repmat( [-2,+2], [N_vars,1] ); % some simple bounds on all our variables

% Run our genetic algorithm and see how well we can determine our linear model coefficients A: 
% 
fprintf('Running continious genetic algorithm.  Please wait...\n');
[pop,pop_costs,best_fn_values,avg_fn_values] = continuous_GA( wfn, bounds, ...
							      N_pop, X_rate, mu, max_number_of_iterations, parent_selection_method, crossover_method );

figure();
%plot( avg_fn_values, '-or' ); hold on; 
plot( best_fn_values, '-g' );
title('Evolution of the fit of the best chromosome in the');

% Lets see how similar these two solutions are: 
%
pi = 1;
A = reshape( pop(pi,1:N_vars), [x_dim,x_dim] );
A_truth
A

[t_sample,x_sample] = ode23s( @(t,x) linear_ode_function( x, A ), t_truth, x0 );

plot3( x_truth(:,1), x_truth(:,2), x_truth(:,3), '-g' ); hold on; 
plot3( x_sample(:,1), x_sample(:,2), x_sample(:,3), '-r' );
%saveas(gcf,'../../WriteUp/Graphics/Chapter6/linear_inverse_model_attempt_2.eps','epsc');