function [Act,CavLPF,FittingOut,mixer] = FSS_TFs(ADC_V2C,DAC_C2V,Elec,varargin)

import_fittingdata
%%

if sum(ismember(varargin,'Plot')) 
    plotnoplot = 'Plot';
else
    plotnoplot = 'noPlot';
end
if sum(ismember(varargin,'PrintFitResult'))
    printnoprint = 'PrintFitResult';
else
    printnoprint = 'noPrint';
end


TTFSS_Elec = TTFSS_filters();
CMS_Elec = CMS_filters();
Fitting = containers.Map;
FittingOut = containers.Map;
Model = containers.Map;
CavLPF = containers.Map;
Act = containers.Map;
TimeDelay = containers.Map;
RTS = containers.Map;
Sigma = containers.Map;
FitParam = containers.Map;
%% TTFSS servo fitting
%% make model of TTFSS

Model('TTFSS_PZT') = -minreal(TTFSS_Elec('common_gain1') * TTFSS_Elec('common_gain2') ...
    * TTFSS_Elec('fast_gain1') * TTFSS_Elec('fast_LPF_10k') * TTFSS_Elec('fast_LPF_34k')...
    * TTFSS_Elec('fast_notch') * TTFSS_Elec('fast_LPF_out'));
Model('TTFSS_EOM') = minreal(TTFSS_Elec('common_gain1') * TTFSS_Elec('common_gain2') ...
    * (TTFSS_Elec('EOM_BPF1') * TTFSS_Elec('EOM_BPF2') * TTFSS_Elec('EOM_HPF') + TTFSS_Elec('EOM_BPF3')) * TTFSS_Elec('EOM_HV'));

%% PZT path fitting
% model_c = mnbode_list(Model('TTFSS_PZT'),Freq('TTFSS_PZT'),'c');
% mnbode(Freq('TTFSS_PZT'),Abs('TTFSS_PZT'),Phase('TTFSS_PZT'),Freq('TTFSS_PZT'),abs(model_c),angle(model_c)/pi*180);

FG = -4;
CG = -4;

[~,abs_fitting_data] = mntrim(0,5e4,Freq('TTFSS_PZT'),Abs('TTFSS_PZT')/ 10^(CG/20) / 10^(FG/20));
[ff_fitting_data,ang_fitting_data] = mntrim(0,5e4,Freq('TTFSS_PZT'),Phase('TTFSS_PZT'));

[Fitting('TTFSS_PZT'),~,TimeDelay('TTFSS_PZT')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    Model('TTFSS_PZT'),[],[1 2],Sigma,'TTFSS_PZT',FitParam,plotnoplot,printnoprint,'TimeDelay');

% figure()
% bode(minreal(Fitting('PZT_path')*Model('TTFSS_PZT')))
%% EOM path
% 
% bode(Model('TTFSS_EOM'))
% [z,p,k] = zpkdata(Model('TTFSS_EOM'));
% num2str(abs(z{1})/2/pi/1e3)
% num2str(abs(p{1})/2/pi/1e3)
% 
% model_c = mnbode_list(Model('TTFSS_EOM'),Freq('TTFSS_EOM'),'c');
% mnbode(Freq('TTFSS_EOM'),Abs('TTFSS_EOM'),Phase('TTFSS_EOM'),Freq('TTFSS_EOM'),abs(model_c),angle(model_c)/pi*180);
% 
[~,abs_fitting_data] = mntrim(0,3e4,Freq('TTFSS_EOM'),Abs('TTFSS_EOM')/ 10^(CG/20));
[ff_fitting_data,ang_fitting_data] = mntrim(0,3e4,Freq('TTFSS_EOM'),Phase('TTFSS_EOM'));
[Fitting('TTFSS_EOM'),~,TimeDelay('TTFSS_EOM')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('TTFSS_EOM')),[],[1 3 10],Sigma,'TTFSS_EOM',FitParam,plotnoplot,printnoprint,'TimeDelay');
 
% bode(minreal(Fitting('EOM_path')*Model('TTFSS_EOM')))


