
function mod02_02
  % Simulate automotive suspension system
  %
  % This Octave/MATLAB script was written to mimic a corresponding
  % Labview module (Module 2.2) that accompanies the free text 
  % "Signals and Systems: Theory and Applications" by Ulaby and Yagle
  % (ss2.eecs.umich.edu).
  %
  % In almost all cases the compuation code used in these scripts
  % is the same as that used in the Labview module.  Credit for the
  % computation code should go to the textbook authors.
  
  % These scripts have been tested under Octave 4.2.2, Octave 4.4.0
  % and MATLAB R2015a.  The Octave GUI implementation is still a
  % little buggy.  Some of strange plotting code used here attempts
  % to work around a few Octave bugs.

  % Copyright (C) 2018 Anthony Richardson <richardson.tony@gmail.com>
  %
  % This program is free software; you can redistribute it and/or modify it under
  % the terms of the GNU General Public License as published by the Free Software
  % Foundation; either version 3 of the License, or (at your option) any later
  % version.
  %
  % This program is distributed in the hope that it will be useful, but WITHOUT
  % ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  % FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  % details.
  %
  % You should have received a copy of the GNU General Public License along with
  % this program; if not, see <http://www.gnu.org/licenses/>.
  %
  
  
  % Initial car mass, spring constant, damping coefficient
  m = 1000; k = 1e5; b = 1e4; 
  % Save initial car state
  simState('m', m); simState('k', k); simState('b', b);

  % Initial curb amplitude
  CA = 0.3;
  
  % Initial pothole depth, width, car speed
  PA = 0.3; PW = 0.3; PS = 2;
  
  % Initial wavy pavement amplitude and period
  WA = 6; WP = 0.2;

  % Save initial pavement state
  simState('CA', CA);
  simState('PA', PA); simState('PW', PW); simState('PS', PS);
  simState('WA', WA); simState('WP', WP);
  
  % Set initial pavement to curb (0)
  simState('pavement', 0);
  
  clf;
  createGUI();
  createPlot();
end

function createGUI  
  
  % Create the three car related sliders (m, k, b)
  % The slider handle and text handle for each slider are saved in GUI state
  pos = [.66 .0 .33 1];
  p = uipanel ('position', pos, 'visible', 'off');
  guiState('guipanel', p);
  
  %% BUG workaround - may be able to remove this in a
  %% future version of octave
  if (exist('OCTAVE_VERSION', 'builtin'))
    % The panel position seems to get messed up when the
    % window is resized in Octave. This was an attempt to fix it.
    %set(gcf, 'sizechangedfcn', @resetControlPanel);
    % That didn't work but this seems too.
    dellistener(gcf, 'position');
    addlistener (gcf, 'position', {@resetControlPanel, p, pos});
  end
  
  h = sliderControl(p, 'car mass (kg)', 500, ...
                      1500, simState('m'), [50 100], ...
                      [.1 .9 1 0.05], @guiCallback );
  guiState('mh', h);
  h = sliderControl(p, 'spring constant (N/m)', 50000, ...
                      150000, simState('k'), [1000 5000] , ...
                      [.1 .8 1 0.05], @guiCallback );
  guiState('kh', h);
  h = sliderControl(p, 'damping (Ns/m)', 5000, 20000, ...
                      simState('b'), [100 1000], ...
                      [.1 .7 1 0.05], @guiCallback );
  guiState('bh', h);
  
  % Create buttons for switching between pavement types
  % Save each button handle in GUI state
  gp = uibuttongroup (p, 'Position', [ 0 0.5 1 0.1]);
  b = uicontrol (gp, 'style', 'radiobutton', 'units', 'normalized',...
            'callback', @guiCallback, 'value', 1,...
            'Position', [ 0.2 0.7 0.2 0.2 ]);
  guiState('cbutton', b);
  uicontrol(gp, 'style', 'text', 'units', 'normalized',...
           'position', [0.08 0.2 0.3 0.3], 'String', 'curb');
  b = uicontrol (gp, 'style', 'radiobutton', 'units', 'normalized',...
            'callback', @guiCallback, 'value', 0,...
            'Position', [ 0.5 0.7 0.2 0.2 ]);
  guiState('pbutton', b);
  uicontrol(gp, 'style', 'text', 'units', 'normalized',...
           'position', [0.38 0.2 0.3 0.3], 'String', 'pothole');
  b = uicontrol (gp, 'style', 'radiobutton', 'units', 'normalized',...
            'callback', @guiCallback, 'value', 0,...
            'Position', [ 0.8 0.7 0.2 0.2 ]);
  guiState('wbutton', b);
  uicontrol(gp, 'style', 'text', 'units', 'normalized',...
           'position', [0.68 0.2 0.3 0.3], 'String', 'wavy');
  
  % Create separate panels for pavement types
  % Hide/unhide panels depending on pavement type
  % The curb panel has one slider for curb height
  cpanel = uipanel (p, 'position', [.0 .0 1 .5], 'visible', 'off');
  guiState('cpanel', cpanel);
  h = sliderControl(cpanel, 'curb amplitude (cm)', 0.1, 10, ...
                      simState('CA'), [.05 1], ...
                      [.1 .8 1 0.1], @guiCallback );
  guiState('cah', h);
  
  % The pothole panel has three sliders
  ppanel = uipanel (p, 'position', [.0 .0 1 .5], 'visible', 'off');
  guiState('ppanel', ppanel);
  h = sliderControl(ppanel, 'depth (cm)', 0.1, 10, ...
                      simState('PA'), [.1 1], ...
                      [.1 .8 1 0.1], @guiCallback );
  guiState('pah', h);
  h = sliderControl(ppanel, 'width (m)', 0.1, 10, ...
                      simState('PW'), [.1 1], ...
                      [.1 .5 1 0.1], @guiCallback );
  guiState('pwh', h);
  h = sliderControl(ppanel, 'car speed (m/s)', 0.1, 10, ...
                      simState('PS'), [.1 1], ...
                      [.1 .2 1 0.1], @guiCallback );
  guiState('psh', h);
  
  % The wavy panel has two sliders
  wpanel = uipanel (p, 'position', [.0 .0 1 .5], 'visible', 'off');
  guiState('wpanel', wpanel);
  h = sliderControl(wpanel, 'amplitude (cm)', 0.1, 10, ...
                      simState('WA'), [.1 1], ...
                      [.1 .8 1 0.1], @guiCallback );
  guiState('wah', h);
  h = sliderControl(wpanel, 'period (s)', 0.1, 1, ...
                      simState('WP'), [.01 .05], ...
                      [.1 .5 1 0.1], @guiCallback );
  guiState('wph', h);
  set(cpanel, 'visible', 'on');
