%% preparation
clear all
rootpath = findNbFSSroot_addpath(fileparts(mfilename('fullpath')));
%%
% make containers for calculation
Servo_0dB = containers.Map;
Servo_FitParam = containers.Map;
Servo_Sigma = containers.Map;

Act = containers.Map;
Act_Sigma = containers.Map;
Suspension = containers.Map;

Cav = containers.Map;
Cav_Sigma = containers.Map;

dt = containers.Map;
dt_Sigma = containers.Map;

c = 299792458;
%% path
addpath('Servo','Servo/data')
addpath('Actuator','Actuator/data')
addpath('Cavity','Cavity/data')
addpath('Common')


%% Analog Servo filters
TTFSS_Elec = TTFSS_filters(1);
CMS_Elec = CMS_filters();
FB = IOO_FB_list();
AA_AI = AA_AI_filters();

%% TTFSS servo fitting
% 1. fitting datas
% PZT path T0712_1G(P).CSV, EOM path T0712_2G(P).CSV
% 2. fitting parameters
% PZT path: overall gain(TTFSS_PZT_G), 10kHz/34kHz pole(TTFSS_PZT_10kHz/34kHz_pole)
% EOM path: overall gain(TTFSS_EOM_G), 2kHz/1kHz/33kHz pole(TTFSS_PZT_2kHz/1kHz/33kHz_pole)
% 3, other output param name: 
% FitParam('TTFSS_EOM_2kHz_pole')
% Sigma('TTFSS_EOM_2kHz_pole')
% FitParam('TTFSS_EOM_1kHz_pole')
% Sigma('TTFSS_EOM_1kHz_pole')
% FitParam('TTFSS_EOM_33kHz_pole')
% Sigma('TTFSS_EOM_33kHz_pole')
% FitParam('TTFSS_EOM_G')
% Sigma('TTFSS_EOM_G')
close all
[TTFSS_Elec('FitStage_PZT'),TTFSS_Elec('FitStage_EOM'),Servo_0dB('TTFSS_PZT'),Servo_0dB('TTFSS_EOM'),Servo_0dB('TTFSS_SLOW')]...
    = Fit_TTFSS( TTFSS_Elec, AA_AI, Servo_FitParam, Servo_Sigma, 'Plot');

%% Common mode servo Fitting
% Other output names
% Servo_Sigma('CMS_AOM_4kHz_zero')
% Servo_Sigma('CMS_AOM_400Hz_zero')
% Servo_Sigma('CMS_AOM_40Hz_pole')
% Servo_Sigma('CMS_AOM_100kHz_pole')
close all
[CMS_Elec('FitStage_AOM'),Servo_0dB('CMS_AOM'), Servo_0dB('CMS_SUS_analog'), Servo_0dB('CMS_SUS_digital'),...
    Servo_0dB('CMS_SUS_analog_susonly'),Servo_0dB('CMS_SUS_digital_susonly'),...
    Servo_0dB('CMS_SUS_analog_susonly_bst'),Servo_0dB('CMS_SUS_digital_susonly_bst')] =...
    Fit_CMS( AA_AI, CMS_Elec, FB, Servo_FitParam, Servo_Sigma, 'Plot' );
%% Estimate the actuator effieicency ofAOM
% 1. fitting datas
% P0727.txt
% 2. measurement
% klog 3172
% 3. fitting parameter
% DC efficiency of AOM(Aaom) [Hz/V]
% 4. output param: Act('Aaom'), Act_Sigma('Aaom')
close all
[Act('Aaom'), Act_Sigma('Aaom')] = Fit_Aaom('Plot');


%% Estimate the actuatior efficiency of PZT
% Estimate a NPRO PZT actuator efficiency by ratio by a AOM actuator
% efficiency
% 1. fitting datas
% T0831_1-3G(P).CSV,O0904_1-4G(P).CSV
% 2.fitting parameters
% DC efficiency of PZT(Apzt) [Hz/V]