%% TTFSS TF from OUT1 to OUT2 measurement
%fitting parameters
% 
% mnbode(Freq('TTFSS_OUT1-OUT2'),Abs('TTFSS_OUT1-OUT2'),Phase('TTFSS_OUT1-OUT2'))
% 

%% PZT efficiency
Model('NPRO_PZT') = 51.75e6/35.0;

%% temperature
Model('NPRO_TEMP') = 2e9;
%% Reference cavity cavity pole fitting 
% estimate the optical gain and a cavity pole of reference cavity
% measure a TF from SR560 output to TTFSS OUT1 with lock the FSS only using
% PZT
%fitting parameters

h_H_RefCav = 0.48e-6;
h_cp_RefCav = 80e3;%cavity pole

Model('NPRO_PZT+RefCav+mixer') = Model('NPRO_PZT')*tf(h_H_RefCav,[1/h_cp_RefCav/2/pi 1]);
% mnbode(Freq('NPRO_PZT+RefCav+mixer'),Abs('NPRO_PZT+RefCav+mixer'),Phase('NPRO_PZT+RefCav+mixer'));

[~,abs_fitting_data] = mntrim(0,6e4,Freq('NPRO_PZT+RefCav+mixer'),Abs('NPRO_PZT+RefCav+mixer'));
[ff_fitting_data,ang_fitting_data] = mntrim(0,6e4,Freq('NPRO_PZT+RefCav+mixer'),Phase('NPRO_PZT+RefCav+mixer'));
[Fitting('NPRO_PZT+RefCav+mixer'),~,TimeDelay('NPRO_PZT+RefCav+mixer')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('NPRO_PZT+RefCav+mixer')),[],[1],Sigma,'NPRO_PZT+RefCav+mixer',FitParam,plotnoplot,'TimeDelay',printnoprint);
Model('RefCav') = Model('NPRO_PZT+RefCav+mixer') * Fitting('NPRO_PZT+RefCav+mixer') / Model('NPRO_PZT');
%% Open loop gain of FSS loop only with PZT
% measure the open loop gain of the FSS using only PZT. These plot confirm
% the TF of a TTFSS PZT servo, a Reference cavity LPF and PZT actuator
% efficiency.
% config
CG_OLG_FSSpzt = -10; % different from memo.txt. 
FG_OLG_FSSpzt = 15.4;
SR560 = 5;
PZT_servo = Fitting('TTFSS_PZT')* Model('TTFSS_PZT') * 10^((CG_OLG_FSSpzt)/20) * 10^((FG_OLG_FSSpzt)/20) * SR560;
Model('OLG_FSS_PZTonly(OUT2-OUT1)') = PZT_servo * Model('RefCav') * Model('NPRO_PZT')/TTFSS_Elec('common_gain1');
% model_c = mnbode_list(Model('OLG_FSS_PZTonly(OUT2-OUT1)'),Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),'c');
% 
% 
% mnbode(Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),Abs('OLG_FSS_PZTonly(OUT2-OUT1)'),Phase('OLG_FSS_PZTonly(OUT2-OUT1)'),...
%     Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),abs(model_c),angle(model_c)*180/pi)
[~,abs_fitting_data] = mntrim(0,6e4,Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),Abs('OLG_FSS_PZTonly(OUT2-OUT1)'));
[ff_fitting_data,ang_fitting_data] = mntrim(0,6e4,Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),Phase('OLG_FSS_PZTonly(OUT2-OUT1)'));
[Fitting('OLG_FSS_PZTonly(OUT2-OUT1)'),~,TimeDelay('OLG_FSS_PZTonly(OUT2-OUT1)')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('OLG_FSS_PZTonly(OUT2-OUT1)')),[],[],Sigma,'OLG_FSS_PZTonly(OUT2-OUT1)',FitParam,plotnoplot,'TimeDelay',printnoprint);

% include the timedelay of the SR560 and modify the OLG model
Model('OLG_FSS_PZTonly') = -Model('OLG_FSS_PZTonly(OUT2-OUT1)')*Fitting('OLG_FSS_PZTonly(OUT2-OUT1)')*TTFSS_Elec('common_gain1');
% model_c = mnbode_list(Model('OLG_FSS_PZTonly')*tf(1,1,'InputDelay',TimeDelay('OLG_FSS_PZTonly(OUT2-OUT1)')),Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),'c');

