function [mot1,mot2]=identifyFxFyStage()
  %sample idfrd
  %f = logspace(-1,1,100);
  %[mag,phase] = bode(idtf([1 .2],[1 2 1 1]),f);
  %response = mag.*exp(1j*phase*pi/180);
  %m = idfrd(response,f,0.08);

  function obj=loadData(path,motid)
    obj=struct();
    f=load(strcat(path,sprintf('curr_step%d.mat',motid)));
    obj.currstep=f;
    %prepend sone zeros to stable system identification
    obj.currstep=iddata([zeros(10,1); obj.currstep.data(:,2)],[zeros(10,1); obj.currstep.data(:,3)],50E-6);
    
    f=load(strcat(path,sprintf('full_bode_mot%d.mat',motid)));
    obj.w=f.frq*2*pi;            %convert from Hz to rad/s
    if motid==2
      f.db_mag(1:224)=f.db_mag(225); % reset bad values at low frequencies
    end
    obj.mag=10.^(f.db_mag/20);     %mag not in dB
    obj.phase=f.deg_phase*pi/180;  %phase in rad
    response = obj.mag.*exp(1j*obj.phase);
    obj.meas= idfrd(response,obj.w,0);
  end

  function tfc=currstep(obj)
    opt=tfestOptions;
    opt.Display='off';
    tfc = tfest(obj.currstep, 2, 0,opt);
    s=str2ndOrd(tfc);
    %disp(s);
    %h = stepplot(tf1);
    %l=obj.currstep.OutputData
    t=(0:199)*50E-6;
    [y,t]=step(tfc,t);
    f=figure();
    subplot(1,2,1);
    plot(t,y*1000,'r',t,obj.currstep.OutputData(11:210),'b');
    title(s);
    subplot(1,2,2);
    h=bodeplot(tfc,'r');
    setoptions(h,'FreqUnits','Hz','Grid','on');
  end

  function s=str2ndOrd(tf)
    den=tf.Denominator;
    num=tf.Numerator;
    k=num(1)/den(3);
    w0=sqrt(den(3));
    damp=den(2)/2/w0;
    s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp);
  end

  function tf=fyModel()
    num=[ 2.36527033e+11,   1.17108082e+13,   3.62387303e+17];
    den=[ 1.00000000e+00,   6.64495206e+03,   2.12777376e+07, ...
          1.23728427e+10,   3.07054470e+13,   1.72592127e+15, ...
          5.39888656e+17];

    tf=idtf(num,den);
  end

  function tf=fxModel()
    num=[ 1.23284092e+11,   4.14791803e+13,   1.18702926e+18, ...
          2.96296718e+20,   2.67179357e+24,   4.04662786e+26, ...
          1.59131515e+30,   1.02778572e+32,   1.64551888e+35];
    den=[ 1.00000000e+00,   6.93892369e+03,   3.17041055e+07, ...
          7.66104262e+10,   2.36504992e+14,   2.23054854e+17, ...
          5.12578678e+20,   2.04416512e+23,   3.27771400e+26, ...
          4.77145416e+28,   3.85452959e+31,   1.28911178e+33, ...
          9.52157664e+34];
     tf=idtf(num,den);
  end

  function mot=fyStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
    mot.tfc=currstep(mot);

    opt=tfestOptions;
    opt.Display='off';
    opt.initializeMethod='iv';
    opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion

    figure(); 
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
    mot.tf_mdl= fyModel();
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    
  end

  function mot=fxStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2);
    currstep(mot);

    opt=tfestOptions;
    opt.Display='off';
    opt.initializeMethod='iv';
    opt.WeightingFilter=[1,4;10,670]*(2*pi); % Hz->rad/s conversion

    figure();
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
    mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
    mot.tf_mdl = fxModel();
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    %controlSystemDesigner('bode',1,mot.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited

  end
  close all
  mot1=fyStage();
  mot2=fxStage();

end


function f=SCRATCH()
  [m1,m2]=identifyFxFyStage();
  controlSystemDesigner(1,m2.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited

  %identification toolbox
  systemIdentification

  
  
  
  
  %opt=tfestOptions('Display','off');
  %opt=tfestOptions('Display','on','initializeMethod','svf');
  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[]);
  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[1,5;20,570]);
  %tf1 = tfest(mot1frq, 6, 4, opt);
  % Model refinement
  % Options = tf1.Report.OptionsUsed;
  % Options.WeightingFilter = 'prediction';
  % tf1_1 = pem(mot1frq, tf1, Options)





  bodeplot(mot1frq,tf1)
  mag,phase=bode(tf1,frq)
  figure(1)
  subplot(211)


  bodeplot(tf1)

  Opt = n4sidOptions('N4Horizon',[15 15 15]);
  n4s3 = n4sid(mot1frq, 3, Opt)





  %tf([1 2],[1 0 10])
  %specifies the transfer function (s+2)/(s^2+10) while
  sys=tf([1],[1,0,0])
  bode(sys)
  step(sys)

  sys=tf([1],[1,-1,2]) %instable

  sys=tf([1],[1,1,2]) %stable


  %0dB at 12 Hz=12*2*pi rad/s =75.4=k^2 -> k=8.6833

  sys=tf([10],[1,0,0])
  %1/s^2 -> 0dB at 1Hz -40dB/decade
  %10=+20dB


  sys=tf([1],[1,0,2]) %not damped constant sine after step


  sys=zpk([],[1,0,0],100) %stable

  sys=zpk([],[-10,-10],100)


  %parker stage 1 
  %!encoder_sim(enc=1,tbl=9,mot=9,posSf=13000./2048)
  %!encoder_inc(enc=1,tbl=1,mot=1,posSf=13000./650000)
  %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
  %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
  Ts=2E-4 % discrete sample time (servo period)
  Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000
  Kp=25,Kvfb=0,Ki=0,Kvff=0,Kaff=0,MaxInt=0


  num=7.32
  den=[5.995e-04 4.897e-02   1.]
  open('stage_closed_loop.slx')
  %sim('stage_closed_loop.slx')

  sys=tf(num,den)
  bode(sys)
  G = tf(1.5,[1 14 40.02]);
  controlSystemDesigner('bode',sys);
  controlSystemDesigner
  linearSystemAnalyzer
  load ltiexamples
  linearSystemAnalyzer(sys_dc)

  controlSystemDesigner('bode',sys);
  controlSystemDesigner(1,sys);    % <<<<<<<<< This opens a transferfiûnction that can be edited

  num=[8.32795069e-11, 1.04317228e-08, 6.68431323e-05, 3.31861324e-03, 7.32824533e+00];
  den=[5.26156641e-18, 1.12897840e-14, 7.67853031e-12, 1.03201301e-08, 2.05154780e-06, 1.34279894e-03, 7.19229912e-02, 1.00000000e+00];
  mot2=tf(num,den);
  controlSystemDesigner('bode',mot2);
  
  

end