%% SQZ Noise Budget
% 10dB attenuator is in

%% constants
c= 299792458; %speed of light
h = 6.626e-34; %J.s
%l = 532e-9; %laser wavelength
l = 1064e-9; %laser wavelength
fl = c/l; %laser frequency

n_ktp = 1.8868; % crystal index of refraction for green. 1.8296 for 1064. 
d_crys = 10e-3; %crystal length
L = 2*0.175; %OPO cavity length 
FSR = c/(L); %Hz c/round trip
%Fin = pi*(.875*.999*.999*.994)^(1/5)/( 1-sqrt(.875*.999*.999*.994) ); %Chua's thesis eq3.53
Fin = pi*(.98*.999*.999*.994)^(1/5)/( 1-sqrt(.98*.999*.999*.994) ); %green Fin
%Fin = 44
FWHM = FSR / Fin;
V2Hz = 40/FSR;
cav_pole = (FSR/Fin)/2;

%% Free-running NPRO model
f = logspace(-1,5,1e3);
fc = 1;
w = 2*pi*f;
wc = 2*pi*fc; %pole frequency [rad]
noise = 2*1e4./(1+(i*w/wc)); %[Hz/sqrt(Hz) -- estimate frequency noise of Mephisto laser -- basically a LP

%% Load data

LO_IMON = load('LO_IMON.TXT');
LO_IMON_x = LO_IMON(:,1);
LO_IMON_y = LO_IMON(:,2);

LO_ctrl = load('LO_ctrl.TXT');
LO_ctrl_x = LO_ctrl(:,1);
LO_ctrl_y = LO_ctrl(:,2);


OPO_IMON = load('OPO_IMON.TXT');
OPO_IMON_x = OPO_IMON(:,1);
OPO_IMON_y = OPO_IMON(:,2);

CLF_QMON = load('CLF_QMON.TXT');
CLF_QMON_x = CLF_QMON(:,1);
CLF_QMON_y = CLF_QMON(:,2);

TTFSS_IMON = load('FSS_IMON.TXT');
TTFSS_IMON_x = TTFSS_IMON(:,1);
TTFSS_IMON_y = TTFSS_IMON(:,2);

TTFSS_PZT = load('FSS_FASTMON.TXT');
TTFSS_PZT_x = TTFSS_PZT(:,1);
TTFSS_PZT_y = TTFSS_PZT(:,2);

TTFSS_EOM = load('FSS_EOMMON.TXT');
TTFSS_EOM_x = TTFSS_EOM(:,1);
TTFSS_EOM_y = TTFSS_EOM(:,2);

TTFSS_PZT_noLO = load('FSS_FASTMON_noLO.TXT');
TTFSS_PZT_noLO_x = TTFSS_PZT_noLO(:,1);
TTFSS_PZT_noLO_y = TTFSS_PZT_noLO(:,2);



%% Shotnoise, Darknoise, combined noise
% or google shotnoise on photodetector
% shotnoise (ASD) = sqrt(2*e*I)
%dLsn = 2310*sqrt(2*1.6e-19*0.6e-3)*5*sqrt(2);
%DN = 2310*sqrt(2*1.6e-19*0.29e-3)*5*sqrt(2);
%DNSN = sqrt(dLsn^2+DN^2);

%% LO TF model
LO_CMgain = 0.398; %14dB
LO_fastgain = 1.99; %6dB, factor of 2 going from red to green
IQdemod_cal = 5./2; %V/rad

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);
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*2*pi*20); %PZTdriver gain = 20 
%PFD = zpk(-2.*pi*[],-2.*pi*[0], 1.6*2.*pi); %1.6V/rad, 32deg/V, has to be measured in the lab
PZT = zpk([],[], 10.7e6); %OPO PZT calibration Hz/V for red, 21.4MHz/V for green
%PZT = zpk([],[], 21e6); %OPO PZT calibration Hz/V for red, 21.4MHz/V for green