% mnbode(Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),Abs('OLG_FSS_PZTonly(OUT2-OUT1)')*-TTFSS_Elec('common_gain1'),Phase('OLG_FSS_PZTonly(OUT2-OUT1)'),...
%     Freq('OLG_FSS_PZTonly(OUT2-OUT1)'),abs(model_c),angle(model_c)*180/pi,'linestyle',{'*','-'})
%% Broadband EOM modulation depth
% lock the FSS with only using PZT.
% excite the frequency with EOM and measure the tf from EOM excitation
% signal to TTFSS OUT1

% TTFSS mixer transfer function
pole_TTFSS_mixer = 1e6;
Model('mixer') = zpk([],-[pole_TTFSS_mixer pole_TTFSS_mixer]*2*pi,(pole_TTFSS_mixer*2*pi)^2);

%fitting parameters
h_A_BEOM = 2.5e-3;

Model('BEOM') = tf([h_A_BEOM 0],1);
Model('BEOM+RefCav+mixer') = minreal(Model('BEOM')*Model('RefCav')*Model('mixer'));
% model_c = mnbode_list(Model('BEOM+RefCav+mixer')/(1+Model('OLG_FSS_PZTonly')),Freq('BEOM+RefCav+mixer'),'c');

% mnbode(Freq('BEOM+RefCav+mixer'),Abs('BEOM+RefCav+mixer'),Phase('BEOM+RefCav+mixer'),...
%     Freq('BEOM+RefCav+mixer'),abs(model_c),angle(model_c)*180/pi)
% [z,p,~] = zpkdata(Model('BEOM+RefCav+mixer'));
% num2str(abs(z{1})/2/pi/1e3)
% num2str(abs(p{1})/2/pi/1e3)

[~,abs_fitting_data] = mntrim(4e4,1e7,Freq('BEOM+RefCav+mixer'),Abs('BEOM+RefCav+mixer'));
[ff_fitting_data,ang_fitting_data] = mntrim(4e4,1e7,Freq('BEOM+RefCav+mixer'),Phase('BEOM+RefCav+mixer'));
[Fitting('BEOM+RefCav+mixer'),~,TimeDelay('BEOM+RefCav+mixer')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('BEOM+RefCav+mixer')),[],[2 3],Sigma,'BEOM+RefCav+mixer',FitParam,plotnoplot,'TimeDelay');

[z,p,k] = zpkdata(minreal(Model('BEOM+RefCav+mixer')*Fitting('BEOM+RefCav+mixer')/Model('RefCav')));
mixer = zpk([],p{1},prod(p{1}));
Model('BEOM') = zpk(z,-1e9,k*1e9/prod(p{1}));%*tf(1,1,'InputDelay',TimeDelay('BEOM+RefCav+mixer'));


% model_c = mnbode_list(Model('BEOM')*mixer*Model('RefCav')/(1+Model('OLG_FSS_PZTonly'))*tf(1,1,'InputDelay',TimeDelay('BEOM+RefCav+mixer')),Freq('BEOM+RefCav+mixer'),'c');
% mnbode(Freq('BEOM+RefCav+mixer'),Abs('BEOM+RefCav+mixer'),Phase('BEOM+RefCav+mixer'),Freq('BEOM+RefCav+mixer'),abs(model_c),angle(model_c)/pi*180,'linestyle',{'*','-'})

%% AOM efficiency
% lock the FSS with only using PZT.
% excite the frequency with AOM and measure the tf from AOM excitation
% signal to TTFSS OUT1

%fitting parameters
h_A_AOM = 1e7;
h_dt_AOM = 0.5e-6;
h_pole_AOM = 1.5e5;
h_zero_AOM = 5e6;

Model('AOM') = -tf(h_A_AOM*[1/h_zero_AOM 1],[1/h_pole_AOM/2/pi 1]);
Model('AOM+RefCav+mixer') = minreal(Model('AOM')*Model('RefCav')*Model('mixer'));
% model_c = mnbode_list(Model('AOM+RefCav+mixer'),Freq('AOM+RefCav+mixer'),'c');

