%% CLF transfer function 6dB RF6

%% Call all spec
run 'Marc2024ALLSQZSPEC.m'
%run OPOTF_highCLF_March112024.m
%run LOTF_highCLF_March112024.m

%%
c = 299792458;
L = 2*0.175; %OPO cavity length 
FSR = c/(L); %Hz c/round trip

%% load data

CLFTF_amp = load('SCRN0137.TXT'); 
CLFTF_phase = load('SCRN0138.TXT');
CLFTF_ampx = CLFTF_amp(:,1); %freq
CLFTF_ampy = CLFTF_amp(:,2); %amp
CLFTF_phy = CLFTF_phase(:,2); %phase

CLFTFamp = 10.^(CLFTF_ampy/20); %dB to power ratio
CLFcmplx = CLFTFamp.*exp(i.*deg2rad(CLFTF_phy)); 
CLFOLmag = abs(CLFcmplx);
CLFOLphase = angle(CLFcmplx);

%% OPO param

c = 299792458;
L = 2*0.175; %OPO cavity length 
FSR = c/(L); %Hz c/round trip
R1 = 0.93; %1064
R2 = 0.999; %1064
Fin_red = pi*(R1*R2*.99993587*.99993625)^(1/5)/( 1-sqrt(R1*R2*.99993587*.99993625) ); %https://dcc.ligo.org/DocDB/0166/E2000155/003/E2000155-v3.pdf

%Fin_red = pi*(.87298077*.99818636*.99993587*.99993625)^(1/5)/( 1-sqrt(.87298077*.99818636*.99993587*.99993625) ); %
%Fin = pi*(.98*.999*.999*.994)^(1/5)/( 1-sqrt(.98*.999*.999*.994) ); %green Fin
%Fin = 44
OPOFWHMred = FSR / Fin_red;

%% Generic common mode board components

%VCXOV2Hz = 925; %Hz/V calibration for CLF AOM
%Hz/V T2400083 https://dcc.ligo.org/DocDB/0192/T2400083/001/DCSR200-5.pdf
%VCOV2Hz = 1e6*2; %x2 because 6 MHz. The number is for 203.
VCOV2Hz = 425e3*2; %425kHz/V https://alog.ligo-wa.caltech.edu/aLOG/index.php?callRep=77049. x2 because 6 MHz. The number is for 203 (3MHz).

CLF_CMgain = 0.158; %-16dB
CLF_fastgain = 1; %0dB
%CLF_cal = 0.05; %V/rad
CLF_cal = 9.72/2; %V/2rad
%1.5 is a fudge factor

CM_comp = zpk(-2.*pi.*[4e3  ], -2*pi.*[40 ], 1);
CM_boost1 = zpk(-2.*pi.*[10  ], -2*pi.*[0 ], 1);
CM_boost2 = zpk(-2.*pi.*[2e3  ], -2*pi.*[20 ], 1);
CM_boost3 = zpk(-2.*pi.*[2e3  ], -2*pi.*[20 ], 1);

SLOW_boost = zpk(-2.*pi.*[400  ], -2*pi.*[4 ], 1);
SLOW_filters = zpk(-2.*pi.*[  ], -2*pi.*[100e3 ], 100e3*2*pi);
PZTdriver = zpk(-2.*pi.*[  10 ],...
            -2*pi.*[ 1 400], (1/10)*400*20); %PZTdriver gain = 20


%% CLF stuff

%VCXOV2Hz = 925; %Hz/V calibraitno for CLF AOM done at 3MHz

flat = zpk([],[], VCOV2Hz); %Hz/V 
oneonf = zpk(-2*pi*[],-2*pi*[0], 1*2*pi ); %1/Hz phase detection frequency actuation

%VCO = zpk(-2*pi*[ 40],-2*pi*[1.6], 281479.*(1/40).*1.6); %Hz/V 80MHz VCO calibration at DC
%mysteriouspole = zpk([],-2*pi*[8.5e3 14e3], (2*pi)^2*5e3*20e3 ); %prob VCXO
VCOzpk = zpk(-2.*pi.*[40],-2*pi*[1.6], 1.6/40); 
mysteriouspole = zpk([],-2*pi*[35e3 35e3], (2*pi)^2*35e3*35e3*1/3 ); %This is actually a 105kHz notch
%pztnotch = zpk([-103e3+1j*41651.73, -103e3-1j*41651.73 -2*pi*265],...
            %[-20828.759+1j*36076.469, -20828.759-1j*36076.469 -2*pi*66], 1);