PM = zpk(-2*pi*[60],-2*pi*[10], (1/60)*10*.0084); 
%flat = zpk([],[], 3.54*72e3/10); %flat from fast path. 11dB fast gain, 1/10 from AO
%flat = zpk([],[], 20.9e3); %Hz/V calibration fast path, use OPO refl Vpp/Hz
flat = zpk([],[], 20.9e3/2); %Hz/V calibration fast path, use OPO refl Vpp/Hz

oneonf = zpk([],-2*pi*[0], 2*pi ); %phase detection frequency actuation
laserPZTpole = zpk([],-2*pi*[0], 2*pi );

common_path = LO_CMgain.*CM_comp.*CM_boost1; %no boost2 today
slow_path = SLOW_boost.*SLOW_filters.*PM.*PZTdriver.*PZT;
fast_path = LO_fastgain.*flat; 

%gain = 1./(0.233.*FSR); %LO rad per Hz


OPO_IMON = PM*PZTdriver;
LO_IMON = flat;
LO_TF = (1/IQdemod_cal).*common_path.* ( (fast_path) + (slow_path) ).*oneonf;


%LOTF_ampx_lf = logspace(0,6,1e5);

[LOslowamp, LOslowphase] = bode(OPO_IMON, 2*pi*OPO_IMON_x); 
LOslowamp = squeeze(LOslowamp);
LOslowphase = squeeze(LOslowphase);
LOslowfilter = LOslowamp.*exp(i.*deg2rad(LOslowphase));
LOSLOWmagzpk = abs(LOslowfilter);
LOSLOWphasezpk = angle(LOslowfilter);

[LOfastamp, LOfastphase] = bode(LO_IMON, 2*pi*LO_IMON_x); 
LOfastamp = squeeze(LOfastamp);
LOfastphase = squeeze(LOfastphase);
LOfastfilter = LOfastamp.*exp(i.*deg2rad(LOfastphase));
LOFASTmagzpk = abs(LOfastfilter);
LOFASTphasezpk = angle(LOfastfilter);


% [LOamp, LOphase] = bode(LO_TF, 2*pi*LO_IMON_x); 
% LOamp = squeeze(LOamp);
% LOphase = squeeze(LOphase);
% LOfilter = LOamp.*exp(i.*deg2rad(LOphase));
% LOOLmagzpk = abs(LOfilter);
% LOOLphasezpk = angle(LOfilter);
% 
% LOCLgain = abs(1./(1+LOOLmagzpk));


%% TTFSS TF model
%commongain = 2.8/2; %9dB /2 bc of the chip (100Ohms input)
%commongain = 5/2; %13.9dB /2 bc of the chip (100Ohms input)
commongain = 5.62/2; %15dB /2 bc of the chip (100Ohms input)

PZTcalib = 1.7e6; %laser PZT Hz/V, red
%PZTcalib = 2*1.7e6; %laser PZT Hz/V, green

%PZTcalib = 11.8967e6; %laser PZT Hz/V, green, calibrated Jan 11
%PZTcalib = 0.202e6; %laser PZT Hz/V, green, calibrated Jan 11

FSScom = zpk(-2*pi*[1.6e6  ],-2*pi*[ 75.8e3 ], 0.952);
commboost = zpk(-2*pi*[ 480 ],-2*pi*[ 0 ], 1); 

temp = zpk(-2*pi*[ ],-2*pi*[40 ], 1*40*2*pi*0.2);
%fast path
%When EOM not on
%23e3*2*pi fudge factor to make low f gain correct
fastonlyboost1 = zpk(-2*pi*[300 ],-2*pi*[30 23e3], 4.5e5); 
fastonlyboost2 = zpk(-2*pi*[500 ],-2*pi*[5], 0.99);
fastboost2 = zpk(-2*pi*[ 150],-2*pi*[0 ], 1); %calculated gain with C was 3.16e7

fastonly = fastonlyboost1*fastonlyboost2*fastboost2;

