% This is a macro used to create the plots for Example 8.4
% - simulation of an antenna with friction

% Clear memory
clear all

global tnext;  % used to help display the current time

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


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

% Define the system size
n = 3;  % number of plant states
p = 13;  % 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 = 0;
cs(1).fign = 2;
cs(1).lnsty = '-';

cs(2).param = 'adapt';
cs(2).value = 1;
cs(2).fign = 3;
cs(2).lnsty = '-';

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

% Loop over the case studies
h = [];
for j=1:ns
  tnext = 0;   % used to dispay the current time

  % define default system/controller parameters
  xi0 = zeros(n, 1);
  th0 = zeros(p, 1);
  th0(3) = 1/20;
  kappa = 5;
  eta = 2;
  b_th = 100^2;
  sigma = 1/(b_th);
  Gamma = 100*eye(p)/sigma;
  Gamma(1,1) = 0.1/sigma;
  Gamma(2,2) = 0.1/sigma;
  Gamma(3,3) = 0.1/sigma;
  
  % 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, 'Vmax', 7);
  SysData = setfield(SysData, 'adapt', 1);

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

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

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

  % Define the system outputs
  y = zeros(length(t), 5);  % 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, 180*y(:,1)/pi, '-'); hold on;
  h = [h; plot(t, 180*y(:,4)/pi, ':')];
  set(h, 'LineWidth', 1.5);

end

% without adaptation
figure(2);
axis([0, tf, -12, 12]);
xlabel('t');
ylabel('\theta', 'Rotation', 0);
eval(sprintf('print -deps %s_b\n', exname));  % print out the results


% with adaptation
figure(3);
axis([0, tf, -12, 12]);
xlabel('t');
ylabel('\theta', 'Rotation', 0);
eval(sprintf('print -deps %s_c\n', exname));  % print out the results


% plot the friction
fprintf('Calculating friction...\n');

nits = 2000;   % used for plotting not simulation step size
t = t0:(tf-t0)/nits:tf;

x0 = [SysData.xi0; SysData.th0];
[t, x] = ode23(sysname, t, x0, opts, SysData, 'friction');
figure(1); clf
h = plot(x(:,2), x(:,3));
set(h, 'LineWidth', 1.5);
xlabel('\omega (rad/sec)');
ylabel('q', 'Rotation', 0);
eval(sprintf('print -deps %s_a\n', exname));  % print out the results


% calculate theta
T_f = 40;   % Dahl friction
T_c = 10;
phi = pi/2;
J = 20;
th1 = T_c*sin(phi)/J;
th2 = T_c*cos(phi)/J;
th3 = 1/J;
theta = [th1; th2; th3; T_f^2*ones(p-3, 1)/(J*J)];
fprintf('  |theta| = %f\n', norm(theta));