% mnbode(Freq('AOM+RefCav+mixer'),Abs('AOM+RefCav+mixer'),Phase('AOM+RefCav+mixer'),...
%     Freq('AOM+RefCav+mixer'),abs(model_c),angle(model_c)*180/pi)
% [z,p,~] = zpkdata(Model('AOM+RefCav+mixer'));
% num2str(abs(z{1})/2/pi/1e3)
% num2str(abs(p{1})/2/pi/1e3)

[~,abs_fitting_data] = mntrim(4e4,3e6,Freq('AOM+RefCav+mixer'),Abs('AOM+RefCav+mixer'));
[ff_fitting_data,ang_fitting_data] = mntrim(4e4,3e6,Freq('AOM+RefCav+mixer'),Phase('AOM+RefCav+mixer'));
[Fitting('AOM+RefCav+mixer'),~,TimeDelay('AOM+RefCav+mixer')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('AOM+RefCav+mixer')),[1],[1],Sigma,'AOM+RefCav+mixer',FitParam,plotnoplot,'TimeDelay',printnoprint);
Model('AOM') = Model('AOM+RefCav+mixer')*Fitting('AOM+RefCav+mixer')/Model('RefCav')/Model('mixer');
% model_c = mnbode_list(Model('AOM')*Model('RefCav')*mixer/(1+Model('OLG_FSS_PZTonly'))*tf(1,1,'InputDelay',TimeDelay('AOM+RefCav+mixer')),Freq('AOM+RefCav+mixer'),'c');
% mnbode(Freq('AOM+RefCav+mixer'),Abs('AOM+RefCav+mixer'),Phase('AOM+RefCav+mixer'),Freq('AOM+RefCav+mixer'),abs(model_c),angle(model_c)/pi*180,'linestyle',{'*','-'})

