baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab/figures';

clear;clear global;
close all;disp('load identification data...');
mot=identifyFxFyStage(7);

close all;disp('simulate stage closed loop...');
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');
  f=bodeSamples(desPos_actPos);
  print(f,sprintf('figures/sim_cl_DTGz_bode%d',mot{k}.id),'-depsc');
end

close all;disp('simulate observer...');
for sscType=0%0:1
  for k=1:2
    [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
    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_obs_%d_%d',m,mot{k}.id),'-depsc');
    f=bodeSamples(desPos_actPos);
    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',m,mot{k}.id),'-depsc');
  end
end
disp('document figure generation done');close all;



function SCRATCH()
  %plots bode of desPos_actPos
  [mot1,mot2]=identifyFxFyStage();
  close all;
  [pb]=simFxFyStage(mot2);sim('stage_closed_loop');


  t=desPos_actPos.Time;
  u=desPos_actPos.Data(:,1);
  y=desPos_actPos.Data(:,2);
  figure(1);
  clf;
  plot(t,u,'b');hold on;
  plot(t,y,'color',[0 .5 0]);
  grid on;ylim([-4 4]);
  grid on;ylim([-2 2]);
  saveas(gca, baseDir+"m2_sim_pb.eps",'epsc');
  saveas(gca, baseDir+"m1_sim_ss.eps",'epsc');
  saveas(gca, baseDir+"m2_sim_ss.eps",'epsc');
  saveas(gca, baseDir+"m1_sim_ss_pref.eps",'epsc');
  saveas(gca, baseDir+"m2_sim_ss_pref.eps",'epsc');


  saveas(gca, baseDir+"m2_mdl_bode.eps",'epsc');

  minFrq=1;maxFrq=1000;
  tSec=t(length(t));
  f=(1:length(t))/tSec;
  fu=fft(u);
  fy=fft(y);
  ph=phase(fy./fu)
  mag=abs(fy)./abs(fu);
  magDb=20*log10(mag);
  figure(2);
  subplot(2,1,1);
  semilogx(f,magDb);
  subplot(2,1,2);
  semilogx(f,ph);
  grid on;
end