% This is a macro used to create the plots for Example 8.1
% - simulation of a spacecraft with a nozzle fault

% Clear memory
clear all

% Define the input file names
exname = 'sfbindir_ex1';    % the name of this macro
sysname = [exname, 'sys'];

rad2deg = 180/pi; % convert radians to degrees

% The simulation time (and step size for plotting)
t0 = 0;
tf = 10;
nits = 200;   % used for plotting not simulation step size
t = t0:(tf-t0)/nits:tf;

% Define the system size
n = 6;  % number of plant states
p = 8;  % number of approximator (controller) states


% This is what to vary for each case-study
ns = 2;  % number of studies
study = struct('param', 'name', 'value', 0, 'fign', 0, 'lnsty', '-');
cs(ns) = study;

% define the case-studies
cs(1).param = 'adapt';
cs(1).value = 1;
cs(1).fign = 2;
cs(1).lnsty = '-';

cs(2).param = 'adapt';  % turn off adaptation
cs(2).value = 0;
cs(2).fign = 4;
cs(2).lnsty = '-';


% Clear the figures
figure(1); clf
figure(2); clf
figure(3); clf
figure(4); clf
figure(5); clf

% Loop over the case studies
h = [];
for j=1:ns

  % Reset values according to the current case-study
  eval(sprintf('SysData.%s = cs(j).value;', cs(j).param));
  % define default system/controller parameters
  xi0 = zeros(n, 1);
  th0 = zeros(p, 1);
  kappa = 2;
  b_w = (10)^2;     % bound on representation error
  b_th = (500)^2;  % bound on initialization error
  sigma = 1/(4*b_th);
  Gamma = 4*eye(p)/sigma;
  eta = 2*b_w;
  adapt = 1;
  J =[100 -10 40;-10 150 30;40 30 150];
  
  % used to easily pass simulation data
  SysData = struct('n', n, 'p', p);
  SysData = setfield(SysData, 'xi0', xi0);
  SysData = setfield(SysData, 'th0', th0);
  SysData = setfield(SysData, 'kappa', kappa);
  SysData = setfield(SysData, 'eta', eta);
  SysData = setfield(SysData, 'Gamma', Gamma);
  SysData = setfield(SysData, 'sigma', sigma);
  SysData = setfield(SysData, 'adapt', Gamma);
  SysData = setfield(SysData, 'J', J);

  % Reset values according to the current case-study
  eval(sprintf('SysData.%s = cs(j).value;', cs(j).param));

  % Define the system initial conditions
  x0 = [SysData.xi0; SysData.th0];

  % Run the simulation
  opts = '';
  fprintf('Running case-study #%d\n', j);
  [t, x] = ode23(sysname, t, x0, opts, SysData, 'deriv');

  % Define the system outputs
  y = zeros(length(t), 9);  % needs to be the size of the output vector
  for i=1:length(t);
    tmp = feval(sysname, t(i), x(i,:), '', SysData, 'output');
    tmp = tmp(:);  % make sure it fits
    y(i, :) = tmp';
  end

  % Plot the results
  figure(cs(j).fign);
  h = plot(t, rad2deg*y(:,1), '-'); hold on;
  h = [h; plot(t, rad2deg*y(:,2), ':')];
  h = [h; plot(t, rad2deg*y(:,3), '--')];
  set(h, 'LineWidth', 1.5);

  figure(cs(j).fign + 1);
  h = plot(t, rad2deg*y(:,4), '-'); hold on;
  h = [h; plot(t, rad2deg*y(:,5), ':')];
  h = [h; plot(t, rad2deg*y(:,6), '--')];
  set(h, 'LineWidth', 1.5);

  
end


% do a LS fit to find theta
x = -2:0.1:2;
x=x(:); F = zeros(length(x), p);
delta = 10 + 450*x./(1 + 2*abs(x));
x1 = -2.5; x2 = 2.5;
range = [x1; x2];
sig = (max(range) - min(range))/p;
for i=1:length(x);
  [y, mu] = rbnn(x(i), range, sig, zeros(p, 1));
  F(i,:) = mu(:)';
end
theta = inv(F'*F)*F'*delta;

% Plot the neural network basis functions
figure(1); clf; hold on
d = (x2 - x1)/(p-1);
xc = (0:(p-1))*d + x1;
xc = xc(:);  % make sure its a column-vector

x = -2.5:0.01:2.5;
x=x(:); y = zeros(size(x));
delta = 10 + 450*x./(1 + 2*abs(x));
mu = zeros(length(x), p);
for i=1:length(x)
  range = [x1; x2];
  sig = (max(range) - min(range))/p;
  [y(i), dF] = rbnn(x(i), range, sig, theta);
  mu(i,:) = dF(:)'; 
end

indx = 1:p;
h = [];
for i=1:length(indx)
  h = [h; plot(x, mu(:, indx(i)))];
end
set(h, 'LineWidth', 1.5);
xlabel('d\phi/dt');
eval(sprintf('print -deps %s_a\n', exname));  % print out the results


% calculate the values for representation error and theta(0)
indx = find((x >= -2.) & (x <= 2.));
W_d = max(abs(y(indx) - delta(indx)));
fprintf('bound on representation error : %f\n', W_d);
fprintf('bound on theta : %f\n', norm(theta));


% adaptation - angular position
figure(2);
axis([0, tf, -2, 2]);
xlabel('t');

eval(sprintf('print -deps %s_b\n', exname));  % print out the results


% adaptation - angular rates
figure(3);
axis([0, tf, -0.5, 0.5]);
xlabel('t');

eval(sprintf('print -deps %s_c\n', exname));  % print out the results


% static - angular position
figure(4);
axis([0, tf, -2, 2]);
xlabel('t');

eval(sprintf('print -deps %s_d\n', exname));  % print out the results