%% Open loop gain of FSS loop
% measure the open loop gain of the FSS(this measurement has been done on
% the other day
FG_FSS_OLG = 15.4;
CG_FSS_OLG = 30;
SR560 = 5;
PZT_servo = Fitting('TTFSS_PZT')* Model('TTFSS_PZT') * 10^((CG_FSS_OLG)/20) * 10^((FG_FSS_OLG)/20) * SR560;
EOM_servo = Fitting('TTFSS_EOM')* Model('TTFSS_EOM') * 10^((CG_FSS_OLG)/20);
Model('OLG_FSS(OUT2-OUT1)') = minreal(Model('RefCav') * mixer * (PZT_servo *  Model('NPRO_PZT') + EOM_servo * Model('BEOM'))/TTFSS_Elec('common_gain1'));
% model_c = mnbode_list(Model('OLG_FSS(OUT2-OUT1)'),Freq('OLG_FSS(OUT2-OUT1)'),'c');
% mnbode(Freq('OLG_FSS(OUT2-OUT1)'),Abs('OLG_FSS(OUT2-OUT1)'),Phase('OLG_FSS(OUT2-OUT1)'),...
%     Freq('OLG_FSS(OUT2-OUT1)'),abs(model_c),angle(model_c)*180/pi)
% [z,p,~] = zpkdata(Model('OLG_FSS(OUT2-OUT1)'));
% num2str((z{1})/2/pi/1e3)
% num2str((p{1})/2/pi/1e3)
[~,abs_fitting_data] = mntrim(0,1.5e6,Freq('OLG_FSS(OUT2-OUT1)'),Abs('OLG_FSS(OUT2-OUT1)'));
[ff_fitting_data,ang_fitting_data] = mntrim(0,1.5e6,Freq('OLG_FSS(OUT2-OUT1)'),Phase('OLG_FSS(OUT2-OUT1)'));
[Fitting('OLG_FSS(OUT2-OUT1)'),~,TimeDelay('OLG_FSS(OUT2-OUT1)')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    minreal(Model('OLG_FSS(OUT2-OUT1)')),[],[],Sigma,'OLG_FSS(OUT2-OUT1)',FitParam,plotnoplot,'TimeDelay',printnoprint);

Model('OLG_FSS') = -Model('OLG_FSS(OUT2-OUT1)') * Fitting('OLG_FSS(OUT2-OUT1)') * TTFSS_Elec('common_gain1');

% model_c = mnbode_list(Model('OLG_FSS')*tf(1,1,'InputDelay',TimeDelay('OLG_FSS(OUT2-OUT1)')),Freq('OLG_FSS(OUT2-OUT1)'),'c');
% mnbode(Freq('OLG_FSS(OUT2-OUT1)'),Abs('OLG_FSS(OUT2-OUT1)')*-TTFSS_Elec('common_gain1'),Phase('OLG_FSS(OUT2-OUT1)'),...
%     Freq('OLG_FSS(OUT2-OUT1)'),abs(model_c),angle(model_c)*180/pi,'linestyle',{'*','-'})
%% Common mode servo
IN1GAIN_L = -16;
IN1GAIN_H = -14;
Model('CMS_AOM') = -minreal(CMS_Elec('common_comp') * CMS_Elec('slow_bst') * CMS_Elec('slow_LPF'));

[~,temp_abs] = mntrim(10,min(Freq('CMS_AOM_H')),Freq('CMS_AOM_L'),Abs('CMS_AOM_L'));
[temp_ff,temp_phase] = mntrim(10,min(Freq('CMS_AOM_H')),Freq('CMS_AOM_L'),Phase('CMS_AOM_L'));
Freq('CMS_AOM') = [temp_ff; Freq('CMS_AOM_H')];
Abs('CMS_AOM') = [temp_abs/10^(IN1GAIN_L/20); Abs('CMS_AOM_H')/10^(IN1GAIN_H/20)];
Phase('CMS_AOM') = [temp_phase; Phase('CMS_AOM_H')];
% model_c = mnbode_list(Model('CMS_AOM'),Freq('CMS_AOM'),'c');
% mnbode(Freq('CMS_AOM'),Abs('CMS_AOM'),Phase('CMS_AOM'));
% [z,p,~] = zpkdata(Model('CMS_AOM'));
% num2str(abs(z{1})/2/pi)
% num2str(abs(p{1})/2/pi)


abs_fitting_data = Abs('CMS_AOM');
ff_fitting_data = Freq('CMS_AOM');
ang_fitting_data = Phase('CMS_AOM');

[Fitting('CMS_AOM'),~,TimeDelay('CMS_AOM')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    Model('CMS_AOM'),[1 2],[1 3],Sigma,'CMS_AOM',FitParam,plotnoplot,printnoprint,'TimeDelay');
% 
% bode(minreal(Fitting('PZT_path')*Model('TTFSS_PZT')))

%% Common mode servo
IN1GAIN = -14;
RTS('K1IMC-MCL_SERVO_FB1') = zpk(-1*2*pi,-10*2*pi,10);
RTS('K1IMC-MCL_SERVO_FB2') = zpk(-0.5*2*pi,-0.05*2*pi,1);
RTS('K1IMC-MCL_SERVO_FB3') = zpk(-[100 100]*2*pi,-[10 10]*2*pi,1/10);

Model('CMS_SUS') = -minreal(CMS_Elec('common_comp') * CMS_Elec('slow_bst') * CMS_Elec('slow_mon_generic') * CMS_Elec('slow_mon_output') ...
    * Elec('Analog_AA') * ADC_V2C * Elec('Digital_AA') ...
    * RTS('K1IMC-MCL_SERVO_FB1') * RTS('K1IMC-MCL_SERVO_FB2') * RTS('K1IMC-MCL_SERVO_FB3')  * 0.2 * Fitting('CMS_AOM'));

% 
% model_c = mnbode_list(Model('CMS_SUS'),Freq('CMS_SUS'),'c');
% mnbode(Freq('CMS_SUS'),Abs('CMS_SUS'),Phase('CMS_SUS'));
% [z,p,~] = zpkdata(Model('CMS_SUS'));
% num2str(abs(z{1})/2/pi)
% num2str(abs(p{1})/2/pi)

abs_fitting_data = Abs('CMS_SUS')/10^(IN1GAIN/20);
ff_fitting_data = Freq('CMS_SUS');
ang_fitting_data = Phase('CMS_SUS');

[Fitting('CMS_SUS'),~,TimeDelay('CMS_SUS')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    Model('CMS_SUS'),[],[],Sigma,'CMS_SUS',FitParam,plotnoplot,printnoprint,'TimeDelay');

%% IMC optical gain fitting 
% estimate the optical gain and a cavity pole of reference cavity
% measure a TF from SR560 output to TTFSS OUT1 with lock the FSS only using
% PZT
%fitting parameters

h_H_IMC = -3e-6;
h_cp_IMC =5.6e3;%cavity pole

Model('AOM+FSS+IMC+IQdemod') = minreal(Model('AOM'))*tf(h_H_IMC,[1/h_cp_IMC/2/pi 1]);
% mnbode(Freq('AOM+FSS+IMC+IQdemod'),Abs('AOM+FSS+IMC+IQdemod'),Phase('AOM+FSS+IMC+IQdemod'));

[~,abs_fitting_data] = mntrim(0,3e4,Freq('AOM+FSS+IMC+IQdemod'),Abs('AOM+FSS+IMC+IQdemod'));
[ff_fitting_data,ang_fitting_data] = mntrim(0,3e4,Freq('AOM+FSS+IMC+IQdemod'),Phase('AOM+FSS+IMC+IQdemod'));
[Fitting('AOM+FSS+IMC+IQdemod'),~,TimeDelay('AOM+FSS+IMC+IQdemod')] = ...
    mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    (Model('AOM+FSS+IMC+IQdemod')),[],[2],Sigma,'AOM+FSS+IMC+IQdemod',FitParam,'Plot','TimeDelay',printnoprint);