end

function resetControlPanel(s, e, p, pos)
  set(p, 'visible', 'off');
  set(p, 'position', [0 0 1 1]);
  set(p, 'position', pos);
  set(p, 'visible', 'on');
end

% The same function is used to handle events from all GUI control elements.
function guiCallback(s, e)
  replot = false;
  g = guiState();
  if (s == g.mh)
    % Car mass slider
    v = round(get(s, 'value')/50)*50;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('m', v);
    replot = true;
  elseif (s == g.kh)
    % Spring constant slider
    v = round(get(s, 'value')/1000)*1000;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('k', v);
    replot = true;
  elseif (s == g.bh)
    % Damping coefficient
    v = round(get(s, 'value')/100)*100;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('b', v);
    replot = true;
  elseif (s == g.cbutton)
    % Curb button
    % Selecting a button causes the previously selected button to also respond
    % Ignore the deselection event
    % Switch between three control panels based on button selection
    if(get(s,'value') == 1)
      set(g.ppanel,'visible','off');
      set(g.wpanel,'visible','off');
      set(g.cpanel,'visible','on');
      simState('pavement', 0);
      replot = true;
    end
  elseif (s == g.pbutton)
    % Pothole button
    if(get(s,'value') == 1)
      set(g.cpanel,'visible','off');
      set(g.wpanel,'visible','off');
      set(g.ppanel,'visible','on');
      simState('pavement', 1);
      replot = true;
    end
  elseif (s == g.wbutton)
    % Wavy pavement button
    if(get(s,'value') == 1)
      set(g.cpanel,'visible','off');
      set(g.ppanel,'visible','off');
      set(g.wpanel,'visible','on');
      simState('pavement', 2);
      replot = true;
    end
  elseif (s == g.cah)
    % Curb amplitude
    v = round(get(s, 'value')*100)/100;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('CA', v);
    replot = true;
  elseif (s == g.pah)
    % Pothole amplitude
    v = round(get(s, 'value')*10)/10;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('PA', v);
    replot = true;
  elseif (s == g.pwh)
    % Pothole width
    v = round(get(s, 'value')*10)/10;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('PW', v);
    replot = true;
  elseif (s == g.psh)
    % Car speed
    v = round(get(s, 'value')*10)/10;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('PS', v);
    replot = true;
  elseif (s == g.wah)
    % Wavy pavement amp
    v = round(get(s, 'value')*10)/10;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('WA', v);
    replot = true;
  elseif (s == g.wph)
    % Wavy pavement period
    v = round(get(s, 'value')*100)/100;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v,'%4.2f'));
    simState('WP', v);
    replot = true;
  end
  
  % Don't replot on button deselection
  if (replot)
    createPlot();
  end
