  % Simulated Inverted Pendulum Control
  %  
  % This Octave/MATLAB script was written to mimic a corresponding
  % Labview module (Module 4.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/>.
  %
  
  clf;
  % K is proportional control value
  K = 1.25; 
  % K1 and K2 are PI control values
  K1 = 1.25; K2 = 2; 

  % Period of the sinusoidal input
  T0 = 1;
  
  % Pendulum length (meters)
  L = 0.6125;

  % Scale factor for input
  f = 1e-3;
  
  % Save initial simulation state
  simState('K', K); simState('K1', K1); simState('K2', K2);
  simState('T0', T0); simState('L', L); simState('f', f);
  
  createGUI();
  createPlot();
end

function createGUI
  % The slider handle and text handle for each slider are saved in GUI state
  pos = [.57 .5 .25 0.5];
  p = uipanel ('position', pos, 'visible', 'off');
  guiState('cpanel', 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
  
  panel1 = uipanel (p,'position', [0 .75 1 1/4]);
  h = sliderControl(panel1, 'K', 0.7, ...
                      2, simState('K'), [.05 .2], ...
                      [0.1 .5 1 .5], @guiCallback );
  guiState('kh', h);
  
  panel2 = uipanel (p,'position', [0 0.25 1 1/2]);
  h = sliderControl(panel2, 'K1', 0.7, ...
                      2, simState('K1'), [.05 .2], ...
                      [0.1 .75 1 .25], @guiCallback );
  guiState('k1h', h);
  h = sliderControl(panel2, 'K2', 0, ...
                      10, simState('K2'), [.25 1], ...
                      [0.1 .25 1 .25], @guiCallback );
  guiState('k2h', h);
  
  panel3 = uipanel (p,'position', [0 0 1 1/4]);
  h = sliderControl(panel3, 'Period', 0.5, ...
                      5, simState('T0'), [.25 .5], ...
                      [0.1 .5 1 .5], @guiCallback );
  guiState('t0h', h);

  % Draw the plot
  createPlot();
end

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

function guiCallback(s, e)
  g = guiState();
  if (s == g.kh)
    % K slider
    v = round(get(s, 'value')/0.05)*0.05;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('K', v);
  elseif (s == g.k1h)
    % K1
    v = round(get(s, 'value')/0.05)*0.05;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('K1', v);
  elseif (s == g.k2h)
    % K2
    v = round(get(s, 'value')/0.2)*0.2;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('K2', v);
  elseif (s == g.t0h)
    % Period
    v = round(get(s, 'value')/0.25)*0.25;
    set(s, 'value', v);
    set(get(s, 'userdata'), 'string', num2str(v));
    simState('T0', v);
  end
  
  createPlot();
end

function createPlot()
  % The t vector is only computed once for efficiency.
  persistent t;
  t = linspace(0, 5, 1000);
  g = 9.8;
  
  % Get the current state
  s = simState();
  K = s.K; K1 = s.K1; K2 = s.K2; 
  T0 = s.T0; L = s.L;
  W0 = 2*pi/T0; f = s.f;

  % This is P control (step)
  %{
  % Use numerical residue method to find ILT
  B = [-1, 0]*f;
  A = [L-K 0 -g];
  xstep = ilt(B, A, t);
  %}
  % Use analytical partial fraction method to find ILT
  WN = sqrt(g/(K-L));
  xstep = (f/(K-L))*cos(WN*t);

  % This is PI control (not PD)
  %{
  % Use numerical residue method to find ILT
  B = [-1, 0]*f;
  A = [L-K1 -K2 -g];
  xstep2 = ilt(B, A, t);
  %}
  % Use analytical partial fraction method to find ILT
  A = 0.5*K2/(K1-L); B = sqrt(g/(K1-L));
  WN = sqrt(B^2 - A^2);
  xstep2 = (f/(K1-L))*exp(-A*t).*(cos(WN*t) - (A/WN)*sin(WN*t));
  
  subplot(2, 2, 1)
  plot(t, 1000*xstep, 'linewidth', 2);
  hold on;
  plot(t, 1000*xstep2, 'linewidth', 2);
  hold off;
  grid on;
  xlabel('Time (s)');
  ylabel('\theta (milliradians)');
  title('Response to 0.001 u(t)');
  %set(h, 'horizontalalignment', 'right')
  legend('P', 'PI', 'location', 'northeastoutside');
  %legend('P', 'PI', 'location', 'northoutside');
  set(gca, 'xtick' , 0:0.5:5);
  ya = ylim();
  set(gca, 'ytick', ya(1):ya(2)/4:ya(2));
  
  % Ramp
  %{
  % Use numerical residue method to find ILT
  B = [-1]*f;
  A = [L-K 0 -g];
  xramp = ilt(B, A, t);
  %}
  %%{
  % Use analytical partial fraction method to find ILT
  WN = sqrt(g/(K-L));
  xramp = (f/(WN*(K-L)))*sin(WN*t);
  %}

  %{
  % Use numerical residue method to find ILT
  B = [-1]*f;
  A = [L-K1 -K2 -g];
  xramp2 = ilt(B, A, t);
  %}
  %%{
  % Use analytical partial fraction method to find ILT
  A = 0.5*K2/(K1-L); B = sqrt(g/(K1-L));
  WN = sqrt(B^2 - A^2);
  xramp2 = (f/(WN*(K1-L)))*exp(-A*t).*sin(WN*t);
  %}

  subplot(2, 2, 3)
  plot(t, 1000*xramp, 'linewidth', 2);
  hold on;
  plot(t, 1000*xramp2, 'linewidth', 2);
  hold off;
  grid on;
  xlabel('Time (s)');
  ylabel('\theta (milliradians)');
  title('Response to 0.001 t u(t)');
  %set(h, 'horizontalalignment', 'right')
  legend('P', 'PI', 'location', 'northeastoutside');
  %legend('P', 'PI', 'location', 'northoutside');
  set(gca, 'xtick' , 0:0.5:5);
  ya = ylim();
  set(gca, 'ytick', ya(1):ya(2)/4:ya(2));
  
  % Cosine
  %{
  % Use numerical residue method to find ILT
  B = [-1 0 0 0]*f;
  A = conv([L-K 0 -g], [1 0 W0^2]);
  xcos = ilt(B, A, t);
  %}
  %%{
  % Use analytical partial fraction method to find ILT
  WN = sqrt(g/(K-L));
  xcos = (f/((K-L)*(WN^2-W0^2)))*(WN^2*cos(WN*t) - W0^2*cos(W0*t));
  %}

  %{
  % Use numerical residue method to find ILT
  B = [-1 0 0 0]*f;
  A = conv([L-K1 -K2 -g], [1 0 W0^2]);
  xcos2 = ilt(B, A, t);
  %}
  %%{
  % Use analytical partial fraction method to find ILT
  % Finding this one was fun!
  A = 0.5*K2/(K1-L); B = sqrt(g/(K1-L));
  WN = sqrt(B^2 - A^2);
  D = B^4 + (4*A^2 - 2*B^2)*W0^2 + W0^4;
  C1 = (B^4 + (4*A^2 - B^2)*W0^2)/D;
  C2 = (2*A*B^2*W0^2)/D;
  C3 = W0^2*(W0^2 - B^2)/D;
  C4 = -2*A*W0^4/D;
  xcos2 = (f/(K1 - L))*exp(-A*t).*(C1*cos(WN*t) + (C2-A*C1)/WN*sin(WN*t)) + ...
          (f/(K1 - L))*(C3*cos(W0*t) + (C4/W0)*sin(W0*t));
  %}

  subplot(2, 2, 4)
  plot(t, 1000*xcos, 'linewidth', 2);
  hold on;
  plot(t, 1000*xcos2, 'linewidth', 2);
  hold off;
  grid on;
  xlabel('Time (s)');
  ylabel('\theta (milliradians)');
  title('Response to 0.001 cos(\omega_0 t) u(t)');
  %set(h, 'horizontalalignment', 'right')
  legend('P', 'PI', 'location', 'northeastoutside');
  %legend('P', 'PI', 'location', 'northoutside');
  set(gca, 'xtick' , 0:0.5:5);
  ya = ylim();
  set(gca, 'ytick', ya(1):ya(2)/4:ya(2));
  set(guiState('cpanel'), 'visible', 'on');  
end

% Numerical Inverse Laplace Transform
function r = ilt(N, D, t)
  [re, po , ~] = residue(N, D);
  % Determine multiplicity of pole
  [e, idx] = mpoles(po);
  
  %{
  % Method one
  % This method calculates the result in a single expression,
  % but uses more memory than method two.  It does not seem to be any faster
  % Octave version (it uses Octave's broadcasting feature)
  %r = real((re(idx)./factorial(e(idx)-1)).'*(t.^((e(idx)-1)).*exp(po(idx)*t)));
  % Octave/MATLAB version (works for both)
  r = real((re(idx)./factorial(e(idx)-1)).'* ...
          (((ones(size(e))*t).^((e(idx)-1)*ones(size(t)))).*exp(po(idx)*t)));
  %}
  
  %%{
  % Method two
  % This method uses a loop and adds in the contribution from each pole
  % individually.  It uses less memory  and does not seem to be any slower.
  r = zeros(size(t));
  for ind = 1:length(idx);
    r = r + real((re(idx(ind))/factorial(e(idx(ind))-1))* ...
                 ((t.^(e(idx(ind))-1)).*exp(po(idx(ind))*t)));
  end
  %}
 
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