fastinterface = zpk(-2*pi*[100 ],-2*pi*[10 ], 1);
fastinterfacepole = zpk(-2*pi*[23e3],-2*pi*[100], (1/23e3)*100); 
PZTpole = zpk(-2*pi*[ ],-2*pi*[100e3], 1*100e3*2*pi); 

%EOM boosts
EOMboost1 = zpk(-2*pi*[0],-2*pi*[1.6e3 1.7e3], 1*3e5); 
EOMboost2 = zpk(-2*pi*[0 32e3],-2*pi*[1.2e3 1e3], 1*0.9667);
%EOMboost3 = zpk(-2*pi*[3.2e3],-2*pi*[0], 20);
EOMboost3 = zpk(-2*pi*[0],-2*pi*[3.2e3], 20);

%When EOM on
fastgain = 15/2; %22dB
%fastgain = 12.6/2; %22dB
%fastgain = 20/2; %26dB

%fastgain = 15.8/2; %22dB

%fastgain = 1*0.5; % /2 at the input
fastboost1 = zpk(-2*pi*[ ],-2*pi*[2.3e3 ], 9e5);  
fastLP = zpk(-2*pi*[ ],-2*pi*[130e3], 5.128e6);
%fast = fastboost1*fastLP*fastboost2*notch130k;

%fastinterface is the same
EOM = EOMboost1*EOMboost2*EOMboost3;
EOM_par = zpk(-2*pi*[0 340e3 ],-2*pi*[50e3 760e3], 8); 
EOMinterface = zpk(-2*pi*[0 3e6 ],-2*pi*[240 33e3], 0.33);
EOMinterfacepole = zpk(-2*pi*[ ],-2*pi*[2.8e6], 1*2.8e6*2*pi); 
EOMcalib = 15e-3; %15mrad/V for red
%EOMcalib = 30e-3; %15mrad/V for red 30mrad for green

phasetofreq = zpk(-2*pi*[0 ],-2*pi*[], 1/(2*pi)); %have to multiply EOMcalib rad/V 
%with freq to get Hz/V that agrees with PZT actuator Unit and cancels out
%PD detector V/Hz

%% notches

% what's in the [] is already including 2pi (w)
notch130k = zpk(1e5*[ -0.351063829787234+(1j*8.290539855508047) -0.351063829787234-(1j*8.290539855508047)],...
            1e5*[ -5.659574468085103+(1j*6.068402899326267) -5.659574468085103-(1j*6.068402899326267)], 1);
notch210k = zpk(1e6*[ -0.035106382978723+(1j*1.377847448198228) -0.035106382978723-(1j*1.377847448198228)],...
            1e6*[ -0.565957446808511+(1j*1.256737131238753) -0.565957446808511-(1j*1.256737131238753)], 1);
notch280k = zpk(1e6*[ -0.075000000000000+(1j*1.753864047927675) -0.075000000000000-(1j*1.753864047927675)],...
            1e6*[ -1.209090909090909+(1j*1.272699207262647) -1.209090909090909-(1j*1.272699207262647)], 1);
%notch73k = zpk(1e5*[ 0+(1j*4.612656040144425) 0-(1j*4.612656040144425)],...
%            1e5*[ -2.648936170212766+(1j*3.776206153114442) -2.648936170212766-(1j*3.776206153114442)], 1);
%5 Ohm fudge factor
notch73k = zpk(1e5*[ -0.212765957446809+(1j*4.588245809226557) -0.212765957446809-(1j*4.588245809226557)],...
            1e5*[ -2.861702127659575+(1j*3.592760761737690) -2.861702127659575-(1j*3.592760761737690)], 1);
%1 Ohm fudge factor
%notch73k = zpk(1e5*[ -0.010638297872340+(1j*4.593164027677620) -0.010638297872340-(1j*4.593164027677620)],...
%            1e5*[ -5.681818181818181+(1j*3.576084503804999) -5.681818181818181-(1j*3.576084503804999)], 1*1.5);

IQdmod_diffin = zpk(-2*pi*[ ],-2*pi*[1.6e6 723e3], 4.5e15);       
        
