% This is a macro used to create the plots for Example 7.4
% -- Cruise control for an automobile

% Clear memory
clear all

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


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


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


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

% define the case-studies
cs(1).param = 'blank';
cs(1).value = 0;
cs(1).fign = 3;    % run in the 2nd window
cs(1).lnsty = '-';

cs(2).param = 'fs_flag';
cs(2).value = 0;
cs(2).fign = 3;    % run in the 2nd window
cs(2).lnsty = '-.';


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

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

  % define default system/controller parameters
  xi0 = [10];
  th0 = zeros(p, 1);
  kappa = 1/10;
  eta = 10;
  sigma = 2e-7;
  Gamma = 2.5e6*eye(p);

  % 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, 'sigma', sigma);
  SysData = setfield(SysData, 'Gamma', Gamma);
  SysData = setfield(SysData, 'x1', 0);        % FS input range
  SysData = setfield(SysData, 'x2', 40);
  SysData = setfield(SysData, 'fs_flag', 1);   % turn adaptation on
  SysData = setfield(SysData, 'rho', 0.4);
  SysData = setfield(SysData, 'mass', 1300);

  % 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 = odeset('RelTol', 1e-5);
  fprintf('Running case-study #%d\n', j);
  [t, x] = ode45(sysname, t, x0, opts, SysData, 'deriv');

  % Define the system outputs
  y = zeros(length(t), 4+p);  % 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, y(:,1), cs(j).lnsty); hold on;
  set(h, 'LineWidth', 1.5);
end

% Plot Figure 1 (input membership functions)
figure(1);
in_var = (SysData.x1 - 5):0.05:(SysData.x2 + 5);
mu = zeros(length(in_var), p);
for i=1:length(in_var)
  tmp = feval(sysname, t(1), [in_var(i); th0], '', SysData, 'output');
  tmp = tmp(:);  % make sure it fits
  mu(i,:) = tmp((4+1):(4+p))';
end

for i=1:p
  h = plot(in_var, mu(:,i), '-'); hold on;
  set(h, 'LineWidth', 1.5);
end
axis([SysData.x1 - 5, SysData.x2 + 5, 0, 1.2])
xlabel('x (m/sec)');
h = ylabel('\mu');
set(h, 'Rotation', 0);  % dont rotate variable name

axis_h = get(1, 'Children');
set(axis_h, 'Box', 'off');
set(axis_h, 'YTick', [0; 1]);
set(axis_h, 'YTickLabel', ['0'; '1']);

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


% Plot figure 2 - the desired aerodynamic drag function
x = SysData.x1:0.02:SysData.x2;
x = x(:);
drag = SysData.rho*x.^2;

figure(2); clf;
dx = SysData.x2 - SysData.x1;
cents = SysData.x1:(dx / (p-1)):SysData.x2;
theta = SysData.rho*cents.^2;  % ideal parameter vector

drag_approx = zeros(length(x), 1);
w = zeros(length(x), 1);
for i=1:length(x)
  drag_approx(i) = fuzzy_sys(x(i), [SysData.x1; SysData.x2], theta);
end
w = drag - drag_approx;
h = plot(x, abs(w));
set(h, 'LineWidth', 1.5);
xlabel('x');
h = ylabel('|w|');
set(h, 'Rotation', 0);  % dont rotate variable name

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


% Plot Figure 3
figure(3);
indx = 1:(length(t) - 1);
h = plot(t(indx), y(indx,2), '--');
set(h, 'LineWidth', 1.5);
xlabel('t (sec)');

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


fprintf('|theta| = %f\n', norm(theta))
fprintf('lam_max(Gamma^-1)/sigma = %f\n', max(eig(inv(Gamma)))/sigma)

W = 2; ntheta = 980;
d = W^2/(4*eta) + sigma*ntheta*ntheta/2;
Vr = d/(2*kappa) + d*max(eig(inv(Gamma)))/sigma;
fprintf('d = %f\n', d);
fprintf('V_r = %f\n', Vr);