function [fitting_cms_aom, fitservo_0dB_aom, fitservo_0dB_sus_analog, fitservo_0dB_sus_digital,...
    fitservo_0dB_sus_analog2, fitservo_0dB_sus_digital2,fitservo_0dB_sus_analog_bst, fitservo_0dB_sus_digital_bst]...
    = Fit_CMS( Elec, CMS_Elec, RTS, Servo_FitParam, Servo_Sigma, varargin )

Model = containers.Map;
Freq = containers.Map;
Abs = containers.Map;
Phase = containers.Map;
temp_Fitparam = containers.Map;
temp_Sigma = containers.Map;

% plot or not
if mnismember(varargin,'Plot')
    doplot = true;
else
    doplot = false;
end

IN1GAIN_L = mn_CMSgain(-16);
IN1GAIN_H = mn_CMSgain(-14);

% Common mode servo SLOW path
Model('CMS_AOM') = -minreal(CMS_Elec('common_comp') * CMS_Elec('slow_bst') * CMS_Elec('slow_LPF'));

% TF from IN1 to SLOW out of CMS
data = importdata('T0707_17G.CSV');
Freq('CMS_AOM_H') = data.data(:,1);
Abs('CMS_AOM_H') = 10.^(data.data(:,2)/20);
data = importdata('T0707_17P.CSV');
Phase('CMS_AOM_H') = data.data(:,2);

data = importdata('T0705_5.txt');
Freq('CMS_AOM_L') = data(:,1);
Abs('CMS_AOM_L') = data(:,2);
Phase('CMS_AOM_L') = data(:,3);

[~,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,~,dt] = ...
    TF_Fit(ff_fitting_data,abs_fitting_data,ang_fitting_data,...
    Model('CMS_AOM'),[1 2],[1 3],temp_Sigma,'CMS_AOM',temp_Fitparam,doplot,'TimeDelay','title','CMS AOM');

Servo_FitParam('CMS_AOM_4kHz_zero') = temp_Fitparam('CMS_AOM_zero1');
Servo_FitParam('CMS_AOM_400Hz_zero') = temp_Fitparam('CMS_AOM_zero2');
Servo_FitParam('CMS_AOM_40Hz_pole') = temp_Fitparam('CMS_AOM_pole1');
Servo_FitParam('CMS_AOM_100kHz_pole') = temp_Fitparam('CMS_AOM_pole3');
Servo_Sigma('CMS_AOM_4kHz_zero') = temp_Sigma('CMS_AOM_zero1');
Servo_Sigma('CMS_AOM_400Hz_zero') = temp_Sigma('CMS_AOM_zero2');
Servo_Sigma('CMS_AOM_40Hz_pole') = temp_Sigma('CMS_AOM_pole1');
Servo_Sigma('CMS_AOM_100kHz_pole') = temp_Sigma('CMS_AOM_pole3');

close all

fitservo_0dB_aom = minreal(Model('CMS_AOM')*fitting_cms_aom);
if mnismember(varargin,'Plot')
    close all
    freq = 10.^(0:0.02:7);
    model_c = mnbode_list(fitservo_0dB_aom*tf(1,1,'InputDelay',dt),freq,'c');
    mnbode(ff_fitting_data,abs_fitting_data,angle(exp(1i*pi/180*ang_fitting_data))*180/pi,...
        freq,abs(model_c),angle(model_c)*180/pi,...
        'ylim',[1e-3 1e4],'xlim',[1 1e7],'title','CMS IN1 to SLOW (IN1GAIN = 0dB)','linestyle',{'*','-'},'legend',{'measured','fitted'})
end


% Common mode servo suspension path
% TF from IN1 to K1:IMC_MCL_SERVO_OUT
data = importdata('T0713_23.txt');
Freq('CMS_SUS') = data(:,1);
Abs('CMS_SUS') = data(:,2);
Phase('CMS_SUS') = data(:,3);

IN1GAIN = mn_CMSgain(-14);
ADC_V2C = 2^16/40; %ADC factor [cnts/V]

fitservo_0dB_sus_analog = minreal(CMS_Elec('common_comp') * CMS_Elec('slow_bst') * CMS_Elec('slow_mon_generic') * CMS_Elec('slow_mon_output') ...
    * Elec('Analog_AA')*fitting_cms_aom*ADC_V2C*Elec('Digital_AA'))*2*fitting_cms_aom;
fitservo_0dB_sus_digital = IMC_RTS_SERVO(RTS, [1 2 10]);
Model('CMS_SUS') = -minreal(fitservo_0dB_sus_analog*fitservo_0dB_sus_digital);

RTS('K1:IMC-MCL_SERVO_GAIN') = 0.2;
abs_fitting_data = Abs('CMS_SUS')/10^(IN1GAIN/20)/RTS('K1:IMC-MCL_SERVO_GAIN');
ff_fitting_data = Freq('CMS_SUS');
ang_fitting_data = Phase('CMS_SUS');


if mnismember(varargin,'Plot')
    close all
    freq = 10.^(-2:0.02:3);
    model_c = mnbode_list(Model('CMS_SUS'),freq,'c');
    mnbode(ff_fitting_data,abs_fitting_data,angle(exp(1i*pi/180*ang_fitting_data))*180/pi,...
        freq,abs(model_c),angle(model_c)*180/pi,...
        'xlim',[0.01 1e3],'title','CMS IN1 to MCL-SERVO-OUT (IN1GAIN = 0dB)','linestyle',{'*','-'},'legend',{'measured','fitted'})
end

% servo filters when suspension locking
fitservo_0dB_sus_digital2 = IMC_RTS_SERVO(RTS, [3 10]);
fitservo_0dB_sus_analog2 = -minreal(CMS_Elec('slow_mon_generic') * CMS_Elec('slow_mon_output') ...
    * Elec('Analog_AA')*ADC_V2C*Elec('Digital_AA'))*2*fitting_cms_aom;
% servo filters when suspension locking
fitservo_0dB_sus_digital_bst = IMC_RTS_SERVO(RTS, [3 4 10]);
fitservo_0dB_sus_analog_bst = -minreal(CMS_Elec('slow_mon_generic') * CMS_Elec('slow_mon_output') ...
    * Elec('Analog_AA')*ADC_V2C*Elec('Digital_AA'))*2*fitting_cms_aom;
end