fast = fastboost1*fastLP*fastboost2*notch73k*notch130k*notch210k*notch280k;

%% CLF VCO TF
VCXO = zpk(-2*pi*[ 40],-2*pi*[1], (1/40).*869.5111); %Hz/V 615Hz/V at DC

%% LO VCO
VCO = zpk(-2*pi*[ 40],-2*pi*[1.6], 281479.*(1/40).*1.6); %Hz/V 80MHz VCO calibration at DC
%VCO = VCO.*oneonf;
%% making transfer function

FSSTF_ampx_lf = logspace(0,7,1e5);

P = (1.84./FWHM); %optical gain (plant, input), PDH Vpk


FASTPATH = PZTcalib.*fastgain.*fast.*0.5.*fastinterface.*fastinterfacepole*PZTpole;
EOMPATH = ((EOM+EOM_par).*EOMinterface.*EOMinterfacepole.*EOMcalib.*phasetofreq);
COMMONPATH = FSScom.*commongain.*commboost;

% All that's needed for the control signal 
filter_FSS_EOM = EOMinterface.*EOMinterfacepole.*EOMcalib.*phasetofreq; %Hz/V
filter_FSS_FAST = 0.5.*fastinterface.*fastinterfacepole.*PZTpole.*PZTcalib; %Hz/V factor of 1/2 on interface board

[TSEOMamp, TSEOMphase] = bode(filter_FSS_EOM, 2*pi*TTFSS_EOM_x);
TSEOMamp = squeeze(TSEOMamp);
TSEOMphase = squeeze(TSEOMphase);
TSEOMfilter = TSEOMamp.*exp(i.*deg2rad(TSEOMphase));
TSEOMOLmagzpk = abs(TSEOMfilter);
TSEOMOLphasezpk = angle(TSEOMfilter);

[TSFASTamp, TSFASTphase] = bode(filter_FSS_FAST, 2*pi*TTFSS_PZT_x);
TSFASTamp = squeeze(TSFASTamp);
TSFASTphase = squeeze(TSFASTphase);
TSFASTfilter = TSFASTamp.*exp(i.*deg2rad(TSFASTphase));
TSFASTOLmagzpk = abs(TSFASTfilter);
TSFASTOLphasezpk = angle(TSFASTfilter);
% 
[VCOamp, VCOphase] = bode(VCO, 2*pi*LO_ctrl_x);
VCOamp = squeeze(VCOamp);
VCOphase = squeeze(VCOphase);
VCOfilter = VCOamp.*exp(i.*deg2rad(VCOphase));
VCOOLmagzpk = abs(VCOfilter);
VCOOLphasezpk = angle(VCOfilter);

