%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];
  SIM1=[10 0;7 3];
  SIM1=[10 0];
  %SIM2=[9 0];
  if 1 || 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
  tfz=c2d(tf(tfs),1/5000)
  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');
  
  
  %C=num=0   -1.3236    6.2472  -11.8555   11.3067   -5.4188    1.0440
  %D=den=1.0000   -6.6330   17.6945  -24.5314   18.7409   -7.5020    1.2309
  % controlSystemDesigner('bode',1,tf(1,1))
 % controlSystemDesigner( 1/simData2(2).tfEst2)
 % controlSystemDesigner(tfa)
 
 %C =
 %  1035.3 (s+217.8)
 % -------------------
 % (s+845.3) (s+266.8)
 
 %  0.18956 (z-0.9574)
 % --------------------
 % (z-0.948) (z-0.8444)
 
%    0.1896 z - 0.1815
%  ----------------------
%  z^2 - 1.792 z + 0.8006
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');
  legend('Location','best');
end