function [suspension, act] = Fit_MCsus_Response(RTS, FitServo_0dB, Cav, Act, Act_Sigma, varargin)
%UNTITLED9 Summary of this function goes here
%   Detailed explanation goes here
% plot or not
if mnismember(varargin,'Plot')
    doplot = 'Plot';
else
    doplot = 'noPlot';
end

Freq = containers.Map;
Abs = containers.Map;
Phase = containers.Map;
Servo = containers.Map;

% Servo model
IN1GAIN = mn_CMSgain(-10);
RTS('K1IMC-MCL_SERVO_GAIN') = -0.2;
Servo('CMS_AOM') = FitServo_0dB('CMS_AOM')*10^(IN1GAIN/20);
Servo('CMS_SUS') = FitServo_0dB('CMS_SUS_analog')*FitServo_0dB('CMS_SUS_digital')*10^(IN1GAIN/20)*RTS('K1IMC-MCL_SERVO_GAIN');

fit_Asus = zeros(1,6);
sigma_Asus = zeros(1,6);
fit_f0 = zeros(1,6);
sigma_f0 = zeros(1,6);
fit_Q = zeros(1,6);
sigma_Q = zeros(1,6);

% importdata
mnimportdata('O0901_30.txt',Freq,Abs,Phase,'CROSS_IMC_MCE1','tf');
% fit
[fit_Asus(1), sigma_Asus(1), fit_f0(1), sigma_f0(1), fit_Q(1), sigma_Q(1)] = ...
    mn_estimate_Asus(Freq('CROSS_IMC_MCE1'), Abs('CROSS_IMC_MCE1'), Phase('CROSS_IMC_MCE1'),...
    Cav('LPF_IMC_0901'), 3e4, 1, 5, Servo('CMS_AOM'),Servo('CMS_SUS'),Act('Aaom'),doplot);

for ii = 1:5
    mnimportdata(['O0913_' num2str(ii) '.txt'],Freq,Abs,Phase,['CROSS_IMC_MCE' num2str(ii+1)],'tf');
    if ii == 2
      [~,Abs('CROSS_IMC_MCE3')] = mntrim(0.2657,2e5,Freq('CROSS_IMC_MCE3'),Abs('CROSS_IMC_MCE3'));
      [Freq('CROSS_IMC_MCE3'),Phase('CROSS_IMC_MCE3')] = mntrim(0.2657,2e5,Freq('CROSS_IMC_MCE3'),Phase('CROSS_IMC_MCE3'));
    end
    
    [fit_Asus(ii+1), sigma_Asus(ii+1), fit_f0(ii+1), sigma_f0(ii+1), fit_Q(ii+1), sigma_Q(ii+1)] = ...
    mn_estimate_Asus(Freq(['CROSS_IMC_MCE' num2str(ii+1)]), Abs(['CROSS_IMC_MCE' num2str(ii+1)]), Phase(['CROSS_IMC_MCE' num2str(ii+1)]),...
    Cav('LPF_IMC_0901'), 3e4, 1, 5, Servo('CMS_AOM'),Servo('CMS_SUS'),Act('Aaom'),'');
end

Act('MCE_Asus') = mean(fit_Asus);
Act('MCE_f0') = mean(fit_f0);
Act('MCE_Q') = mean(fit_Q);
Act_Sigma('MCE_Asus') = std(fit_Asus);
Act_Sigma('MCE_f0') = std(fit_f0);
Act_Sigma('MCE_Q') = std(fit_Q);
suspension = tf(1,[1/(Act('MCE_f0')*2*pi)^2 1/(Act('MCE_f0')*2*pi)/Act('MCE_Q') 1]);
act = Act('MCE_Asus') * suspension;
end

function [fit_Asus, sigma_Asus, fit_f0, sigma_f0, fit_Q, sigma_Q] = mn_estimate_Asus(freq, gain, phase, Himc, predict_Asus, predict_resfreq, predict_Q, servo_model_aom,servo_model_sus,Aaom,varargin)
%mn_estimate_Apzt
%  the script to estimate the actuator efficiency of NPRO PZT
[xData, yData] = prepareCurveData( freq*2*pi, gain );

% create string of fitting equation
[z,p,k] = zpkdata(Himc);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
Himc_str = make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index);

w0_str = [num2str(predict_resfreq*2*pi) '*(1+df0)'];
Q_str = [num2str(predict_Q) '*(1+dQ)'];
SUS_str = [num2str(predict_Asus) '*(1+dAsus)/((1i*w)^2/(' w0_str ')^2+1i*w/(' w0_str ')/(' Q_str ')+1)'];
AOM_str = num2str(Aaom);
[z,p,k] = zpkdata(servo_model_aom);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
servo_str_aom = make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index);
[z,p,k] = zpkdata(servo_model_sus);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
servo_str_sus = make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index);

fit_str = ['abs(-' Himc_str '*' servo_str_sus '*' SUS_str '/'...
    '(1+' Himc_str '*' servo_str_aom '*' AOM_str '))'];

% Set up fittype and options.
ft = fittype( fit_str, 'independent', 'w', 'dependent', 'y' );
opts = fitoptions( 'Method', 'NonlinearLeastSquares' );
opts.Display = 'Off';
opts.StartPoint = [0. 0. 0.];
opts.Robust = 'LAR';
% Fit model to data.
[fitresult, gof] = fit( xData, yData, ft, opts );
fit_Asus = (1+fitresult.dAsus)*predict_Asus;
fit_f0 = predict_resfreq*(1+fitresult.df0);
fit_w0 = 2*pi*fit_f0;
fit_Q = predict_Q*(1+fitresult.dQ);
fit_sus = tf(fit_Asus,[1/fit_w0^2 1/fit_w0/fit_Q 1]);
sys_sus = -Himc*servo_model_sus*fit_sus / (1+Himc*servo_model_aom*Aaom);

if mnismember(varargin,'Plot')
model_c = mnbode_list(sys_sus,freq,'c');
mnbode(freq,gain,phase,freq,abs(model_c),angle(model_c)*180/pi,...
    'linestyle',{'*','-'},'title','G_{sus}/(1+G_{aom})','xlim',[min(freq) max(freq)],'legend',{'measured','fitted'});
end

sigma = confint(fitresult,0.6827);
namelist = coeffnames(fitresult);
sigma_Asus = abs(fitresult.dAsus-sigma(mnismember(namelist,'dAsus')))*predict_Asus;
sigma_f0 = abs(fitresult.df0-sigma(mnismember(namelist,'df0')))*predict_resfreq;
sigma_Q = abs(fitresult.dQ-sigma(mnismember(namelist,'dQ')))*predict_Asus;
end