OPOcavLP_red = zpk(-2*pi*[],-2.*pi.*[OPOFWHMred./2], 2.*pi.*OPOFWHM/2 );


%CLFcommon_path = CLF_CMgain.*CM_boost1.*CM_boost2;
CLFcommon_path = CLF_CMgain.*CM_boost1.*CM_boost2.*CM_boost3;

CLFfast_path = flat*oneonf*VCOzpk; 

%CLF_TF = CLF_cal.*CLFcommon_path.*CLFfast_path.*mysteriouspole;
CLF_TF = CLF_cal.*CLFcommon_path.*CLFfast_path*1/3; %1/3 is a fudge factor
%CLF_TF = oneonf;
%CLF_TF = VCOzpk;
CLFCMboard = CLFcommon_path;
CLFAct = CLFfast_path;

[CLFamp, CLFphase] = bode(CLF_TF, 2*pi*CLFQMON_x); 
CLFamp = squeeze(CLFamp);
CLFphase = squeeze(CLFphase);
CLFfilter = CLFamp.*exp(i.*deg2rad(CLFphase));
CLFOLmagzpk = abs(CLFfilter);
CLFOLphasezpk = angle(CLFfilter);


[CLFoneplusamp, CLFoneplusphase] = bode(1+CLF_TF, 2*pi*CLFQMON_x); 
CLFoneplusamp = squeeze(CLFoneplusamp);
CLFoneplusphase = squeeze(CLFoneplusphase);
CLFoneplusfilter = CLFoneplusamp.*exp(i.*deg2rad(CLFoneplusphase));
CLFonelusOLmagzpk = abs(CLFoneplusfilter);
CLFoneplusOLphasezpk = angle(CLFoneplusfilter);

[CLFoneononeplusamp, CLFoneononeplusphase] = bode(1/(1+CLF_TF), 2*pi*CLFQMON_x); 
CLFoneononeplusamp = squeeze(CLFoneononeplusamp);
CLFoneononeplusphase = squeeze(CLFoneononeplusphase);
CLFoneononeplusfilter = CLFoneononeplusamp.*exp(i.*deg2rad(CLFoneononeplusphase));
CLFoneononelusmagzpk = abs(CLFoneononeplusfilter);
CLFoneononeplusphasezpk = angle(CLFoneononeplusfilter);

[CLFGononeplusamp, CLFGononeplusphase] = bode(CLF_TF/(1+CLF_TF), 2*pi*CLFQMON_x); 
CLFGononeplusamp = squeeze(CLFGononeplusamp);
CLFGononeplusphase = squeeze(CLFGononeplusphase);
CLFGononeplusfilter = CLFGononeplusamp.*exp(i.*deg2rad(CLFGononeplusphase));
CLFGononelusmagzpk = abs(CLFGononeplusfilter);
CLFGononeplusphasezpk = angle(CLFGononeplusfilter);

[CLFCMamp, CLFCMphase] = bode(CLFCMboard, 2*pi*CLFQMON_x); 
CLFCMamp = squeeze(CLFCMamp);
CLFCMphase = squeeze(CLFCMphase);
CLFCMfilter = CLFCMamp.*exp(i.*deg2rad(CLFCMphase));
CLFCMOLmagzpk = abs(CLFCMfilter);
CLFCMOLphasezpk = angle(CLFCMfilter);

[CLFAamp, CLFAphase] = bode(CLFAct, 2*pi*CLFQMON_x); 
CLFAamp = squeeze(CLFAamp);
CLFAphase = squeeze(CLFAphase);
CLFAfilter = CLFAamp.*exp(i.*deg2rad(CLFAphase));
CLFAOLmagzpk = abs(CLFAfilter);
CLFAOLphasezpk = angle(CLFAfilter);

[OPOcavLPredamp, OPOcavLPredphase] = bode(OPOcavLP_red, 2.*pi.*CLFQMON_x);
OPOcavLPredamp = squeeze(OPOcavLPredamp);
OPOcavLPredphase = squeeze(OPOcavLPredphase);
OPOcavLPredfiltercomplex = OPOcavLPredamp.*exp(i.*deg2rad(OPOcavLPredphase));
OPOcavLPredOLmagzpk = abs(OPOcavLPredfiltercomplex);
OPOcavLPredOLphasezpk = angle(OPOcavLPredfiltercomplex);

%% CLF read out pole

CLFsenspole = zpk(-2*pi*[],-2.*pi.*[100e3], 2.*pi.*100e3);