close all
[Act('NPRO_PZT'), Act_Sigma('NPRO_PZT')] = Fit_Apzt( Servo_0dB, Act, Act_Sigma, 'Plot' );

%% optical gain of RefCav estimation
% cavity pole
% measured TF from REFCAV DC REFL to REFCAV DC TRANS with AM
% see klog2837
% 2. fitting parameter
% Cavity pole of refcav [Hz]
% 3. otheroutput param names
% Cav('Finnese_RefCav')
% Cav('r_RefCav')
% Cav('R_RefCav')
% Cav('T_RefCav')
% Cav('L_RefCav')
[Cav('Cpole_RefCav'), Cav_Sigma('Cpole_RefCav')] = ...
    Fit_Cavpole_RefCav( Cav, Cav_Sigma, 'Plot' );

%% Fit Actuator response of AOM
% lock with PZT only
% FG:	0, CG: 0
% Actuate the laser frequency by AOM
% measured a TF from AOM driver in to OUT1
% output param names
% Act('AOM_pole')
% Act_Sigma('AOM_pole')
% Act(AOM)
close all
Act('AOM') = Fit_AOM_Response(Servo_0dB, Cav, Act, Act_Sigma, 'Plot');


%% Estimate the optical gain of RefCav and timedalay of PZT loop with OLG
close all
[Cav('Opgain_RefCav_0901'), Cav_Sigma('Opgain_RefCav_0901'), Cav('LPF_RefCav_0901'),...
    dt('NPRO_PZT'), dt_Sigma('NPRO_PZT')]...
    = Fit_Hrefcav_0901(TTFSS_Elec, Servo_0dB, Act, Cav,'Plot');

%% Estimate the actuator response of BEOM
close all
[Act('Aeom'), Act_Sigma('Aeom'), Act('BEOM')] = Fit_Aeom(Servo_0dB, Act, Act_Sigma, Cav, 'Plot');

%% Estimate the actuator response of NPRO temperature control
close all
% [Act('Atemp'), Act_Sigma('Atemp'), Act('TEMP_pole'), Act_Sigma('TEMP_pole'), dt('NPRO_TEMP'), dt_Sigma('NPRO_TEMP'), Act('NPRO_TEMP')]...
%     = Fit_Atemp(Servo_0dB, Act, Cav, 'Plot');
 

%% Estimation of the mixer poles and timedlay in FSS loop from Open loop TF
close all
[Cav('MixerPole_RefCav'), Cav_Sigma('MixerPole_RefCav'), dt('BEOM'),dt_Sigma('BEOM'), Cav('Mixer_RefCav')] ...
    = Fit_MixerPole(TTFSS_Elec, Servo_0dB, Cav, Act,'Plot');
TTFSS_Elec('mixer') = mnzpk([],-[Cav('MixerPole_RefCav') Cav('MixerPole_RefCav')],1);


%% Estimate the cavity pole of IMC
% 1.measurement
% Lock the IMC and measured the transfer function from CMS SLOW OUT to
% output of IQ demod.
% 2.measurement data
%  T0901_41-50
% 3. otheroutput param names
% Cav('Finnese_IMC')
% Cav('r_IMC')
% Cav('R_IMC')
% Cav('T_IMC')
% Cav('L_IMC')
close all
[Cav('Cpole_IMC'), Cav_Sigma('Cpole_IMC')] = Fit_Cavpole_IMC(Servo_0dB, Act, Cav, Cav_Sigma, 'Plot' );


%% Estimate the optical gain of IMC and timedalay of AOM loop with OLG
close all
[Cav('Opgain_IMC_0901'), Cav_Sigma('Opgain_IMC_0901'), Cav('LPF_IMC_0901'),...
    dt('AOM'), dt_Sigma('AOM')]...
    = Fit_Himc_0901(FB, Servo_0dB, Act, Cav, 'Plot');