%% plot TTFSS FAST TF
% FSSTF_ampx_lf = logspace(0,7,1e5);
% 
figure(27);
subplot(2,1,1);
semilogx(LO_ctrl_x, 20*log10(VCOOLmagzpk),'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
%xlim([1e2,1e6])
%legend('PD amp board (PDOLmagzpk)')
title('CLF VCO');
grid on;
subplot(2,1,2);
semilogx(LO_ctrl_x, rad2deg(VCOOLphasezpk), 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
grid on;



%% calibration

PFDcal = 0.6283; %rad/V, PFD LO calibration -- has to be measured in the lab
LOcal = 9.68./2; %V/rad
CLFcal = 432e-3; %V/rad
OPOcal = (2*5.2*222e-3)./FWHM; %V/Hz *2 for red bc we're comparing it with everything else red

%% RMS
% 
% %LO out of loop rms
% flipdiff = flip(diff(LO_IMON_x));
% flipPSD = flip((LO_cal_fast.*LOFASTmagzpk.*LO_IMON_y).^2);
% meansquare = cumsum(flipPSD(1:end-1).*flipdiff);
% rms = flip(sqrt(meansquare));
% 
% %LO in-loop rms
% flipdiff2 = flip(diff(OPO_IMON_x));
% flipPSD2 = flip((LO_cal_slow.*OPO_IMON_y.*LOSLOWmagzpk).^2);
% meansquare2 = cumsum(flipPSD2(1:end-1).*flipdiff2);
% rms2 = flip(sqrt(meansquare2));
% % 
% %CLF QMON rms
% flipdiff3 = flip(diff(CLF_QMON_x));
% flipPSD3 = flip((1e3.*CLF_QMON_y/(340e-3)).^2);
% meansquare3 = cumsum(flipPSD3(1:end-1).*flipdiff3);
% CLF_QMON_rms = flip(sqrt(meansquare3));
% 
% %CLF FAST rms
% flipdiff4 = flip(diff(CLF_FAST_x));
% flipPSD4 = flip((1e3.*CLF_FAST_y.*VCOOLmagzpk./CLF_FAST_x).^2);
% meansquare4 = cumsum(flipPSD4(1:end-1).*flipdiff4);
% CLF_FAST_rms = flip(sqrt(meansquare4));

%% plot LO fast and slow

figure(300)
set(gcf,'color',[0.5, 0.5, 0.5]) 
fig = gcf;
fig.InvertHardcopy = 'off';

PZTMON = loglog(TTFSS_PZT_x, 1e3.*TTFSS_PZT_y.*TSFASTOLmagzpk./TTFSS_PZT_x,  '--', 'LineWidth', 1.5, 'Color', [0, 0.4470, 0.7410]);
PZTMON.Color(4) = 0.5;

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

EOMMON = loglog(TTFSS_EOM_x, 1e3.*TTFSS_EOM_y.*TSEOMOLmagzpk./TTFSS_EOM_x,  '--', 'LineWidth', 1.5, 'Color', [0.9290, 0.6940, 0.1250]);
EOMMON.Color(4) = 0.5;
loglog(OPO_IMON_x, 1e3.*(1./OPO_IMON_x).*OPO_IMON_y./OPOcal, 'LineWidth', 2, 'Color', [0.6350, 0.0780, 0.1840]);

%loglog(TTFSS_PZT_x, TTFSS_PZT_y.*TSFASTOLmagzpk,   'LineWidth', 2, 'Color', [0, 0.4470, 0.7410]);
%loglog(TTFSS_EOM_x, TTFSS_EOM_y.*TSEOMOLmagzpk,   'LineWidth', 2, 'Color', [0.8500, 0.3250, 0.0980]);
loglog(LO_IMON_x, 1e3.*LO_IMON_y./LOcal ,'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]);

LOctrl = loglog(LO_ctrl_x, 1e3.*LO_ctrl_y.*VCOOLmagzpk./LO_ctrl_x ,'--', 'LineWidth', 1.5, 'Color', [0.4940, 0.1840, 0.5560]);
LOctrl.Color(4) = 0.5;

loglog(CLF_QMON_x, 1e3.*CLF_QMON_y./CLFcal ,'LineWidth', 2, 'Color', [0.8500, 0.3250, 0.0980]);
%loglog(TTFSS_IMON_x, 1e3.*TTFSS_IMON_y.*PFDcal , 'LineWidth', 2, 'Color', [0, 0.4470, 0.7410]);



%p6 = loglog(LO_FAST_x(1:end-1), rms, '--', 'LineWidth', 1.5, 'Color', [0, 0.4470, 0.7410]); 
%p7 = loglog(LO_FAST_x(1:end-1), rms2, '--', 'LineWidth', 1.5, 'Color',  [0.8500, 0.3250, 0.0980]); 

set(gca,'FontSize',18, 'Color', 'k');
hTitle = title('LHO SQZ Noise Budget L1 scheme (Jan 25)');
xlim([10,1e5])
ylim([1e-4,1e4])
ax = gca
ax.XColor = 'k'; 
ax.YColor = 'k'; 
ax.MinorGridColor = 'w';
legend({'\color{white} TTFSS PZT output','\color{white} TTFSS EOM output', '\color{white} OPO IMON (in-loop)', '\color{white} LO IMON (in-loop)', '\color{white} LO FAST output', '\color{white} CLF QMON (in-loop)','\color{white} TTFSS IMON (in-loop)'},'FontSize',14, 'Location', 'Northeast');
hold off

%%
figure(301)
set(gcf,'color',[0.5, 0.5, 0.5]) 
fig = gcf;
fig.InvertHardcopy = 'off';


PZTMON = loglog(TTFSS_PZT_x, TTFSS_PZT_y.*TSFASTOLmagzpk,  '--', 'LineWidth', 1.5, 'Color', [0, 0.4470, 0.7410]);
PZTMON.Color(4) = 0.5;

xlabel('Frequency, Hz'), ylabel('Hz/\surd Hz');
grid on;
hold on
EOMMON = loglog(TTFSS_EOM_x, TTFSS_EOM_y.*TSEOMOLmagzpk,  '--', 'LineWidth', 1.5, 'Color', [0.9290, 0.6940, 0.1250]);
EOMMON.Color(4) = 0.5;

loglog(OPO_IMON_x, OPO_IMON_y./OPOcal, 'LineWidth', 2, 'Color', [0.6350, 0.0780, 0.1840]);

LOIMON=loglog(LO_IMON_x, LO_IMON_x.*LO_IMON_y./LOcal , 'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]);

LOctrl = loglog(LO_ctrl_x, LO_ctrl_y.*VCOOLmagzpk ,'--', 'LineWidth', 1.5, 'Color', [0.4940, 0.1840, 0.5560]);
LOctrl.Color(4) = 0.5;

loglog(CLF_QMON_x, CLF_QMON_x.*CLF_QMON_y./CLFcal ,'LineWidth', 2, 'Color', [0.8500, 0.3250, 0.0980]);

%loglog(TTFSS_IMON_x, TTFSS_IMON_y.*PFDcal.*TTFSS_IMON_x , 'LineWidth', 2, 'Color', [0, 0.4470, 0.7410]);


%p6 = loglog(LO_FAST_x(1:end-1), rms, '--', 'LineWidth', 1.5, 'Color', [0, 0.4470, 0.7410]); 
%p7 = loglog(LO_FAST_x(1:end-1), rms2, '--', 'LineWidth', 1.5, 'Color',  [0.8500, 0.3250, 0.0980]); 

set(gca,'FontSize',18, 'Color', 'k');
hTitle = title('LHO SQZ Noise Budget L1 scheme (Jan 25)');
xlim([10,1e5])
ylim([1e-4,1e4])
ax = gca
ax.XColor = 'k'; 
ax.YColor = 'k'; 
ax.MinorGridColor = 'w';
legend({'\color{white} TTFSS PZT output', '\color{white} TTFSS EOM output',  '\color{white} OPO IMON (in-loop)', '\color{white} LO IMON (in-loop)', '\color{white} LO FAST output','\color{white} CLF QMON (in-loop)','\color{white} TTFSS IMON (in-loop)'},'FontSize',14, 'Location', 'Northeast')
hold off

%% LO in-loop rms
flipdiff2 = flip(diff(LO_IMON_x));
flipPSD2 = flip((1e3.*LO_IMON_y./(2.74/2)).^2);
meansquare2 = cumsum(flipPSD2(1:end-1).*flipdiff2);
LO_IMON_rms = flip(sqrt(meansquare2));

%% plot LO rms

figure(302)
loglog(LO_IMON_x, 1e3.*LO_IMON_y./(2.74/2) ,'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]);
xlabel('Frequency, Hz'), ylabel('mrad/\surd Hz');
grid on;
hold on
loglog(LO_IMON_x(1:end-1), LO_IMON_rms, '--', 'LineWidth', 1.5, 'Color', [0.4940, 0.1840, 0.5560]); 

set(gca,'FontSize',18);
hTitle = title('LO Phase Noise L1 Scheme (Jan 25, 2019)');
xlim([10,1e5])
ylim([1e-3,1e2])
legend({'LO IMON in-loop', 'rms'},'FontSize',14, 'Location', 'Northeast')
hold off