[CLFsenspoleamp, CLFsenspolephase] = bode(CLFsenspole, 2.*pi.*CLFQMON_x);
%[OMCcavLPamp, OMCcavLPphase] = bode(OMCcavLP, 2.*pi.*OMCTF_ampx_lf);
CLFsenspoleamp = squeeze(CLFsenspoleamp);
CLFsenspolephase = squeeze(CLFsenspolephase);
CLFsenspolecomplex = CLFsenspoleamp.*exp(i.*deg2rad(CLFsenspolephase));
CLFsenspolemagzpk = abs(CLFsenspolecomplex);
CLFsenspolephasezpk = angle(CLFsenspolecomplex);

CLFsenspole = CLFsenspolemagzpk;
%%

OPOLPred = OPOcavLPredOLmagzpk;
CLFCM = CLFCMOLmagzpk;
CLFA = CLFAOLmagzpk;
CLFG = CLFOLmagzpk;
oneplusCLFG = CLFonelusOLmagzpk;
oneononeplusCLFG = CLFoneononelusmagzpk;
CLF_CLG = oneononeplusCLFG;
CLFGononeplusCLFG = CLFGononelusmagzpk;
CLF_RLG = CLFGononeplusCLFG;
CLFS = CLF_cal;



%% Plot CLF TF
figure(233);
subplot(2,1,1);
semilogx(CLFTF_ampx, 20*log10(CLFOLmag),...
    CLFQMON_x, 20*log10(CLFOLmagzpk), '--','LineWidth', 1.5, 'Color', [0.8500 0.3250 0.0980]);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
%ylim([-80,60])
xlim([1e2,1e5])
legend('Measured', 'Modeled', 'CLF 1+G', 'CLF G/(1+G)', 'CLF 1/(1+G)', 'interp')
title('CLF TF 6dBm RF6 March 12, 2024');
grid on;
subplot(2,1,2);
semilogx(CLFTF_ampx, rad2deg(CLFOLphase),...
    CLFQMON_x, rad2deg(CLFOLphasezpk),'--','LineWidth', 1.5, 'Color', [0.8500 0.3250 0.0980]);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
xlim([1e2,1e5])
grid on;

%% phi CLF

dNsensCLF = CLFsenshighCLF.*CLF_RLG./CLF_cal.*CLFsenspole.*LO_RLG;
NCLF = sqrt( (CLFQMON_y.*oneplusCLFG).^2-(CLFsenshighCLF.*ones(size(CLFQMON_x)).*CLFsenspole).^2 )./CLF_cal; %dN before entering the loop

%for aLIGO case
phiCLF1 = (NCLF.*CLF_CLG);  %FC_CLG comes from FC_TF_foraLIGO
phiCLF3 = 2.*phiCLF1.*LO_RLG; %multiply by LO_1kRLG_long for no FC case, multiply by LO_RLG for aLIGO case

phiCLFtotal = sqrt(phiCLF3.^2 + (dNsensCLF).^2);

%% rms

flipdiffCLF1 = flip(diff(CLFQMON_x));
flipPSDCLF1 = flip(phiCLF1.^2);
meansquareCLF1 = cumsum(flipPSDCLF1(1:end-1).*flipdiffCLF1);
dNCLF1_rms = flip(sqrt(meansquareCLF1));


% flipdiffCLF1 = (diff(CLFhighpwr_x));
% flipPSDCLF1 = (phiCLF1.^2);
% meansquareCLF1 = cumsum(flipPSDCLF1(1:end-1).*flipdiffCLF1);
% dNCLF1_rms = (sqrt(meansquareCLF1));


flipdiffCLFtotal = flip(diff(CLFQMON_x));
flipPSDCLFtotal = flip(phiCLFtotal.^2);
meansquareCLFtotal = cumsum(flipPSDCLFtotal(1:end-1).*flipdiffCLFtotal);
dNCLFtotal_rms = flip(sqrt(meansquareCLFtotal));

% flipdiffCLFtotal = (diff(CLFhighpwr_x));
% flipPSDCLFtotal = (phiCLFtotal.^2);
% meansquareCLFtotal = cumsum(flipPSDCLFtotal(1:end-1).*flipdiffCLFtotal);
% dNCLFtotal_rms = (sqrt(meansquareCLFtotal));


flipdiffCLFsens = flip(diff(CLFQMON_x));
flipPSDCLFsens = flip(dNsensCLF.^2);
meansquaresens = cumsum(flipPSDCLFsens(1:end-1).*flipdiffCLFsens);
dNsensCLF_rms = flip(sqrt(meansquaresens));