%% MC suspensions actuator efficiency measurement
% lock IMC with AOM and each MC suspension. The servo parameters are same
% in each suspensions.
% measured at fb:K1:IMC-MCL_SERVO.
close all
[Suspension('MCE'), Act('MCEsus')] = Fit_MCsus_Response(FB, Servo_0dB, Cav, Act, Act_Sigma,'Plot');


%% IMC optical gain estimation on 11/02
close all
[Cav('Opgain_IMC_1102'), Cav_Sigma('Opgain_IMC_1102'), Cav('LPF_IMC_1102'),...
    dt('MCEsus'), dt_Sigma('MCEsus')]...
    = Fit_Himc_1102(FB, Servo_0dB, Act, Cav, 'Plot');

%% IMC optical gain estimation on 11/22
close all
[Cav('Opgain_IMC_1122'), Cav_Sigma('Opgain_IMC_1122'), Cav('LPF_IMC_1122'),...
    dt('MCEsus'), dt_Sigma('MCEsus')]...
    = Fit_Himc_1122(FB,Servo_0dB, Act, Cav, 'Plot');

%% RefCav optical gain estimation on 11/22
close all

[Cav('Opgain_RefCav_1122'), Cav_Sigma('Opgain_RefCav_1122'), Cav('LPF_RefCav_1122'),...
    ~, ~]...
    = Fit_Hrefcav_1122(TTFSS_Elec, Servo_0dB, Act, Cav,dt,'Plot');

%% RefCav optical gain estimation on 11/24
% close all

[Cav('Opgain_RefCav_1124'), Cav_Sigma('Opgain_RefCav_1124'), Cav('LPF_RefCav_1124'),...
    ~, ~]...
    = Fit_Hrefcav_1124(TTFSS_Elec, Servo_0dB, Act, Cav,dt,'Plot');


%% IMC optical gain estimation on 11/24
close all
[Cav('Opgain_IMC_1124'), Cav_Sigma('Opgain_IMC_1124'), Cav('LPF_IMC_1124'),~,~]...
    = Fit_Himc_1124(FB,Servo_0dB, Act, Cav, 'Plot');

%% IMC optical gain estimation on 11/30
close all
[Cav('Opgain_IMC_1130'), Cav_Sigma('Opgain_IMC_1130'), Cav('LPF_IMC_1130'),...
    dt('MCEsus'), dt_Sigma('MCEsus')]...
    = Fit_Himc_1130(FB,Servo_0dB, Act, Cav, 'Plot');

%% Angle to length coupling of IMC mirrors
set_suspensionA2L( Suspension, Act, Servo_0dB, Cav)

%% Other parameters
Act('NPRO_TEMP') = 0;
dt('NPRO_TEMP') = 0;

%% CARM
Cav('T_ITM') = 0.004;
Cav('L_ITM') = 45e-6;
Cav('R_ITM') =  1-Cav('T_ITM')-Cav('L_ITM');
Cav('r_ITM') = sqrt(Cav('R_ITM'));
Cav('T_ETM') = 7.5e-6;
Cav('L_ETM') = 45e-6;
Cav('R_ETM') =  1-Cav('T_ETM')-Cav('L_ETM');
Cav('r_ETM') = sqrt(Cav('R_ETM'));
Cav('Finnese_CARM') = pi*sqrt(Cav('r_ITM')*Cav('r_ETM'))/(1-Cav('r_ITM')*Cav('r_ETM'));
Cav('Cpole_CARM') = c/4/3e3/Cav('Finnese_CARM');
Cav('Opgain_CARM') = 7e-6;
Cav('LPF_CARM') = mnzpk([],-[Cav('Cpole_CARM')],Cav('Opgain_CARM'));
%% Save parameters
save([rootpath filesep 'IOO_Parameter/IOO_params'],'Servo_0dB','Act','Cav','Suspension','Act_Sigma','Cav_Sigma','dt','dt_Sigma','TTFSS_Elec','CMS_Elec','AA_AI','FB')