% Nutsinee Kijbunchoo Nov1, 2018
% E/AOM TF


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

L = 2*0.175; %OPO cavity length 
FSR = c/(L); %Hz
%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) ); %Chua's thesis eq3.53
%Fin = 44
FWHM = FSR / Fin; 
V2Hz = 20/FSR;
cav_pole = (FSR/Fin)/2;


%% load EOM TF data 
EOMTF_amp = load('SCRN0000.TXT');
EOMTF_phase = load('SCRN0001.TXT');

EOMTF_ampx = 2*pi*EOMTF_amp(:,1); %freq
EOMTF_ampy = EOMTF_amp(:,2); %amp
EOMTF_phy = EOMTF_phase(:,2); %phase




%% DC
EOMTFamp = 10.^(EOMTF_ampy/20); %dB to power ratio
EOMcmplx = EOMTFamp.*exp(i.*deg2rad(EOMTF_phy)); 
EOMOLmag = abs(EOMcmplx);
EOMOLphase = angle(EOMcmplx);

%% Model
commongain = 3/2; %9dB /2 bc of the chip (100Ohms input)
%commongain = 5/2; %9dB /2 bc of the chip (100Ohms input)

PZTcalib = 1e6; %laser PZT Hz/V
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); %fudgefactor to get low f correct
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 = 12.6/2; %22dB
%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
phasetofreq = zpk(-2*pi*[0 ],-2*pi*[], 1/(2*pi)); %have to multiple 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;

%% making transfer function

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

P = (3.4./4e6); %optical gain (plant, input)
FASTPATH = PZTcalib.*fastgain.*fast.*0.5.*fastinterface.*fastinterfacepole*PZTpole;
EOMPATH = ((EOM+EOM_par).*EOMinterface.*EOMinterfacepole.*EOMcalib.*phasetofreq);
COMMONPATH = FSScom.*commongain.*commboost;

filter_FSS = P.*COMMONPATH.*( (FASTPATH)+(EOMPATH) ); 

[TSamp, TSphase] = bode(filter_FSS,  EOMTF_ampx);
TSamp = squeeze(TSamp);
TSphase = squeeze(TSphase);
TSfilter = TSamp.*exp(i.*deg2rad(TSphase));
TSOLmagzpk = abs(TSfilter);
TSOLphasezpk = angle(TSfilter);

OPOCLgain = abs(1./(1+TSOLmagzpk));


PDamp = zpk(-2*pi*[ ],-2*pi*[8e3 15e3 ], 1*8e3*15e3*4*pi^2); %have to multiple EOMcalib rad/V 
%PDampboard = zpk(-2*pi*[ ],-2*pi*[8e3 ], 1*8e3*2*pi); %have to multiple EOMcalib rad/V 


[PDamp, PDphase] = bode(PDamp, EOMTF_ampx);
PDamp = squeeze(PDamp);
PDphase = squeeze(PDphase);
PDfilter = PDamp.*exp(i.*deg2rad(PDphase));
PDOLmagzpk = abs(PDfilter);
PDOLphasezpk = angle(PDfilter);

%% Multiply two TF
%TF = EOMTFamp.*(1/OPOCLgain);
%magTF = abs(TF);

