function tf_AOM = Fit_AOM_Response( Fitted_Servo_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;


for ii = 1:5
    mnimportdata(['T0907_' num2str(ii+1) '.CSV'],Freq,Abs,Phase,['AOM_OUT' num2str(ii)],'tf');
end

CG = 0;
FG = 0;
Servo('TTFSS_PZT') = Fitted_Servo_0dB('TTFSS_PZT')*10^(CG/20)*10^(FG/20);
n = 5;
fit_H = ones(1,n);
sigma_H = zeros(1,n);
fit_aompole = zeros(1,n);
sigma_aompole = zeros(1,n);
temp_dt = zeros(1,n);
sigma_dt = zeros(1,n);
for ii = 1:n
    if ii == n
        tempdoplot = doplot;
    else
        tempdoplot = '';
    end
    [fit_H(ii), sigma_H(ii), fit_aompole(ii), sigma_aompole(ii), temp_dt(ii), sigma_dt(ii)]...
        = mn_estimate_H(Freq(['AOM_OUT' num2str(ii)]),Abs(['AOM_OUT' num2str(ii)]),Phase(['AOM_OUT' num2str(ii)]),...
        -2.6e-6*tf(1,[1/Cav('Cpole_RefCav')/2/pi 1]),1.65e5,Act('NPRO_PZT'),...
        Act('Aaom'),Servo('TTFSS_PZT'),2e6,tempdoplot);
end


Act('AOM_pole') = mean(fit_aompole);
Act_Sigma('AOM_pole') = std(fit_aompole);
tf_AOM = tf(Act('Aaom'),[1/Act('AOM_pole')/2/pi 1]);
end

function [fit_Hopgain, sigma_Hopgain, fit_aompole, sigma_aompole, dt, sigma_dt]...
    = mn_estimate_H(freq, gain, phase, predict_H, predict_aompole, Apzt, Aaom,servo_model,mixerpole,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
mixer_str = ['1/(1i*w/(' num2str(mixerpole*2*pi) ')+1)'];
AOM_str = [num2str(Aaom) '/(1i*w/(' num2str(predict_aompole*2*pi) '*(1+dpole_aom))+1)'];
PZT_str = num2str(Apzt);
[z,p,k] = zpkdata(servo_model);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
servo_str = make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index);
[z,p,k] = zpkdata(predict_H);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
Hrefcav_str = ['(1+dH)*' make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index)];


fit_str = ['abs(' Hrefcav_str '*' mixer_str '*' AOM_str '/'...
    '(1+' Hrefcav_str '*' mixer_str '*' servo_str '*' PZT_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. ];
opts.Robust = 'LAR';
% Fit model to data.
[fitresult, ~] = fit( xData, yData, ft, opts );
fit_aompole = predict_aompole*(1+fitresult.dpole_aom);
fit_mixerpole = mixerpole;
fit_H = (1+fitresult.dH)*predict_H;
fit_Hopgain = mnbode_list(fit_H,0);
AOM_tf = tf(Aaom,[1/fit_aompole/2/pi 1]);
mixer_tf = tf(1,[1/fit_mixerpole/2/pi 1]);
fit_model = fit_H*AOM_tf*mixer_tf / (1+fit_H*servo_model*Apzt*mixer_tf);

sigma = confint(fitresult,0.6827);
namelist = coeffnames(fitresult);
sigma_Hopgain = abs(mnbode_list(predict_H,0)*(fitresult.dH-sigma(mnismember(namelist,'dH'))));
sigma_aompole = abs(predict_aompole*(fitresult.dpole_aom-sigma(mnismember(namelist,'dpole_aom'))));


[z,p,k] = zpkdata(fit_model);
[real_zero_index,comp_zero_index] = class_real_comp(z{1});
[real_pole_index,comp_pole_index] = class_real_comp(p{1});
str_olg = make_tf_str(z{1},p{1},k,[],[],real_zero_index,comp_zero_index,real_pole_index,comp_pole_index);
str_fit = ['unwrap(angle(' str_olg '*exp(-1i*w*dt)))'];
fitting_data_c = gain.*exp(1i*phase*pi/180);
fitting_angle = unwrap(angle(fitting_data_c));

[xData, yData] = prepareCurveData( freq*2*pi, fitting_angle );
% Set up fittype and options.
ft = fittype( str_fit, 'independent', 'w', 'dependent', 'y' );
opts = fitoptions( 'Method', 'NonlinearLeastSquares' );
opts.Display = 'Off';
opts.StartPoint = [0.];
opts.Robust = 'LAR';
% Fit model to data.
[fitresult, ~] = fit( xData, yData, ft, opts );
dt = fitresult.dt;
sigma = confint(fitresult,0.6827);
sigma_dt = sigma(1);


fit_model = fit_model*tf(1,1,'InputDelay',dt);


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


end