Model('IMC') = Model('AOM+FSS+IMC+IQdemod')* Fitting('AOM+FSS+IMC+IQdemod') / minreal(Model('AOM'));
% model_c = mnbode_list(CavLPF('IMC') * Act('AOM') * tf(1,1,'InputDelay',TimeDelay('AOM+FSS+IMC+IQdemod')) * a * Model('OLG_FSS')*tf(1,1,'InputDelay',TimeDelay('OLG_FSS(OUT2-OUT1)')) / (1+a*Model('OLG_FSS')*tf(1,1,'InputDelay',TimeDelay('OLG_FSS(OUT2-OUT1)'))),Freq('AOM+FSS+IMC+IQdemod'),'c');
% mnbode(Freq('AOM+FSS+IMC+IQdemod'),Abs('AOM+FSS+IMC+IQdemod'),Phase('AOM+FSS+IMC+IQdemod'),...
%     Freq('AOM+FSS+IMC+IQdemod'),abs(model_c),angle(model_c)*180/pi);

%% FSS open loop gain (with IMC)



IN1GAIN = -14;
AOM_servo = Fitting('CMS_AOM')* Model('CMS_AOM') * 10^((IN1GAIN)/20);
Model('OLG_IMC_AOM') = minreal(AOM_servo * Model('AOM') * Model('IMC'));% * tf(1,1,'InputDelay',TimeDelay('AOM+FSS+IMC+IQdemod')));
Model('OLG_FSS_with_IMC') = minreal((1+Model('OLG_IMC_AOM')) * Model('OLG_FSS'));


% model_c = mnbode_list(Model('OLG_FSS_with_IMC'),Freq('OLG_FSS_with_IMC'),'c');
% mnbode(Freq('OLG_FSS_with_IMC'),Abs('OLG_FSS_with_IMC')*-TTFSS_Elec('common_gain1'),Phase('OLG_FSS_with_IMC'),...
%     Freq('OLG_FSS_with_IMC'),abs(model_c),angle(model_c)/pi*180);
% [z,p,~] = zpkdata(Model('OLG_FSS_with_IMC'));
% num2str(abs(z{1})/2/pi/1e3)
% num2str(abs(p{1})/2/pi/1e3)

