%before calling that put following line in the command window:
%
%   global mot pb;
% or even:
%   clear global all;clear all;global mot pb;

function DeltaTauOptimizer()
  global mot simData1 simData2;
  if isempty(mot)
    mot=identifyFxFyStage(7);
  end
  %SIM1=[1 1; 1 2; 8 2; 9 2;9 3];
  %SIM2=[1 1; 1 2; 8 2; 9 2];
  SIM1=[9 1; 9 2; 9 3; 9 4];
  SIM2=[9 1; 9 2; 9 3; 9 4];
  %SIM2=[8 1; 8 2; 8 3; 8 4];
  %SIM2=[8 1; 8 2; 9 1; 9 2];
  %SIM2=[8 2; 9 2;8 3; 9 3];
  %SIM2=[8 3; 9 3;8 4; 9 4];
  %SIM2=[9 4;9 0];
  %SIM2=[8 3; 9 3];
  if isempty(simData1)
    close all;
    simData1=ExecSim(mot{1},SIM1);
  end
  if isempty(simData2)
    close all;
    simData2=ExecSim(mot{2},SIM2);
  end
  close all;
  %test()
  bodeSim(simData1);
  bodeSim(simData2);
end

function test()
  global pb mot simData1 simData2;
  %pb=DeltaTauParam(mot{2},8,0);
  pb=DeltaTauParam(mot{2},9,0);
  %pb.C=[0.04877];
  %pb.D=[1 -0.9512];
  %pb.C=[1   -1.3236    6.2472  -11.8555   11.3067   -5.4188    1.0440];
  %pb.D=[1.0000   -6.6330   17.6945  -24.5314   18.7409   -7.5020    1.2309];
  global tfs
  Ts=1/5000;
  frq0=55;d0=.5;
  frq1=85;d1=.5;
  w0=frq0*2*pi;
  w1=frq1*2*pi;
  tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
  tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
  tfs=tf0*tf1
  tfz=c2d(tfs,Ts)
  h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
  pb.C=tfz.Numerator{1};
  pb.D=tfz.Denominator{1};
  sim('DeltaTauSim');
  i=2;
  %simData2(i).mot_mdl_param=SIM(i,:);
  simData2(i).pb=pb;
  simData2(i).desPos_actPos=desPos_actPos;
  simData2=bodeSim(simData2);

  opt=tfestOptions;
  opt.Display='off';
%   %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
  tfa=tfest(simData2(i).tfEst, 6, 6,opt);
%   tfb=1/tfa
%   tfc=c2d(tfb,1/5000)
% 
%   tfs=tf([1],[.001 1])
%   tfz=c2d(tfs,1/5000)
%   h=bodeplot(tfs,tfz)
%   setoptions(h,'FreqUnits','Hz','Grid','on');
% controlSystemDesigner(tfa)
end


function simData=ExecSim(mot,SIM)
  global pb;
  %   mot mdl param
  simData=struct;
  for i =1:size(SIM,1)
    [mdl,param]=feval(@(x) x{:}, num2cell(SIM(i,:)));
    pb=DeltaTauParam(mot,mdl,param);
    sim('DeltaTauSim');
    simData(i).mot_mdl_param=SIM(i,:);
    simData(i).pb=pb;
    simData(i).desPos_actPos=desPos_actPos;

    fig=bodeSamples(desPos_actPos);
    title(fig.Children(2),pb.desc);
  end
end

function simData=bodeSim(simData)
  fig=figure();
  ax1=subplot(2,1,1);  hold(ax1,'on')
  ax2=subplot(2,1,2);  hold(ax2,'on')
  Ts=1/5000; % sampling period

  for i =1:length(simData)
    sd=simData(i).desPos_actPos;
    t=0:Ts:sd.Time(end);
    posI=interp1(sd.Time,sd.Data(:,1),t);
    posO=interp1(sd.Time,sd.Data(:,2),t);
    ftI=fft(posI);
    ftO=fft(posO);
    tf=ftO./ftI;

    L=length(t);
    k=(L-1)*Ts; % fmax =1/Ts at sample (L-1)/2 (index0-base)
    N=[10 220]*k; % number of relevant indexes (index0-base)
    frq=(N(1):N(2))/k;
    N=N+1;%index0-base -> index1-base
    tfn=tf(N(1):N(2));

    %fig=figure(); h=plot(abs(ftI));
    %fig=figure(); h=plot(abs(ftO));
    %fig=figure(); h=plot(abs(ftI(N(1):N(2))));
    h=plot(ax1,frq,abs(tfn), 'DisplayName',simData(i).pb.desc);
    p=unwrap(phase(tfn))/(2*pi)*360;
    h=plot(ax2,frq,p, 'DisplayName',simData(i).pb.desc);
    simData(i).tfEst=idfrd(tfn,frq*2*pi,0);
  end
  grid(ax1,'on');grid(ax2,'on');
  set(ax1, 'XScale', 'log');
  set(ax2, 'XScale', 'log');
  linkaxes([ax1,ax2],'x')
  legend('Location','best');
end