% Noise budget  PLL test script
% Originally made by S. Kambara, largely modified by Y. Enomoto in 2018 
% $Id: run_NBGREEN_ONEARM.m 2440 2019-03-09 06:31:33Z lcgt $
% This version (XARM) was initiated on 2018/12/5 by YE

% For X arm commissioning held in the end of 2018. The following changes in
% the servo topology are made.
% Around CARM stability, CARM 1/2 --> 1, CARM2 1/2 --> 0 in .mdl file
% Matrix named Sensing [1/2,1/2;1,-1] --> [1,0;0,0];
% Matrix named Actuator [1,1/2;1,-1/2] --> [1,0;0,0]
% CARM_Cavity_Pole 0.8 --> 17
% IMC sensing gain has been changed, and IMC filter settings as well
% figure(99), when plotting Due to IMC, divide by 2 is omitted

%% Add paths
clear all
close all
addpath('../');
findNbSVNroot;
addpath(genpath([NbSVNroot 'Common/Utils']));
addpath(genpath([NbSVNroot 'Dev/Utils/']));
cd([NbSVNroot,'GreenLock/nbGREEN_ONEARM']);

%% What to plot

plot_TFs = 1;
plot_NBs = 1;
plot_Saturations = 0;
plot_CARMlock = 1;

turb = 1; % turbulence?

%% Select servo topology

use_AOM = 1; % use AOM as a freq actuator of green, or use VCO option of PLL LO instead?

asym = 0.1; % asymmetry between X and Y green lock

use_X_as_CARM = 0;

realCARM = 0;

X = 0; % X arm or Y arm ?
if X
    name = 'X';
else
    name = 'Y';
end
%% Define some filters 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PLL%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Changing_unit = zpk([],[0],2*pi);
Sen = 0.36; % PFD 3.6 V/rad, and 20 dB attn.
%Filt_Temp = 0;
Filt_PZT = 10^(-23/20) * myzpk('zpk',[4e3],[40],100) * myzpk('zpk',[10],[0],10) * myzpk('zpk',[2e3],[20],100)* myzpk('zpk',[2e3],[20],100) * myzpk('zpk',[400],[4],100) * myzpk('zpk',[],[600e3],1)* tf(1,1,'InputDelay',1.4e-6);
%Act_Temp = 0
Act_PZT =  1.4e6; % PROMETHEUS 1.4 MHz/V, roughly

%Gol = Sen*Filt_Temp*Act_Temp+Sen*Filt_PZT*Act_PZT;
Gol_PLL = Changing_unit*Sen*Filt_PZT*Act_PZT;

% Sensing Matrix and Actuator Matrix for CARM/DARM
Sensing = [1,0;0,0];
if use_X_as_CARM
    Sensing = [1,0;0,0];
