%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];
  SIM2=[1 1; 1 2; 8 2; 9 2];
  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},7,0);
  pb=DeltaTauParam(mot{2},9,0);
  sim('DeltaTauSim');
  i=6;
  %simData2(i).mot_mdl_param=SIM(i,:);
  simData2(i).pb=pb;
  simData2(i).desPos_actPos=desPos_actPos;
  bodeSim(simData2);
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 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);
  end
  grid(ax1,'on');grid(ax2,'on');
  set(ax1, 'XScale', 'log');
  set(ax2, 'XScale', 'log');
  legend('Location','best');
end