%%
figure(18);
subplot(2,1,1);
semilogx(EOMTF_ampx./(2*pi), 20*log10(PDOLmagzpk),'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
%xlim([1e2,1e6])
%legend('PD amp board (PDOLmagzpk)')
title('PD amp board (D1200543)');
grid on;
subplot(2,1,2);
semilogx(EOMTF_ampx./(2*pi), rad2deg(PDOLphasezpk), 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
grid on;

%% plot both (COMMON)

figure(19);
subplot(2,1,1);
semilogx(EOMTF_ampx./(2*pi), 20*log10(TSOLmagzpk),...
    EOMTF_ampx./(2*pi), 20*log10(OPOCLgain),'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
%xlim([1e2,1e6])
legend('TTFSS OLG model', '1/(1+OLG)')
title('Bode Plot TTFSS OLG COMMON PATH');
grid on;
subplot(2,1,2);
semilogx(EOMTF_ampx./(2*pi), rad2deg(TSOLphasezpk),...
    EOMTF_ampx./(2*pi), rad2deg(OPOCLgain), 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
grid on;


%% plot suppressed, measured

figure(20);
subplot(2,1,1);
semilogx(EOMTF_ampx./(2*pi), 20*log10(EOMOLmag), 'r','LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
title('Bode Plot TTFSS/GR launch PD, raw data, Nov 1, 2018');
grid on;
subplot(2,1,2);
semilogx(EOMTF_ampx./(2*pi), rad2deg(EOMOLphase), 'r', 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
grid on;

%% plot unsuppressed, calibrated

TTFSS_calib = 3.4/FWHM; %Vpk/FWHM
GR_PD_calib = 0.192*2e3*10*(5.5/100)*1e-3; %V/W gain of 10 from PD amp toggle

figure(21);
loglog(EOMTF_ampx./(2*pi), PDOLmagzpk.*EOMOLmag.*GR_PD_calib.*(1./TTFSS_calib)./(OPOCLgain), 'r','LineWidth', 1.5);
%semilogx(EOMTF_ampx, 20*log10(EOMOLmag)./(20*log10(OPOCLgain)), 'r','LineWidth', 1.5);
hold on
interp_HzpermW = interp1(EOMTF_ampx./(2*pi), PDOLmagzpk.*EOMTFamp.*GR_PD_calib.*(1./TTFSS_calib)./(OPOCLgain),GRPD_spec_x, 'pchip');

semilogx(GRPD_spec_x, interp_HzpermW, 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Hz/mW');
xlim([1e1,1e5])
legend({'TTFSS I-MON/GR launch PD', 'TTFSS I-MON/GR launch PD interpolated'},'FontSize',18)
title('TTFSS/GR launch PD, Calibrated, Corrected for Close Loop gain, Nov 1, 2018');
grid on;
hold off


%% GR Launch PD response
% PD to SR785 CH2
% EXC to EOM and T-off to CH1

%% load EOM TF data 
GRPD_TF_amp = load('GRPD_TF_amp.TXT');
GRPD_TF_phase = load('GRPD_TF_phase.TXT');

GRPD_TF_ampx = GRPD_TF_amp(:,1); %freq
GRPD_TF_ampy = GRPD_TF_amp(:,2); %amp
GRPD_TF_phase = GRPD_TF_phase(:,2); %amp

GRPD_spec = load('GRPD_spec.TXT');
GRPD_spec_x = GRPD_spec(:,1);
GRPD_spec_y = GRPD_spec(:,2);

%% Bode

GRPDTFamp = 10.^(GRPD_TF_ampy/20); %dB to power ratio
GRPDcmplx = GRPDTFamp.*exp(i.*deg2rad(GRPD_TF_phase)); 
GRPDOLmag = abs(GRPDcmplx);
GRPDOLphase = angle(GRPDcmplx);

%% plot TF

figure(22);
subplot(2,1,1);
semilogx(GRPD_TF_ampx, 20*log10(GRPDOLmag), 'r','LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
title('GR Launch PD Response (OPO locked on dual res), Nov 2, 2018');
grid on;
subplot(2,1,2);
semilogx(GRPD_TF_ampx, rad2deg(GRPDOLphase), 'r', 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
%xlim([1e1,1e5])
grid on;

%% plot spec

%interp_HzpermW = interp1(EOMTF_ampx./(2*pi), PDOLmagzpk.*EOMTFamp.*GR_PD_calib.*(1./TTFSS_calib)./(OPOCLgain),GRPD_spec_x, 'pchip');
%GOPOOLmagInt = interp1(GOPOTFx, GOPOOLmag, GOPOLNx, 'pchip');


cal = 0.19*2e3*10*(5.5/100); %(V/A)*(A/W)*PDgain20dB

figure(23);
loglog(GRPD_spec_x, (GRPD_spec_y.*1e3./cal).*interp_HzpermW, 'r','LineWidth', 1.5);
%semilogx(EOMTF_ampx, 20*log10(EOMOLmag)./(20*log10(OPOCLgain)), 'r','LineWidth', 1.5);
%hold on
%semilogx(EOMTF_ampx, 20*log10(EOMOLmag)./(20.*log10(OPOCLgain)), 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('Hz/\surdHz');
xlim([1e1,1e5])
legend({'Estimated frequency noise contribution from GR launch PD'},'FontSize',18)
title('GR launch PD Calibrated, Nov 2, 2018');
grid on;

%% plot GR launch in rad/rtHz

data = (GRPD_spec_y.*1e3./cal).*interp_HzpermW./GRPD_spec_x
% RMS
flipdiff = flip(diff(GRPD_spec_x));
flipPSD = flip(data.^2);
meansquare = cumsum(flipPSD(1:end-1).*flipdiff);
rms = flip(sqrt(meansquare));


figure(24);
loglog(GRPD_spec_x, (GRPD_spec_y.*1e3./cal).*interp_HzpermW./GRPD_spec_x, 'r','LineWidth', 1.5);
%semilogx(EOMTF_ampx, 20*log10(EOMOLmag)./(20*log10(OPOCLgain)), 'r','LineWidth', 1.5);
hold on
loglog(GRPD_spec_x(1:end-1), rms, 'LineWidth' ,2 );
%semilogx(EOMTF_ampx, 20*log10(EOMOLmag)./(20.*log10(OPOCLgain)), 'LineWidth', 1.5);
xlabel('Frequency, Hz'), ylabel('rad/\surdHz');
xlim([1e1,1e5])
legend({'Estimated frequency noise contribution from GR launch PD', 'rms'},'FontSize',18)
title('GR launch PD Calibrated, Nov 2, 2018');
grid on;
hold off