end

function createPlot()
  % The t vector is only computed once for efficiency.
  persistent t;
  t = linspace(-0.01, 0.5, 501);
  
  % Get the current state
  s = simState();
  
  subplot(1,3,[1 2]);
  
  %simState('pavement')
  if (s.pavement == 0)
    % Divide car mass by four
    r = CurbResponse(s.CA, s.m/4, s.b, s.k, t);
    %plot(t, r, 'b', 'linewidth', 4, t, s.CA*(t>=0), 'r', 'linewidth', 4);
    plot(t, r, 'b', 'linewidth', 4);
    hold on;
    plot(t, s.CA*(t>=0), 'r', 'linewidth', 4);
    hold off;
    legend('car', 'ground');
    
    % Set the yaxis ticks
    % ylimt has the thresholds, ylima has the axis limits
    % ylimm has the tick marks
    ylimt = [0.4 1.8 12];
    ylima = [0.5 2.0 16];
    ylimm = [0.05 0.1 1];
    ind = find(s.CA <= ylimt);
    ylim([0 ylima(ind(1))]);
    set(gca, 'ytick', [0:ylimm(ind):ylima(ind(1))]);
    
    xlim([t(1) t(end)]);
    set(gca,'xtick', [0:0.05:0.5]);
    grid on;
    xlabel('Time (s)');
    ylabel('Displacement (cm)');
    title('Car Response');
    % legend('car', 'ground');
  elseif (s.pavement ==  1)
    t0 = s.PW/s.PS;
    r = PotholeResponse(s.PA, s.m/4, s.b, s.k, t, t0);
    %plot(t, r, 'b', 'linewidth', 4, ...
    %      t, s.PA*(((t-t0)>=0) - (t>=0)), 'r', 'linewidth', 4);
    plot(t, r, 'b', 'linewidth', 4);
    hold on;
    plot(t, s.PA*(((t-t0)>=0) - (t>=0)), 'r', 'linewidth', 4);
    hold off;
    legend('car', 'ground');
    % Set the yaxis ticks
    ylimt = [0.4 1.2 12];
    ylima = [0.6 2.0 16];
    ylimm = [0.05 0.1 1];
    ind = find(s.PA <= ylimt);
    ylim([-ylima(ind(1)) ylima(ind(1))/2]);
    set(gca, 'ytick', [-ylima(ind(1)):ylimm(ind):ylima(ind(1))/2]);
    
    xlim([t(1) t(end)]);
    set(gca,'xtick', [0:0.05:0.5]);
    grid on;
    xlabel('Time (s)');
    ylabel('Displacement (cm)');
    title('Car Response');
  elseif (s.pavement ==  2)
    r = WavyResponse(s.WA, s.WP, s.m/4, s.b, s.k, t);
    %plot(t, r, 'b', 'linewidth', 4, ...
    %      t, s.WA*cos(2*pi*t/s.WP), 'r', 'linewidth', 4);
    plot(t, r, 'b', 'linewidth', 4);
    hold on;
    plot(t, s.WA*cos(2*pi*t/s.WP), 'r', 'linewidth', 4);
    hold off;
    legend('car', 'ground');
    
    % Set the yaxis ticks
    ylimt = [0.4 1.6 12];
    ylima = [0.5 2.0 12];
    ylimm = [0.05 0.2 2];
    ind = find(s.WA <= ylimt);
    ylim([-ylima(ind(1)) ylima(ind(1))]);
    set(gca, 'ytick', [-ylima(ind(1)):ylimm(ind):ylima(ind(1))]);

    xlim([t(1) t(end)]);
    set(gca,'xtick', [0:0.05:0.5]);
    grid on;
    xlabel('Time (s)');
    ylabel('Displacement (cm)');
    title('Car Response');
  end
  set(guiState('guipanel'), 'visible', 'on');         
  
