%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
%zustandsregler:
% web(fullfile(docroot, 'simulink/examples.html'))
% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
  sys=mot.ss; 
  sys=ss(sys.A,sys.B,sys.C(3,:),0); %   $$$ only output position

  %sys=ss(tf(mot1.mdl.num1,mot1.mdl.den1));
  %A=sys.A;
  %B=sys.B;
  %C=sys.C;
  %[A,B,C,D]=tf2ss(mot1.mdl.num1,mot1.mdl.den1)
  %tf2ss(mot1.mdl.num1,mot1.mdl.den1)

  figure();h=bodeplot(sys);
  setoptions(h,'IOGrouping','all')
  A=sys.A;
  B=sys.B;
  C=sys.C;
  D=sys.D;

  P=ctrb(A,B);
  if rank(A)==rank(P)
    disp('sys controlable')
  else
    disp('sys not controlable')
  end

  Q=obsv(A,C);
  if rank(A)==rank(Q)
    disp('sys observable')
  else
    disp('sys not observable')
  end


  t = 0:1E-4:.5;
  u = ones(size(t));  %1000um
  x0 = zeros(1,length(sys.A));

  [y,t,x] = lsim(sys,u,t,x0);
  figure();plot(t,y)


  poles = eig(A);

  if motid==1
    p1=-3300+2800i;
    p2=-1500+500i;
    p3=-1200+10i;
    P=[p1 p1' p2 p2' p3 p3'];
  else
  end
  K = place(A,B,P);
  %K = acker(A,B,P);
  %K = acker(A,B,[p1 p1' p2 p2' p3 p3']);
  %K = place(A,B,[p1 p1']);
  %Nbar = rscale(sys,K);
  %Nbar=1;
  Nbar=-1./(C*(A-B*K)^-1*B); %from my notes)
  %Nbar(2)=1; %the voltage stuff is crap for now
  if length(Nbar)>1
    Nbar=Nbar(3); % only the position scaling needed
  end
  sys_cl = ss(A-B*K,B,C,0);
  [y,t,x]=lsim(sys_cl,Nbar*u,t,x0);
  figure();plot(t,y)

  %observer poles-> 5 times farther left than system poles
  if motid==1
    op1=(p1*5);
    op2=(p2*5);
    op3=(p3*5);
    OP=[op1 op1' op2 op2' op3 op3'];
  else
  end
  L=place(A',C',OP)';

  At = [ A-B*K             B*K
         zeros(size(A))    A-L*C ];
  Bt = [    B*Nbar
         zeros(size(B)) ];
  Ct = [ C    zeros(size(C)) ];
  sys = ss(At,Bt,Ct,0);
  lsim(sys,ones(size(t)),t,[x0 x0]);

  %https://www.youtube.com/watch?v=Lax3etc837U
  Ao=A-L*C;
  Bo=[B L];
  Co=K;
  Do=zeros(size(Co,1),size(Bo,2));
  mdlName='observer';
  open(mdlName)
end

function SCRATCH()
  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