% This is a macro used to create the plots for the mag-lev application
% in chapter 12

% Simulation of a magnetically levitated beam
%
% Valid methods
% - sfb
% - asfb
% - aofb

% Clear memory
clear all

global tnext;  % used to help display the current time

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


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


% Define the system size
n = 4;  % number of plant states
p = 20;  % number of adjustable parameters

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

cs(1).param = 'eps_eta';
cs(1).value = 0.1;
cs(1).fign = 3;
cs(1).lnsty = '-.';

cs(2).param = 'eps_eta';
cs(2).value = 0.01;
cs(2).fign = 3;
cs(2).lnsty = ':';

cs(3).param = 'eps_eta';
cs(3).value = 0.001;
cs(3).fign = 3;
cs(3).lnsty = '-';

cs(4).param = 'method';
cs(4).value = 'asfb';
cs(4).fign = 4;
cs(4).lnsty = '-';

cs(5).param = 'method';
cs(5).value = 'sfb';
cs(5).fign = 4;
cs(5).lnsty = ':';

hcs13 = [];
hcs45 = [];

figure(1); clf; hold off;
figure(2); clf; hold off;
figure(3); clf; hold off;
figure(4); clf; hold off;
figure(5); clf; hold off;

% Loop over the case studies
for j=1:ns

  % set the simulation time display
  tnext = 0;

  % define default system/controller parameters
  x0 = [0; 0; 1; 1];
  th0 = zeros(p, 1);
  hatx0 = zeros(2,1);
  m = 1.;
  G = 0.02;
  ka = 0.1;
  kb = 0.1;
  Ra = 2.;
  Rb = 2.;
  La = 0.001;
  Lb = 0.002;

  kappa1 = 50;   % Subsystem 1 poles
  kappa2 = 400;  % Subsystem 2 poles
  rho = 1;
  epsilon = 0.1;
  b = 1;
  Gamma = 5000*eye(p);
  sigma = 0.0001;
  method = 'aofb';

  eps_eta = 0.001;
  w = 5;
  L = [2*w; w*w];

  % used to easily pass simulation data
  SysData = struct('n', n, 'p', p);
  SysData = setfield(SysData, 'x0', x0);
  SysData = setfield(SysData, 'th0', th0);
  SysData = setfield(SysData, 'm', m);
  SysData = setfield(SysData, 'G', G);
  SysData = setfield(SysData, 'ka', ka);
  SysData = setfield(SysData, 'kb', kb);
  SysData = setfield(SysData, 'Ra', Ra);
  SysData = setfield(SysData, 'Rb', Rb);
  SysData = setfield(SysData, 'La', La);
  SysData = setfield(SysData, 'Lb', Lb);
	
  SysData = setfield(SysData, 'kappa1', kappa1);
  SysData = setfield(SysData, 'kappa2', kappa2);
  SysData = setfield(SysData, 'rho', rho);
  SysData = setfield(SysData, 'epsilon', epsilon);
  SysData = setfield(SysData, 'b', b);
  SysData = setfield(SysData, 'Gamma', Gamma);
  SysData = setfield(SysData, 'sigma', sigma);
  SysData = setfield(SysData, 't_step', 0.5);
  SysData = setfield(SysData, 'step', 0.001);
  SysData = setfield(SysData, 'method', method);

  % Estimator parameters
  SysData = setfield(SysData, 'eps_eta', eps_eta);
  SysData = setfield(SysData, 'L', L);

  % 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.x0; SysData.th0; hatx0];

  % 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), 8);  % 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, 1000*y(:,1), cs(j).lnsty); hold on;
  switch (SysData.method)
    case 'aofb'
    hcs13 = [hcs13; h];
    
    otherwise
    hcs45 = [hcs45; h];
  end
end

% Finish printing the case study figures
figure(3)
h2 = plot(t, 1000*y(:,5), '--');
xlabel('t (sec)');
ylabel('x (mm)');
set([hcs13; h2], 'LineWidth', 1.5);
legend(hcs13, '\eta = 0.1', '\eta = 0.01', '\eta = 0.001');
ax = axis;
axis([ax(1) ax(2) -1.2 2])

figure(4)
h2 = plot(t, 1000*y(:,5), '--');
xlabel('t (sec)');
ylabel('x (mm)');
set([hcs45; h2], 'LineWidth', 1.5);
ax = axis;
axis([ax(1) ax(2) -1.2 2])

% Plot the position
figure(1); clf; hold off;
h1 = plot(t, 1000*y(:,1), '-'); hold on;
h2 = plot(t, 1000*y(:,5), '--');
xlabel('t (sec)');
ylabel('x (mm)');
set([h1; h2], 'LineWidth', 1.5);
ax = axis;
axis([ax(1) ax(2) -1.2 2])

% Plot the currents
figure(2); clf; hold off;
h1 = plot(t, 1000*y(:,3), '-'); hold on;
h2 = plot(t, 1000*y(:,4), '--');
xlabel('t (sec)');
ylabel('(mA)');
set([h1; h2], 'LineWidth', 1.5);
ax = axis;
axis([ax(1) ax(2) 0 300])
legend([h1; h2], 'i_a', 'i_b');


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

x = -G:0.0001:G;
x=x(:); z = zeros(size(x));
mu = zeros(length(x), p);
theta = zeros(p, 1);
for i=1:length(x)
  range = [-G; G];
  sig = (max(range) - min(range))/p;
  [z(i), dF] = rbnn(x(i), range, sig, theta);
  mu(i,:) = dF(:)';
end

indx = 1:p;
h = [];
for i=1:length(indx)
  h = [h; plot(1000*x, mu(:, indx(i)))];
end
set(h, 'LineWidth', 1.5);
xlabel('x (mm)');