flipdiffCLF3 = flip(diff(CLFQMON_x));
flipPSDCLF3 = flip(phiCLF3.^2);
meansquareCLF3 = cumsum(flipPSDCLF3(1:end-1).*flipdiffCLF3);
dNCLF3_rms = flip(sqrt(meansquareCLF3));


%phiCLFrms = sqrt(dNCLF2_rms.^2 + dNCLF1_rms.^2 + phigreeninloop_rms.^2);


%% color code
% https://htmlcolorcodes.com
% Convert color code to 1-by-3 RGB array (0~1 each)

str = '#13BAB0';
cyan = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;
str = '#1382BA';
blue1 = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;
str = '#929FB6';
bluegray = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;
str = '#5035BA';
blue2 = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;


%% Plot phiCLF length noise
figure(7000)

loglog(CLFQMON_x, 1e3.*phiCLF1, 'LineWidth', 2, 'Color', blue2);

xlabel('Frequency, Hz'), ylabel('mrad/\surd Hz');
grid on;
hold on

%loglog(CLFhighpwr_x, 1e3.*phiCLF2, 'LineWidth', 2, 'Color', blue1);
loglog(CLFQMON_x, 1e3.*phiCLF3, 'LineWidth', 2, 'Color', cyan);
%loglog(CLFhighpwr_x, 1e3.*philaser, 'LineWidth', 2, 'Color', [0.6350 0.0780 0.1840]);
%loglog(CLFhighpwr_x, 1e3.*sqrt( (CLFhighpwr_y.*CLFCM.*CLFA).^2), 'LineWidth', 2, 'Color', [0.3010 0.7450 0.9330]);
loglog(CLFQMON_x, 1e3.*dNsensCLF, 'LineWidth', 2, 'Color', bluegray);
%NCLFplot = loglog(CLFhighpwr_x, 1e3.*NCLF, 'LineWidth', 2, 'Color', [0.8500 0.3250 0.0980]);
%NCLFplot.Color(4) = 0;
%loglog(CLFhighpwr_x, 1e3.*phiCLFtotal, 'LineWidth', 2, 'Color', bluegray);

%loglog(CLFhighpwr_x, 1e3.*NCLFOLG, 'LineWidth', 2);
%plotimon = loglog(CLFhighpwr_x, 1e3.*CLFhighpwr_y./CLFS, 'LineWidth', 2);
%plotimon.Color(4) = 0.5;

%loglog(CLFhighpwr_x, 1e3.*CLFsens./CLFcalhighpwr, 'LineWidth', 2, 'Color', bluegray);
loglog(CLFQMON_x(1:end-1), 1e3.*dNCLFtotal_rms, '--', 'LineWidth', 2, 'Color', [0.4940 0.1840 0.5560]);

plotsensrms = loglog(CLFQMON_x(1:end-1), 1e3.*dNsensCLF_rms, '--', 'LineWidth', 2, 'Color', bluegray);
%plotsensrms.Color(4) = 0.5


loglog(CLFQMON_x(1:end-1), 1e3.*dNCLF1_rms, '--', 'LineWidth', 2, 'Color', blue2);
%loglog(CLFhighpwr_x(1:end-1), 1e3.*dNCLF2_rms, '--', 'LineWidth', 2, 'Color', blue1);
loglog(CLFQMON_x(1:end-1), 1e3.*dNCLF3_rms, '--', 'LineWidth', 2, 'Color', cyan);

%loglog(CLFhighpwr_x, 1e3.*CLFhighpwr_y.*oneplusCLFG./CLFS, 'LineWidth', 3, 'Color', blue2);

%loglog(CLFhighpwr_x, 1e3.*CLFsens.*ones(size(CLFhighpwr_x)).*CLFsenspole./CLFS, 'LineWidth', 2, 'Color', 'red');

%loglog(CLFhighpwr_x, 1e3.*CLFQMONprop, 'LineWidth', 3);

legend({'Term1+Term2 (\deltaN_{CLF}*CLF\_CLG)', 'Term3 (2*\deltaN_{CLF}*LO\_RLG)', '\deltaN_{sensCLF}*CLF\_RLG*LO\_RLG', 'Total \phi_{CLF} rms'},'FontSize',14, 'Location', 'Northeast');

set(gca,'FontSize',18);
hTitle = title('CLF Loop (6dBm RF6) March 12, 2024');
set(hTitle,'FontSize',16);

xlim([10,1e4])
ylim([1e-5,1e2])
%ylim([1e-12,1e2])

hold off
