%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace

%code snipplets from an example on youtube (see reference at top)
function SCRATCH()
%import numpy as np
%fh=np.load('mode1.npz')
%import scipy.io
%scipy.io.savemat('mode1.mat',fh,do_compression=True)

%matlab:
  load('mode1.mat');
  plot(pts(:,1),pts(:,2),'.');hold on;
  plot(rec(:,5),rec(:,6),'-');%despos
  plot(rec(:,2),rec(:,3),'-');%actPos

  %sig.time = [0 1 1 5 5 8 8 10];
  %sig.signals.values = [0 0 2 2 2 3 3 3]';
  %sig.signals.dimensions = 1;
  sig.time=0:2E-4:(length(rec)-1)*2E-4;
  sig.signals.values=rec(:,5);
  sig.signals.dimensions = 1;

  sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2))

  A = [ 0   1   0
       980  0  -2.8
        0   0  -100 ];

  B = [ 0
        0
        100 ];

  C = [ 1 0 0 ];

  poles = eig(A)

  t = 0:0.01:2;
  u = zeros(size(t));
  x0 = [0.01 0 0];

  sys = ss(A,B,C,0);

  [y,t,x] = lsim(sys,u,t,x0);
  plot(t,y)
  title('Open-Loop Response to Non-Zero Initial Condition')
  xlabel('Time (sec)')
  ylabel('Ball Position (m)')

  p1 = -10 + 10i;
  p2 = -10 - 10i;
  p3 = -50;

  K = place(A,B,[p1 p2 p3]);
  sys_cl = ss(A-B*K,B,C,0);

  lsim(sys_cl,u,t,x0);
  xlabel('Time (sec)')
  ylabel('Ball Position (m)')

  p1 = -20 + 20i;
  p2 = -20 - 20i;
  p3 = -100;

  K = place(A,B,[p1 p2 p3]);
  sys_cl = ss(A-B*K,B,C,0);

  lsim(sys_cl,u,t,x0);
  xlabel('Time (sec)')
  ylabel('Ball Position (m)')


  t = 0:0.01:2;
  u = 0.001*ones(size(t));

  sys_cl = ss(A-B*K,B,C,0);

  lsim(sys_cl,u,t);
  xlabel('Time (sec)')
  ylabel('Ball Position (m)')
  axis([0 2 -4E-6 0])

  Nbar = rscale(sys,K)

  lsim(sys_cl,Nbar*u,t)
  title('Linear Simulation Results (with Nbar)')
  xlabel('Time (sec)')
  ylabel('Ball Position (m)')
  axis([0 2 0 1.2*10^-3])
end

clear;clear global;close all;
mot=identifyFxFyStage(7);
for k =1:2
  [pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
  set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
  print(f,sprintf('figures/sim_cl_DTGz_%d',mot{k}.id),'-depsc');
end