end
Actuator = [1,0;0,0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PDH%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Dictionaries
Filt = containers.Map;  % digital filter transfer functions [cnts/cnts]
Susp = containers.Map;  % suspension actuation transfer functions [m/N]
Noise = containers.Map; % all sorts of noises
PdResp = containers.Map;    % PD rensponse [A/W]
TransImp = containers.Map;  % PD trans impedance [V/A]
Elec = containers.Map;  % electronics gain [V/V]
Coil = containers.Map;  % coil gain [N/V]

CMS_Elec_CARM = containers.Map;


%%%%%%%parameter%%%%%%%%%
c = 299792458;
L = 3000;
%Finesse = 10;
lambda = 1.064e-6;
m2Hz = 2/lambda * c/2/L;
%%%%%%%%%%%%%%%%%%%%%%%%%
%freq = logspace(-3,6,100000);
freq = logspace(-3,6,1000);

%% FIB
Changing_unit3 = myzpk('zpk',[0],[10e6],1);

tau_fib = 60*1.46/c;
s = tf('s');
sys = exp(-tau_fib*s);    
sysx = pade(sys,3);
Fiber_Delay = sysx;

Filt_FIB = 10^(-26/20) * myzpk('zpk',[4000],[40],100)* myzpk('zpk',[400],[4],100)*myzpk('zpk',[],[100e3],1) ;
Act_mirror = 2*7.7*myzpk('zpk',[],[1e3],1); % rad(Gr)/V
Sens_FIB = 4.0; % V/rad(Gr)

%%
%%%%%%%Load IMC parameters%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% copied from FSS noise budget
addpath(genpath([NbSVNroot '/FSS']));
addpath(genpath([NbSVNroot '/FSS/IOO_Parameter']));
addpath(genpath([NbSVNroot '/FSS/functions']));
IOO_params_addpath();
load('IOO_params.mat');
[TTFSS_Gain,FB_SW_TTFSS,CMS_SW_IMC,FB_SW_MCL,CMS_SW_CARM,FB_SW_CARM,CavOpgain,SW] = ...
set_IOONB_config(...
    ...TTFSS
    'CG',              26,...
    'FG',              30,...
    'Fast_Sign',       -1,...
    'TEMPGAIN',         0,...
    ...
    ...IMC
    'IN1EN_IMC',          1,...
    'IN2EN_IMC',          1,...
    'IN2POL_IMC',          -1,...
    'IN1GAIN_IMC',     -4,...
    'IN2GAIN_IMC',     -28,...
    'K1IMC_MCL_GAIN',  0.1,...
    'SLOWPOL_IMC',       -1,...
    'COMCOMP_IMC',        1,...
    'SLOWBST_IMC',        1,...
    'K1IMC_MCL_FBSW',[1 2 10],...
    ...
    ...Opgain
    'Opgain_RefCav',Cav('LPF_RefCav_1124'),...
    ...'Opgain_IMC',-Cav('LPF_IMC_1124'),
    'Opgain_IMC',Cav('LPF_IMC_1124')/7,... 
    ...
    ...LOOP SW
    'LOOP_SW',ones(1,8)...
    );
tau_DGS = 6*61e-6;
s = tf('s');
sys = exp(-tau_DGS*s);    
sysx = pade(sys,3);
Act('MCEsus') =  myzpk('zpk',[],[0.948,4.67],11365)*1.3 *myzpk('zpk',[],[500],-1)*sysx; % measured recently, [Hz/cnt (ISCINF)]

tau_FSS = 1.e-6;
s = tf('s');
sys = exp(-tau_FSS*s);    
sysx = pade(sys,3);
OLG_FSS = myzpk('zpk',[],[1;200e3],80e3)*sysx; % phenomenological model of FSS loop 

change_filter = 1;



tau_path = 1.e-6;
s = tf('s');
sys = exp(-tau_path*s);    
sysx = pade(sys,3);
Relative_Delay = sysx;

Doppler = myzpk('zpk',[0],[5.6e6/pi],1/Cav('Cpole_IMC'));

%[ze,pe,ke] = ellip(2,1,20,2*pi*700,'s');

Filt_MCL = 0.0006104*myzpk('zpk',[100;100],[10;10],0.5)... Anti-whitening for SLOW_DAQ, done in SLOW_DAQ filter bank
    *myzpk('zpk',[30;50],[800;800],10^(90/20))* myzpk('zpk',[3],[0.1],30) *(-0.8);


%%

P_eff = 10e-3; % Effective power of green that enters a reflection PD
beta = 0.05; % mod index of green


T_ITM = 0.06+0.005;
R_ITM = 1-T_ITM;
T_ETM = 0.06;
R_ETM = 1-T_ETM;
%Finesse = pi*sqrt(sqrt(R_ITM*R_ETM)) / (1-sqrt(R_ITM*R_ETM));
Finesse = 41.0; % klog 6697

Cavity_Pole = zpk([],[-2*pi*c/4/L/Finesse],2*pi*c/4/L/Finesse);
%IMC_Cavity_Pole = myzpk('zpk',[],[6.0e3],1);
%IMC_Cavity_Pole = 1;
CARM_Cavity_Pole = myzpk('zpk',[],[17],1);
Sen_1 = Finesse/(c/2/L);
%Sen_2 = 1/m2Hz;
Sen_2 = 1;

%green_PDH = P_eff * beta * T_ITM*sqrt(R_ETM)/(1-sqrt(R_ITM*R_ETM))^2 *
%4*pi*L/c; % W/Hz TO BE CALCULATED!
TransImp('green') = 0.3 * 1000; % A/W * V/A  Hamamatsu S3399 data sheet and JGW-S1808690
%DeModGain = 10.9; % Demodulation gain (see LIGO-F1100004-v4)
DeModGain = 1; % TO BE CHECKED or MEASURED!!!
%green_PDH = 0.034e-3/TransImp('green')/DeModGain; % Total sensing gain is 0.034e-3 [Hz/V] 
green_PDH_X = 1.6e-6/TransImp('green')/DeModGain;
green_PDH_Y = 3.8e-6/TransImp('green')/DeModGain;
if X
    green_PDH = green_PDH_X;
else
    green_PDH = green_PDH_Y;
end

% CARM servo settings
Filt_sum = myzpk('zpk',[48e3],[40;100e3],10);

CMS_SW_CARM('COMCOMP') = 0;
CMS_SW_CARM('COMBST') = 1; % # of common boost engaged
CMS_SW_CARM('IN2GAIN') = 8; % dB
CMS_SW_CARM('IN2EN') = 1;
CMS_SW_CARM('IN2POL') = -1;
CMS_SW_CARM('IN1GAIN') = -6; % dB
CMS_SW_CARM('IN1EN') = 0;
CMS_SW_CARM('IN1POL') = -1;
CMS_SW_CARM('COMOPT') = 0;
CMS_SW_CARM('COMGEN') = 0;
CMS_SW_CARM('FASTEN') = 1;
CMS_SW_CARM('FASTPOL') = -1;
CMS_SW_CARM('FASTOPT') = 0;
CMS_SW_CARM('FASTGAIN') = 12; % dB

CMS_SW_CARM('SLOWPOL') = -1;
CMS_SW_CARM('SLOWBST') = 0;
CMS_SW_CARM('SLOWCOMP') = 1; % Disabled if 1
CMS_SW_CARM('SLOWGEN') = 0;
CMS_SW_CARM('SLOWBY') = 0;
CMS_SW_CARM('SLOWOPT') = 0;

CMS_Elec_CARM = CMS_Elec;

CMS_Elec_CARM('common_comp') = myzpk('zpk',[4e3],[40],100);
CMS_Elec_CARM('common_bst1') = myzpk('zpk',[1000],[10],100);
CMS_Elec_CARM('common_bst2') = myzpk('zpk',[1000],[10],100);

Sen_3 = 4.5e-5*10^(32/20); % 32dB increased from X arm comm.

%CMS_Elec_CARM('FitStage_AOM') = 1;
%CMS_Elec_CARM('common_bst3') = myzpk('zpk',[1000],[10],1);


% DARM servo settings
Filt_darm = myzpk('zpk',[30],[300],3e4);
if use_X_as_CARM
    Filt_darm = myzpk('zpk',[30],[300],3e4)*myzpk('zpk',[10],[1],10);
end
%Filt_darm = myzpk('zpk',[10],[100],1)*myzpk('zpk',[.3],[3],.10);


% RF
%Filt_GR1 = myzpk('zpk',[400],[40;100e3],.1)*myzpk('zpk',[1e3],[10],100)*myzpk('zpk',[1e3],[100],10);
Filt_GR1_X = 10^(6/20) * myzpk('zpk',[400],[40],100)* myzpk('zpk',[400],[4],100)*myzpk('zpk',[],[600e3],1) ;
Filt_GR1_Y = 10^(0/20) * myzpk('zpk',[400],[40],100)* myzpk('zpk',[400],[4],100)*myzpk('zpk',[],[600e3],1) ;
if X
    Filt_GR1=Filt_GR1_X;
else
    Filt_GR1=Filt_GR1_Y;
end
tau_AOM = 6e-6;
s = tf('s');
sys = exp(-tau_AOM*s);    
sysx = pade(sys,3);
Act_AOM = 0.42e6 *2 * sysx; % klog 6544
%Act_AOM = 0.42e6 *2 ; % klog 6544
Act_AOM_X = Act_AOM;
Act_AOM_Y = Act_AOM*(1-asym/2);

% Low frequency
% G_GrM_servo = 2e+2*0;            % F_MASS servo: gain
% pGrM_servo_1 = 2*pi*   0;       % F_MASS servo: pole
% zGrM_servo_1 = 2*pi*  10;       % F_MASS servo: zero
% pGrM_servo_2 = 2*pi*   0;       % F_MASS servo: pole
% zGrM_servo_2 = 2*pi*  10;       % F_MASS servo: zero
% zGrM_servo_3 = 2*pi*  10;       % F_MASS servo: zero
% pGrM_servo_3 = 2*pi* 100;       % F_MASS servo: pole
% pGrM_servo_4 = 2*pi* 5e+2;      % F_MASS servo: pole
% pGrM_servo_5 = 2*pi* 5e+2;      % F_MASS servo: pole
% 
% Filt_mass = zpk([-zGrM_servo_1 -zGrM_servo_2 -zGrM_servo_3],[-pGrM_servo_1 -pGrM_servo_2 -pGrM_servo_3 -pGrM_servo_4 -pGrM_servo_5],G_GrM_servo*pGrM_servo_4*pGrM_servo_5);
% Filt_mass = zpk([-zGrM_servo_1 -zGrM_servo_2 -zGrM_servo_3],[-pGrM_servo_1 -pGrM_servo_2 -pGrM_servo_3 -pGrM_servo_4 -pGrM_servo_5],G_GrM_servo*pGrM_servo_4*pGrM_servo_5);
num = [(5.0e-6)*2*m2Hz*2*pi*2*pi];
den = [1 2*pi/10 4*pi*pi];
Act_mass = tf(num,den);


%Cavity
Changing_unit1 = c/L/532e-9;
num1 = [0.01*pi*pi];
den1 = [1 0.1*pi 0.01*pi*pi];

num2 = [4*pi*pi];
den2 = [1 2*pi/10 4*pi*pi];

inverted_pend = tf(num1,den1);
Pendulum =  tf(num2,den2);

dataH = load('./Noises/typeA20Kvibisoratio_with_IPdamp_L.dat'); 
dataV = load('./Noises/typeA20Kvibisoratio_V.dat'); 

ggH = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
phH = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
ggV = interp1(dataV(:,1), dataV(:,2), freq, 'linear');
phV = interp1(dataV(:,1), dataV(:,3), freq, 'linear');
for ii = 1:length(freq)
        if isnan(ggH(ii))
            if freq(ii) < 0.1
                ggH(ii) = 1;
            else
                ggH(ii) = 1e-36;
            end    
        end
        if isnan(phH(ii))
            if freq(ii) < 0.1
                phH(ii) = 0;
            else
                phH(ii) = 0;
            end    
        end       
        if isnan(ggV(ii))
            if freq(ii) < 0.1
                ggV(ii) = 1;
            else
                ggV(ii) = 1e-18./freq(ii).^9;
            end    
        end
        if isnan(phV(ii))
            if freq(ii) < 0.1
                phV(ii) = 0;
            else
                phV(ii) = -90;
            end    
        end
        
        %if freq(ii) < 0.1
        %    ggH(ii) = 1;
        %elseif freq(ii) < 0.4
        %    ggH(ii) = 0.1./freq(ii);
        %end    
            
end


for mir = {'ETMX','ETMY'}
    mir = char(mir);
    Susp([mir,'_Seism_TM']) = frd(ggH.*exp(i*phH/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM [m/m]
    Susp([mir,'_Seism_Vert']) = frd(ggV.*exp(i*phV/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM from vertical coupling [m/m]
end
VertCoup = 0.01;    % vertical coupling
CMRR_arm = 1;


Calib1 = Act_AOM /2 * CARM_Cavity_Pole;
[Calib1_mag,Calib1_phs] = mybode(Calib1,freq);


SW(1) = 1; 
SW(2) = 1;
SW(3) = 1;
SW(4) = 1;
SW(5) = 1;
SW(6) = 1;
SW(7) = 1;

%% Plot expected TFs

if plot_TFs

%%%% to create a bode plot with the freqeuncy axis in Hz %%%%
set(cstprefs.tbxprefs,'FrequencyUnits','Hz')

loopNames = {'total','SUSP','AOM','IR total','Add','MCL','IMC_AOM'};
SW = ones(1,length(loopNames));    % turn on the loops
swtot = 7;  % switches for the total loop

OLTF=[];
% OLTF for total loop
SW(swtot) = zeros(1,length(swtot));
SW(5) = 0;
SW(6) = 0;
[A,B,C,D]=linmod('GREEN_ONEARM');
systm=ss(A,B,C,D);
OLTF=[OLTF,-1*systm(swtot+4,swtot+4,:)];
% OLTF for each actuator loop

[mag,phs] = mybode(systm(11,11),freq); % IMC OLTF
IMC_OLTF = mag.*exp(i*phs*pi/180);


% IMC loop itself

set(0, 'DefaultAxesFontSize',10);
figure(98)
plotbode(freq,OLTF);
subplot(2,1,1)
ylim([1e-1,1e5]);
xlim([0.1,1e6]);
TF = csvread('./Noises/IMC_OLTF_20190721.csv',9);
measured = interp1(TF(:,1), TF(:,6), freq, 'linear');
measuredp = interp1(TF(:,1), TF(:,7), freq, 'linear');
loglog(freq,10.^((measured)/20));
legend('IMC','measured');
set(gca,'YTick',logspace(-1,5,7));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
semilogx(freq,measuredp);
xlim([0.1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
saveas(gcf,['./results_' name '/IMC_OLTF.png'])

SW = ones(1,length(loopNames));











figure(99)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
TF_Arm=tfs(1,17);
plotbode(freq,TF_Arm/m2Hz);
subplot(2,1,1)
[Due_to_IMC,Due_to_IMCp] = mybode(CARM_Cavity_Pole*(1-myzpk('zpk',[],[Cav('Cpole_IMC')],1)),freq);
loglog(freq,Due_to_IMC);
ylim([1e-6,1e-2]);
xlim([1e-3,1e2]);
set(gca,'YTick',logspace(-6,-2,5));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
semilogx(freq,Due_to_IMCp);
xlim([1e-3,1e2]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
legend('Contribution from Arm Length','Due to IMC Cavity Pole Effect')
saveas(gcf,['./results_' name '/Contribution_from_Arm_Length_Fluctuation.png'])
[mag, phs] = mybode(TF_Arm/m2Hz, freq);
data(:,1) = freq;
data(:,2) = mag;
data(:,3) = phs;
filename = ['results_' name '/Contribution_from_Arm_Length_Fluctuation.dat'];
fileID = fopen(filename,'w');
fprintf(fileID,'%6.10f %.16e %12.30f \r\n',data');
fclose(fileID);
clear data

figure(100)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(1,2);
bode(aaaa)
grid on
legend('TM disp. contribution')

SW(1) = 0; 
SW(7) = 0;

figure(101)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(2,3);
%plotbode(freq,(1/aaaa)-1)
plotbode(freq,Gol_PLL)
subplot(2,1,1)
TF = load('./Noises/PLL_OLTF_181018.txt'); 
measured = interp1(TF(:,1), TF(:,2), freq, 'linear');
measuredp = interp1(TF(:,1), TF(:,3), freq, 'linear');
loglog(freq,10.^(measured/20));
grid on
legend('PLL gain','measured')
xlim([1e1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
semilogx(freq,measuredp);
xlim([1e1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));


figure(102)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(3,4);
bode(aaaa)
grid on
legend('PLL follow')

SW(1) = 1; 
SW(7) = 1;

figure(103)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(1,4);
bode(aaaa)
grid on
legend('contribution from PSL')

%figure(104)


%figure(105)


% DOF Switches
loopNames = {'total','SUSP','AOM','IR total','Add','MCL','IMC_AOM'};
SW = ones(1,length(loopNames));    % turn on the loops
swtot = 1;  % switches for the total loop
swact = 3:4;  % switches for each actuator loop

OLTF=[];
% OLTF for total loop
SW(swtot) = zeros(1,length(swtot));
SW(swact) = ones(1,length(swact));
[A,B,C,D]=linmod('GREEN_ONEARM');
systm=ss(A,B,C,D);
OLTF=[OLTF,-1*systm(swtot+4,swtot+4,:)];
% OLTF for each actuator loop

%SW = zeros(1,length(loopNames));
SW(swtot) = ones(1,length(swtot));
SW(swact) = zeros(1,length(swact));
[A,B,C,D]=linmod('GREEN_ONEARM');
for kk=swact
systm=ss(A,B,C,D);
    OLTF=[OLTF,-1*systm(kk+4,kk+4,:)];
end

% Common green lock loop

set(0, 'DefaultAxesFontSize',10);
figure(110)
plotbode(freq,OLTF);
subplot(2,1,1)
ylim([1e-1,1e5]);
xlim([0.1,1e6]);
if X
    TF = load('./Noises/PDHX_OLTF_190817.txt');
else
    TF = load('./Noises/PDHY_OLTF_190817.txt');
end
measured = interp1(TF(:,1), TF(:,6), freq, 'linear');
measuredp = interp1(TF(:,1), TF(:,7), freq, 'linear');
if X
    loglog(freq,10.^((measured+12)/20));
else
    loglog(freq,10.^(measured/20));
end
legend(loopNames{swtot},loopNames{swact},'measured');
set(gca,'YTick',logspace(-1,5,7));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
semilogx(freq,measuredp);
xlim([0.1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
legend(loopNames{swtot},loopNames{swact});
saveas(gcf,['./results_' name '/green_OLTF.png'])

SW = ones(1,length(loopNames));
SW(swact) = ones(1,length(swact));


%%%%% IR path, additive offset and MCL path
swtot = 4;  
swact = 5:6; 
%Filt_GR1 = myzpk('zpk',[],[4;100e3],1e-6);

OLTF=[];
% OLTF for total loop
SW(swtot) = zeros(1,length(swtot));
SW(swact) = ones(1,length(swact));
[A,B,C,D]=linmod('GREEN_ONEARM');
systm=ss(A,B,C,D);
OLTF=[OLTF,-1*systm(swtot+4,swtot+4,:)];
% OLTF for each actuator loop

%SW = zeros(1,length(loopNames));
SW(swtot) = ones(1,length(swtot));
SW(swact) = zeros(1,length(swact));
[A,B,C,D]=linmod('GREEN_ONEARM');
for kk=swact
systm=ss(A,B,C,D);
    OLTF=[OLTF,-1*systm(kk+4,kk+4,:)];
end

set(0, 'DefaultAxesFontSize',10);
figure(111)
plotbode(freq,OLTF);
subplot(2,1,1)
ylim([1e-2,1e8]);
%xlim([freq(1),freq(end)]);
xlim([0.1,1e5]);
TF = csvread('./Noises/ALS_CARM_OLTF_woboosts_20190721.csv',9); 
TF_comp = 10.^( (TF(:,6) -4-6  )/20).*exp(i*2*pi*TF(:,7)/360); 
% 4dB: IN2 gain at the measuremnts = 12dB while "nominal" IN2 gain = 8dB. 
% 6dB:measurement was done with both arms
TF_1boost = TF_comp.* 100.*(1+j.*TF(:,1)/1000)./(1+j.*TF(:,1)/10);
TF(:,2) = abs(TF_1boost);
TF(:,3) = angle(TF_1boost)*180/pi;
measured = interp1(TF(:,1), TF(:,2), freq, 'linear');
measuredp = interp1(TF(:,1), TF(:,3), freq, 'linear');
loglog(freq,measured);
legend(loopNames{swtot},loopNames{swact},'measured');
set(gca,'YTick',logspace(-2,10,13));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
%xlim([freq(1),freq(end)]);
xlim([0.1,1e5]);
semilogx(freq,measuredp);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
legend(loopNames{swtot},loopNames{swact},'measured');
saveas(gcf,['./results_' name '/ALS_CARM_TF.png'])

SW = ones(1,length(loopNames));
SW(swact) = ones(1,length(swact));


% Crossover freq measurement
SW(6) = 0;
figure(112)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(10,10);
plotbode(freq,aaaa)
subplot(2,1,1)
TF = load('./Noises/crossover_190103.txt'); 
measured = interp1(TF(:,1), TF(:,2), freq, 'linear');
measuredp = interp1(TF(:,1), TF(:,3), freq, 'linear');
loglog(freq,10.^(measured/20));
grid on
legend('Crossover','measured')
xlim([1e1,1e3]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
semilogx(freq,measuredp);
xlim([1e1,1e3]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));

SW(6) = 1;


% % Differential green lock loop
% figure(113)
% SW(1) = 0;
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% D_green_tot = tfs(14,15);
% SW(1) = 1;
% 
% SW(2) = 0;
% SW(3) = 0;
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% D_green_AOM = tfs(15,16);
% D_green_susp = tfs(6,6);
% SW(2) = 1;
% SW(3) = 1;
% opts = bodeoptions('cstprefs');
% opts.YLimMode = 'Manual';
% ylims{1} = [-20 60];
% ylims{2} = [-180 180];
% opts.YLim = ylims;
% hold on
% bodeplot(-1*D_green_tot,opts)
% bodeplot(-1*D_green_AOM,opts)
% bodeplot(-1*D_green_susp,opts)
% grid on
% legend('DARM_total','AOM','susp')
% xlim([0.1 1e6]);
% 
% % DARM loop TFs
% figure(114)
% %Filt_darm = myzpk('zpk',[10],[100],1)*myzpk('zpk',[.3],[3],.10);
% %Filt_GR1 = myzpk('zpk',[],[4;100e3],.1);
% SW(2) = 0;
% %SW(3) = 0;
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% %D_AOM=tfs(15,16);
% D_susp=tfs(6,6);
% opts = bodeoptions('cstprefs');
% %ylims = getoptions(bodeplot(-1*D_AOM),'YLim');
% opts.YLimMode = 'Manual';
% ylims{1} = [-20 100];
% ylims{2} = [-180 180];
% opts.YLim = ylims;
% hold on
% %bodeplot(-1*D_AOM,opts)
% bodeplot(-1*D_susp,opts)
% %bodeplot(1/(1-1*D_susp),opts)
% %bodeplot(-1*(D_AOM+D_susp),opts)
% grid on
% legend('DARM')
% xlim([0.1 1e5]);
% saveas(gcf,'./results_' name '/ALS_DARM_TF.png')
% %Filt_darm = myzpk('zpk',[10],[100],1);
% %Filt_GR1 = myzpk('zpk',[],[4;100e3],0);
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% D_susp_boost=tfs(6,6);
% SW(2) = 1;
% %SW(3) = 1;

% % Hand-over from AOM to DARM
% figure(115)
% opts = bodeoptions('cstprefs');
% %ylims = getoptions(bodeplot(-1*D_AOM),'YLim');
% opts.YLimMode = 'Manual';
% ylims{1} = [-20 60];
% ylims{2} = [-180 180];
% opts.YLim = ylims;
% hold on
% for ii = [-3:1:0]
%     bodeplot(-1*(D_AOM + D_susp*10^ii),opts)
% end
% for ii = -1*[0.5:0.5:1.5]
%     bodeplot(-1*(D_AOM*10^ii + D_susp),opts)
% end
% bodeplot(-1*D_susp_boost,opts)
% legend('1','2','3','4','5','6','7','8')
% grid on
% xlim([0.1 1e4]);
% saveas(gcf,'./results_' name '/DARM_AOM2Susp_handover.png')

SW(4)=0;
CMS_SW_CARM('IN1EN') = 1;
figure(116)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(16,18);
plotbode(freq,-1*aaaa)
subplot(2,1,1)
legend('CARM')
xlim([1e1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
subplot(2,1,2)
xlim([1e1,1e6]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));

SW(4) = 1;
CMS_SW_CARM('IN1EN') = 0;


% SW(1) = 0; 
% figure(106)
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% aaaa=tfs(5,5);
% bode(aaaa)
% grid on
% legend('Green OLG')
% SW(1) = 1; 

SW(4) = 0; 
SW(7) = 0; 
figure(107)
[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(7,4);
bode(aaaa*Act_AOM/2)
grid on
legend('Green as a IR freq sensor')
SW(4) = 1; 
SW(7) = 1;

%figure(108)
%[A,B,C,D] = linmod('GREEN_ONEARM');
%tfs = ss(A,B,C,D);
%aaaa=tfs(8,4);
%bode(Filt_GR*Filt_IMC*Filt_MCL*Act('MCEsus')*Filt_GR)
%grid on
%legend('MCL path')

% SW(5) = 0; 
% SW(6) = 0;
% %CMS_SW_IMC('IN2EN') = 1;
% figure(108)
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% aaaa=tfs(3,9);
% bode(aaaa)
% hold on
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% aaaa1=tfs(3,10);
% bode(aaaa1)
% hold on
% bode(aaaa+aaaa1)
% grid on
% legend('Additive offset','MCL path','sum')
% SW(5) = 1; 
% SW(6) = 1; 
%CMS_SW_IMC('IN2EN') = 0;

% SW(5) = 0;
% SW(6) = 0; 
% SW(7) = 0;
% figure(99)
% [A,B,C,D] = linmod('GREEN_ONEARM');
% tfs = ss(A,B,C,D);
% aaaa=tfs(11,11);
% bode(aaaa)
% grid on
% legend('IMC AOM path')
% SW(5) = 1;
% SW(6) = 1; 
% SW(7) = 1;
end
SW = ones(1,length(loopNames));


%% Define some parameters and get live parts parameters
%freq = logspace(-3,6,100000);
liveModel = 'GREEN_ONEARM';
dof = 'CARM';    % name of DOF to plot NB
 startTime = 1078250000;   % start GPS time
 durationTime = 512;
 IFO = 'K1';
 site = 'kamioka';

%duration = 64;  % data get time duration
freqsamp = 16384;   % sampling frequency
freqmin = 0.1;  % minimum frequency (= frequency reslution)
Ndata = ceil(freqsamp/freqmin);
ndata = 2^nextpow2(Ndata);
duration2 = 64;  % data get time duration
freqsamp2 = 2048;   % sampling frequency
freqmin2 = 0.1;  % minimum frequency (= frequency reslution)
Ndata2 = ceil(freqsamp2/freqmin2);
ndata2 = 2^nextpow2(Ndata2);
 
% Try setting different NDS server if you couldn't get data
% setenv('LIGONDSIP','h1nds1:8088');
% mdv_config;

% load cached outputs
loadFunctionCache()

% get live parts parameters
%liveParts(liveModel, startTime, durationTime, freq)

noise = load('./Noises/ADCnoise.txt'); 
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0;
        end
    end
Noise('ADC') = temp_noise; 

scale = 2.; % [m]; length scale of the turbulence 
Noise('PSL turbulence') = 7.*sqrt(scale/2000).*freq.^(-1/6) *turb;

Noise('SHG') = 1e-5.*freq;  % SHG frequency noise in [Hz/rtHz]
%Noise('Fiber') = 1.;  % Green fiber frequency noise in [Hz/rtHz]
%Noise('Fiber') = sqrt((2e-3)^2 + (3e-2./freq.^2).^2) * sqrt(50/5);  % Green fiber frequency noise in [Hz/rtHz] klog 5417
%noise = load('./Noises/fiberphasenoise_190120.txt'); % klog 7728
%temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
%    for ii = 1:length(freq)
%        if isnan(temp_noise(ii))
%            temp_noise(ii) = 0.1;
%        end
%    end
if X
    noise = load('./Noises/ALS_sensingnoise_X_20190828_2.txt'); 
else
    noise = load('./Noises/ALS_sensingnoise_Y_20190825.txt'); 
end
temp_noise = interp1(noise(:,1), noise(:,4), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0;
        end
    end
[absolute, phs] = mybode(Act_mirror, freq);    
Noise('Fiber') = temp_noise .* absolute .* freq ;

Noise('Shot noise green') = 6.1e-11; % Shot noise in [W/rtHz]

noise = load('./Noises/KamiokaSeismicHighNoise.txt'); % file from Suspension modeling svn
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 1.5e-9.*freq(ii).^-2;
        end
    end
Noise('Seismic test mass') = temp_noise;% Seismic noise of test masses in [m/rtHz]

noise = load('./Noises/KamiokaSeismicNoise_ArmLength.txt'); % Arm length displacement data given by Miyo-kun
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 1.5e-9.*freq(ii).^-2*sqrt(2);
        end
    end
Noise('Seismic arm length') = temp_noise; % Arm length displacement of test masses in [m/rtHz]



noise = load('./Noises/VCO_frequencynoise.txt'); % klog 6552
noise = load('./Noises/VCO_frequencynoise_1218.txt'); % klog 7425
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.06;
        end
    end
Noise('VCO noise') = temp_noise; % klog 6552
Noise('Shot noise IR') = 8e-7; % Shot noise in [Hz/rtHz]

Noise('PSL freq noise') = 0*freq./freq/10+8e-9*freq.^-2./(1+(2./freq).^-8)/26.65*299792458./1064e-9; % PSL freq noise in [Hz/rtHz]

noise = load('./Noises/PLLX_lasernoise.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('Prometheus freq noise') = temp_noise; % PROMETHEUS freq noise in [Hz/rtHz]

Noise('LO noise') = 2.0e-4; % LO noise of PLL loop in [Hz/rtHz]  E8663D
Noise('Shot noise PLL') = 6.39e-7; % Shot noise in [rad/rtHz] JGW-T1809263-v1

noise = load('./Noises/PLLX_servonoise.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('Electronic noise PLL') = temp_noise ; % Electronic noise of PLL loop in [V/rtHz]
    
noise = load('./Noises/PLLX_PFDnoise.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('PFD noise PLL') = temp_noise; % Electronic noise of PFD in [V/rtHz]

noise = load('./Noises/PDHX_servonoise_190104.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('Electronic noise PDH') = temp_noise; % Electronic noise of PDH loop in [V/rtHz]

noise = load('./Noises/CARM_servonoise.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('Electronic noise CARM') = temp_noise; % Electronic noise of CARM loop in [V/rtHz]

noise = load('./Noises/ALS_sensingnoise_SUMMING_20190908.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('Electronic noise SUM') = temp_noise ./abs( 10.*(1+i.*freq/48e3)./(1+i.*freq/40)./(1+i.*freq/100e3) ) ; % Electronic noise of Summing node in [V/rtHz]
% cf.) Filt_sum = myzpk('zpk',[48e3],[40;100e3],10);

%noise = load('./Noises/PDHX_PDnoise.txt');
%noise = load('./Noises/PDnoise_190104.txt');
noise = load('./Noises/PDnoises_30+20dBamp_20190830.txt');
if X
    temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
else
    temp_noise = interp1(noise(:,1), noise(:,3), freq, 'linear');
end
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('PD noise PDH') = temp_noise*10^(-50/20)*40/2^16; % Dark noise of green RF PD in [V/rtHz]


% noise = load('./Noises/KamiokaSeismicHighNoise.txt'); % file from Suspension modeling svn
% temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
%     for ii = 1:length(freq)
%         if isnan(temp_noise(ii))
%             temp_noise(ii) = 1.5e-9.*freq(ii).^-2;
%         end
%     end
% Noise('seismic_IMC') = temp_noise;
noise = load('./Noises/IMC_seismic_freq_190103.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear')*1e7;
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 1e3 * freq(ii).^-4;
        end
    end
Noise('seismic_IMC_inHz') = temp_noise;


noise = load('./Noises/IOO_frequencynoise.txt');
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0.0;
        end
    end
Noise('IOO_freq') = temp_noise.*abs(1+IMC_OLTF)*0;




Noise('act_AOM') = 3e-8; % VCO  noise of FSS loop in [V/rtHz]

load('Sensor_Noise_RefCav.mat')
    temp_noise =...
        interp1(Sensor_Noise_RefCav('ff_KOACH_off'),Sensor_Noise_RefCav('KOACH_off'),freq);
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 8.071e-5*(15./(freq(ii)/0.1+1).^0.6./(freq(ii)/2000+1)./(freq(ii)/3000+1).*(freq(ii)/700+1));
        end
    end
Noise('sensor_RefCav') = temp_noise; % RefCav unknown sensing noise in [V/rtHz]

SW(1) = 1; 


[A,B,C,D] = linmod('GREEN_ONEARM');
tfs = ss(A,B,C,D);
aaaa=tfs(1,8);
green_sensing = aaaa;
[SR_mag,phs] = mybode(Filt_sum,freq);
[TF_mag, phs] = mybode(green_sensing, freq);
green_sen_mag = TF_mag.*SR_mag ;

%noise = load('./Noises/ALS_outofloop_20190102.txt'); 
if X
    noise = load('./Noises/ALS_sensingnoise_X_20190828_2.txt'); 
else
    noise = load('./Noises/ALS_sensingnoise_Y_20190825.txt'); 
end
temp_noise = interp1(noise(:,1), noise(:,2), freq, 'linear');
    for ii = 1:length(freq)
        if isnan(temp_noise(ii))
            temp_noise(ii) = 0;
        end
    end
Noise('measured_sensing') = temp_noise./(10 .* abs( (1+j*freq/48e3)./(1+j*freq/40) ));



%% Compute noises and save cache
% Compute noises
[noises, sys] = nbFromSimulink(liveModel, freq, 'dof', dof);

% save cached outputs
saveFunctionCache();

%% Make a quick NB plot
if plot_NBs
disp('Plotting noises')
nb = nbGroupNoises(liveModel, noises, sys);

% Get noise data from DAQ. Put NdNoiseSource block with DAQ channel
% specified. Put something (e.g. 1) in ASD parameter of that block.
%nb = nbAcquireData(liveModel, sys, nb, startTime, durationTime);

%nb.sortModel();
%matlabNoisePlot(nb);
%figure(1)
%grid on;
%axis tight;
%ylabel('Mag (m/sqrt(Hz))');
%xlabel('Frequency (Hz)');
%ylim([1e-22,1e-10]);
%xlim([1,1e4]);

%%figure(2);
%matlabNoisePlot(nb);
%%legend('Location','southwest')
%%hold on
%plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1]);
%grid on;
%axis tight;
%ylabel('Mag (Hz/sqrt(Hz))','FontSize',20);
%xlabel('Frequency (Hz)','FontSize',20);
%title('Frequency noises');
%xlim([1e-3,1e6]);
%%ylim([1e-22,1e-10]);
%ylim([1e-10,1e2]);
%leg = legend(gca);
%legend({leg.String{:},'RMS'},'Interpreter','None','Location','southwest');
%saveas(gcf,'./results_' name '/nb.fig')

nb.sortModel();
matlabNoisePlot(nb);

figure(2)
xlim([1e-3,1e6]);
ylim([1e-10,1e2]);
%plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1]);
leg = legend(gca,'Location','southwest');
%legend({leg.String{:},'RMS'},'Interpreter','None','Location','southwest');
saveas(gcf,['./results_' name '/Green_noiseNB.fig'])
saveas(gcf,['./results_' name '/Green_noiseNB.png'])
clear leg

figure(3)
xlim([1e-3,1e6]);
ylim([1e-10,1e2]);
%plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1]);
leg = legend(gca,'Location','southwest');
%legend({leg.String{:},'RMS'},'Interpreter','None','Location','southwest');
saveas(gcf,['./results_' name '/IR_noiseNB.fig'])
saveas(gcf,['./results_' name '/IR_noiseNB.png'])
clear leg

figure(4)
xlim([1e-3,1e6]);
ylim([1e-10,1e2]);
%plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1]);
leg = legend(gca);
%legend({leg.String{:},'RMS'},'Interpreter','None','Location','southwest');
saveas(gcf,['./results_' name '/Seismic_noiseNB.fig'])

figure(5)
xlim([1e-3,1e6]);
ylim([1e-10,1e2]);
%plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1]);
leg = legend(gca);
%legend({leg.String{:},'RMS'},'Interpreter','None','Location','southwest');
saveas(gcf,['./results_' name '/PLL_noiseNB.fig'])

figure(1)
xlim([1e-1,1e5]);
ylim([1e-5,1e2]);
%splotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1],0.1);
loglog(freq,Noise('measured_sensing').*green_sen_mag);
leg = legend(gca);
legend({leg.String{:},'Measured'},'Interpreter','None','Location','northeast');
set(gca,'YTick',logspace(-10,2,13));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
saveas(gcf,['./results_' name '/ALS_NB.fig'])
saveas(gcf,['./results_' name '/ALS_NB.png'])

end

%% Output NB data
if plot_NBs
nbdata=[freq;Noise('measured_sensing').*green_sen_mag;nb.sumNoise.asd];
header=['% freq, measured ,sum']; 
for kk=1:length(nb.modelNoises)
    nbdata=[nbdata;nb.modelNoises{kk}.asd];
    hd=strsplit(nb.modelNoises{kk}.name,'}{');
    try
        hdd=char(hd(2));
        hdd=hdd(1:end-1);
    catch
        hdd=hd;
    end
    header=[header,', ',hdd];
end
for kk=1:length(noises)
    nbdata=[nbdata;noises{kk}.asd];
    hd=strsplit(noises{kk}.name,'\n');
    hd=strsplit(char(hd(1)),'/');
    try
        hdd=char(hd(2));
    catch
        hdd=hd;
    end
    header=[header,', ',hdd];
end

dlmwrite(['./results_' name '/ALS_NB.txt'],header,'delimiter','');
dlmwrite(['./results_' name '/ALS_NB.txt'],nbdata','-append','delimiter',' ','precision','%.6e');

end

% %% Actuators saturation check
% act = 'FSS_AOM'; 
% if plot_Saturations
% disp('Checking saturation')
% % Compute noises
% [noises_AOM, sys_AOM] = nbFromSimulink(liveModel, freq, 'dof', act);
% 
% % save cached outputs
% saveFunctionCache();
% 
% % plot
% nb = nbGroupNoises(liveModel, noises_AOM, sys_AOM);
% nb.sortModel();
% matlabNoisePlot(nb);
% 
% figure(6)
% xlim([1e-3,1e6]);
% ylim([1e-10,1e-1]);
% plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1],0.1);
% 
% end


%% Openloop measurement of ALS with CARM locked to the main laser
sys_open = 'OPEN';
SW(4) = 0;
realCARM =1;
CMS_SW_CARM('IN1EN') = 1;
CMS_SW_CARM('IN2EN') = 0;

if plot_CARMlock;
[noises_CARM, sys_CARM] = nbFromSimulink(liveModel, freq, 'dof', sys_open);

% save cached outputs
saveFunctionCache();

% plot
nb = nbGroupNoises(liveModel, noises_CARM, sys_CARM);
nb.sortModel();
matlabNoisePlot(nb);

figure(6)
xlim([1e-1,1e5]);
ylim([1e-5,1e2]);
plotcumulativeRMS2(nb.sumNoise.f,nb.sumNoise.asd,[1,0,1],0.1);
loglog(freq,Noise('measured_sensing').*Calib1_mag);
leg = legend(gca);
legend({leg.String{:},'RMS','Measured'},'Interpreter','None','Location','northeast');
saveas(gcf,['./results_' name '/ALS_openloopNB.fig'])
saveas(gcf,['./results_' name '/ALS_openloopNB.png'])


end
SW(4) = 1;
realCARM =0;
CMS_SW_CARM('IN1EN') = 0;
CMS_SW_CARM('IN2EN') = 1;


%% Output NB data
if plot_CARMlock
nbdata=[freq;Noise('measured_sensing').*Calib1_mag;nb.sumNoise.asd];
header=['% freq, measured, sum']; 
for kk=1:length(nb.modelNoises)
    nbdata=[nbdata;nb.modelNoises{kk}.asd];
    hd=strsplit(nb.modelNoises{kk}.name,'}{');
    try
        hdd=char(hd(2));
        hdd=hdd(1:end-1);
    catch
        hdd=hd;
    end
    header=[header,', ',hdd];
end
for kk=1:length(noises_CARM)
    nbdata=[nbdata;noises_CARM{kk}.asd];
    hd=strsplit(noises_CARM{kk}.name,'\n');
    hd=strsplit(char(hd(1)),'/');
    try
        hdd=char(hd(2));
    catch
        hdd=hd;
    end
    header=[header,', ',hdd];
end

dlmwrite(['./results_' name '/ALS_openloopNB.txt'],header,'delimiter','');
dlmwrite(['./results_' name '/ALS_openloopNB.txt'],nbdata','-append','delimiter',' ','precision','%.6e');

end

%% plot expected curve from calculation

%[mag,ph]=bode((Gol)/(1+Gol),2*pi*freq);
%loglog(freq,squeeze(mag),'b.')
%hold on

%[mag,ph]=bode(1/Sen,2*pi*freq);
%loglog(freq,freq./freq/Sen,'r.')