[~,abs_fitting_data] = mntrim(0,1.5e6,Freq('OLG_FSS_with_IMC'),Abs('OLG_FSS_with_IMC')*-TTFSS_Elec('common_gain1'));
[ff_fitting_data,ang_fitting_data] = mntrim(0,1.5e6,Freq('OLG_FSS_with_IMC'),Phase('OLG_FSS_with_IMC'));
[Fitting('OLG_FSS'),Fitting('OLG_IMC_AOM'),TimeDelay('OLG_FSS'),TimeDelay('OLG_IMC_AOM')] = FSS_IMC_OLG_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,Model('OLG_FSS'),Model('OLG_IMC_AOM'),'Plot',printnoprint);
% [Fitting('OLG_FSS_with_IMC'),~,TimeDelay('OLG_FSS_with_IMC')] = mnTF_fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,minreal(Model('OLG_FSS_with_IMC')),[1 2 9 10 19],[],plotnoplot,'TimeDelay',printnoprint);

% %% IMC open loop gain (with FSS)
% 
% model_fss = Fitting('OLG_FSS') * Model('OLG_FSS') * tf(1,1,'InputDelay',TimeDelay('OLG_FSS'));
% model_imc_aom = Fitting('OLG_IMC_AOM') * Model('OLG_IMC_AOM') * tf(1,1,'InputDelay',TimeDelay('OLG_IMC_AOM'));
% model_imc_fss = model_imc_aom*model_fss/(1+model_fss);
% model_imc_fss_c = mnbode_list(model_imc_fss,Freq('OLG_IMC'),'c');
% % 
% mnbode(Freq('OLG_IMC'),Abs('OLG_IMC'),Phase('OLG_IMC'),...
%     Freq('OLG_IMC'),abs(model_imc_fss_c),angle(model_imc_fss_c)/pi*180);

%% Suspension actuator efficiency

c_fitting_data = Abs('CROSS_IMC').*exp(1i*Phase('CROSS_IMC')/180*pi)...
    .*mnbode_list(AOM_servo * Model('AOM'),Freq('CROSS_IMC'),'c')...
    ./mnbode_list(Fitting('CMS_SUS')* Model('CMS_SUS') /Elec('Analog_AI')/Elec('Digital_AI'),Freq('CROSS_IMC'),'c')/DAC_C2V;

[sus_m,sus_n] = invfreqs(c_fitting_data,Freq('CROSS_IMC')*2*pi,0,2);
Model('Type-C_ALL') = tf(sus_m,sus_n);
model_c = mnbode_list(tf(sus_m,sus_n),Freq('CROSS_IMC'),'c');
mnbode(Freq('CROSS_IMC'),abs(c_fitting_data),angle(c_fitting_data)*180/pi,Freq('CROSS_IMC'),abs(model_c),angle(model_c)*180/pi,'linestyle',{'*','-'})
% %% MC open loop gain
% Model('OLG_IMC_ALL') = model_imc_fss + Model('SUS_ALL') * Model('IMC') * Fitting('CMS_SUS')* Model('CMS_SUS');
% bode(Model('OLG_IMC_ALL'))

%% Define outputs
CavLPF('IMC') = Model('IMC');
CavLPF('RefCav') = Model('RefCav');
Act('NPRO_TEMP') = Model('NPRO_TEMP');
TimeDelay('OLG_FSS_PZTonly(OUT2-OUT1)')
Act('NPRO_PZT') = Model('NPRO_PZT') * tf(1,1,'InputDelay',TimeDelay('OLG_FSS_PZTonly(OUT2-OUT1)'));
Act('BEOM') = Model('BEOM') * tf(1,1,'InputDelay',TimeDelay('OLG_FSS'));
Act('AOM') = -Model('AOM') * tf(1,1,'InputDelay',TimeDelay('OLG_IMC_AOM'));
Act('Type-C') = Model('Type-C_ALL');
FittingOut('TTFSS_PZT') = Fitting('TTFSS_PZT');
FittingOut('TTFSS_EOM') = Fitting('TTFSS_EOM');
FittingOut('TTFSS_TEMP') = 1;
FittingOut('CMS_AOM') = Fitting('CMS_AOM');
FittingOut('CMS_SUS') = Fitting('CMS_SUS');
end