end

function r = CurbResponse(A, m, b, k, t)
  a1 = b/m; a2 = k/m;
  b1 = b/m; b2 = k/m;
  w0 = sqrt(a2); alpha = a1/2; epsilon = alpha/w0;
  
  if (epsilon > 1)
    % Overdamped
    p1 = w0*(-epsilon + sqrt(epsilon^2 - 1));
    p2 = w0*(-epsilon - sqrt(epsilon^2 - 1));
    A1 = (b1*p1 + b2)/(p1 - p2);
    A2 = -(b1*p2 + b2)/(p1 - p2);
    r = A*(A1/p1*(exp(p1*t) - 1) + A2/p2*(exp(p2*t) - 1)).*(t >= 0);
  elseif (epsilon == 1)
    % Critical damping
    C1 = b1;
    C2 = b2 - alpha*b1;
    r = A*((C1/alpha + C2/alpha^2)*(1 - exp(-alpha*t)) - ...
                                C2/alpha*t.*exp(-alpha*t)).*(t >= 0);
  else
    % Underdamped
    wd = w0*sqrt(1 - epsilon^2);
    B1 = b1;
    B2 = (b2 - b1*alpha)/wd;
    r = A/(alpha^2+wd^2)* ...
       ((-(B1*alpha+B2*wd)*cos(wd*t) + ...
              (B1*wd+B2*alpha)*sin(wd*t)).*exp(-alpha*t) + ...
                                      (B1*alpha+B2*wd) ).*(t >= 0);
  end
end

function r = PotholeResponse(A, m, b, k, t, t0)
  r = CurbResponse(A, m, b, k, t-t0) - CurbResponse(A, m, b, k, t);
end

function r = WavyResponse(A, T0, m, b, k, t)
  a1 = b/m;   a2 = k/m;

  % Transfer function
  Hs = @(s) (a1*s + a2) ./ (s.^2 + a1*s + a2);
  w0 = 2*pi/T0;
  % Frequency response at input frequency
  H = Hs(j*w0);
  r = A*abs(H)*cos(w0*t + angle(H));
end

function v = guiState(key, val)
  persistent state;
  if (nargin == 0)  
    v = state;
  elseif (nargin == 1)
    v = state.(key);
  elseif (nargin == 2)
    state.(key) = val;
    v = val;
  end
end

function v = simState(key, val)
  persistent state;
  if (nargin == 0)  
    % Return the entire struct
    v = state;
  elseif (nargin == 1)
    % Return a particular field from the struct
    v = state.(key);
  elseif (nargin == 2)
    % Set a key value (key must be a string)
    state.(key) = val;
    v = val;
  end
end

function h = sliderControl(parent, label, min, max, val, step, pos, cback)
  % sliderstep = [minstep maxstep]
  % where minstep is fraction of range on arrow click
  % and maxstep is fraction of range on trough click
  h = uicontrol(parent, 'style' ,'slider',...
           'units', 'normalized',...
           'position', [pos(1)+0.1*pos(3) pos(2) 0.7*pos(3) pos(4)],...
           'min', min, 'max', max,...
           'sliderstep', [step(1)/(max-min) step(2)/(max-min)],...
           'value', val, 'callback', cback);
  uicontrol(parent, 'style', 'text',...
           'units', 'normalized',...
           'position', [pos(1) pos(2) 0.1*pos(3) pos(4)],...
           'String', num2str(min));
  %addproperty('my_slider_min', h, 'handle', hc);
  uicontrol(parent, 'style', 'text',...
           'units', 'normalized',...
           'position', [pos(1)+0.8*pos(3) pos(2) 0.1*pos(3) pos(4)],...
           'String', num2str(max));         
  %addproperty('my_slider_max', h, 'handle', hc);
  uicontrol(parent, 'style', 'text',...
           'units', 'normalized',...
           'position', [pos(1)+0.15*pos(3) pos(2)-pos(4) 0.4*pos(3) pos(4)],...
           'horizontalalignment','left',...
           'String', label);        
  hc = uicontrol(parent, 'style', 'text',...
           'units', 'normalized',...
           'position', [pos(1)+.5*pos(3) pos(2)-pos(4) 0.3*pos(3) pos(4)],...
           'String', num2str(val));
  %addproperty('my_slider_val', h, 'handle', hc);
  set(h, 'userdata', hc);
end