%% PMC transfer function

%% Call spec to get the x axes
run 'Marc2024ALLSQZSPEC.m'
%run LOTF_highCLF_March112024.m

%% load TF measurement
PMCTF_amp = load('PMC_March112024_amp.TXT');
PMCTF_phase = load('PMC_March112024_phase.TXT');
PMCTF_freq = PMCTF_amp(:,1); %freq
PMCTF_amp = PMCTF_amp(:,2); %amp
PMCTF_ph = PMCTF_phase(:,2); %phase

PMCTFamp = 10.^(PMCTF_amp/20); %dB to power ratio
PMCcmplx = PMCTFamp.*exp(i.*deg2rad(PMCTF_ph)); 
PMCOLmag = abs(PMCcmplx);
PMCOLphase = angle(PMCcmplx);

%% model
%Vpp = 650e-3; %fudge it
%SHGFWHM = 100e6;
PMCFSR = 713e6; %Vicky measurement at MIT
%SHGcal = Vpp/SHGFWHM; %Already defined in Marc2024ALLSQZSPEC.m
PMCcommongain = 0.025; %-32dB
PMCcavLP = zpk(-2*pi*[],-2.*pi.*[PMCFWHM./2], 2.*pi.*PMCFWHM/2 ); 
PMCcavHP = zpk(-2*pi*[0],-2.*pi.*[PMCFWHM./2], 1 ); 

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);
%PZTcal = zpk([],[], 10.7e6);  %Hz/V    
        
% optional daughter board
pztnotch = zpk([-694.29+1j*41651.73, -694.29-1j*41651.73 -2*pi*265],...
            [-20828.759+1j*36076.469, -20828.759-1j*36076.469 -2*pi*66], 1);
pole = zpk(-2*pi*[],-2*pi*[0], 2*pi ); %phase detection frequency actuation
zero = zpk(-2*pi*[0],-2*pi*[], 1/(2*pi) ); %phase detection frequency actuation


RFamp = 27.3;
IQdemodboardgain = 5.785;
PMCPZTcal = 84.6/PMCFSR; %V/Hz Measured at LHO

PMCCM = PMCcommongain.*CM_boost1.*SLOW_boost.*pztnotch.*SLOW_filters;
PMCA = PZTdriver.*(1./PMCPZTcal).*pole;
PMCS = PMCcal.*zero;
PMCfilter = PMCcal.*PMCCM.*PZTdriver.*(1./PMCPZTcal).*1.5; %1.5 fudge factor

%x = logspace(0,10,1e5);

[PMCamp, PMCphase] = bode(PMCfilter, 2*pi*PMCIMON_x);
PMCamp = squeeze(PMCamp);
PMCphase = squeeze(PMCphase);
PMCfilterzpk = PMCamp.*exp(i.*deg2rad(PMCphase));
PMCOLmagzpk = abs(PMCfilterzpk);
PMCOLphasezpk = angle(PMCfilterzpk);

[PMCCMamp, PMCCMphase] = bode(PMCCM, 2*pi*PMCIMON_x);
PMCCMamp = squeeze(PMCCMamp);
PMCCMphase = squeeze(PMCCMphase);
PMCCMfilterzpk = PMCCMamp.*exp(i.*deg2rad(PMCCMphase));
PMCCMmagzpk = abs(PMCCMfilterzpk);
PMCCMphasezpk = angle(PMCCMfilterzpk);

[PMCAamp, PMCAphase] = bode(PMCA, 2*pi*PMCIMON_x);
PMCAamp = squeeze(PMCAamp);
PMCAphase = squeeze(PMCAphase);
PMCAfilterzpk = PMCAamp.*exp(i.*deg2rad(PMCAphase));
PMCAmagzpk = abs(PMCAfilterzpk);
PMCAphasezpk = angle(PMCAfilterzpk);

[PMCSamp, PMCSphase] = bode(PMCS, 2*pi*PMCIMON_x);
PMCSamp = squeeze(PMCSamp);
PMCSphase = squeeze(PMCSphase);
PMCSfilterzpk = PMCSamp.*exp(i.*deg2rad(PMCSphase));
PMCSmagzpk = abs(PMCSfilterzpk);
PMCSphasezpk = angle(PMCSfilterzpk);

[PMCamponeplus, PMCphaseoneplus] = bode(1+PMCfilter, 2*pi*PMCIMON_x);
PMCamponeplus = squeeze(PMCamponeplus);
PMCphaseoneplus = squeeze(PMCphaseoneplus);
PMCfilteronepluszpk = PMCamponeplus.*exp(i.*deg2rad(PMCphaseoneplus));
oneplusPMCmagzpk = abs(PMCfilteronepluszpk);
oneplusPMCphasezpk = angle(PMCfilteronepluszpk);


[oneononeplusPMCamp, oneononeplusPMCPMCphase] = bode(1/(1+PMCfilter), 2*pi*PMCIMON_x);
oneononeplusPMCamp = squeeze(oneononeplusPMCamp);
oneononeplusPMCPMCphase = squeeze(oneononeplusPMCPMCphase);
oneononeplusPMCPMCfilterzpk = oneononeplusPMCamp.*exp(i.*deg2rad(oneononeplusPMCPMCphase));
oneononeplusPMCmagzpk = abs(oneononeplusPMCPMCfilterzpk);
oneononeplusPMCphasezpk = angle(oneononeplusPMCPMCfilterzpk);

[GoneononeplusPMCamp, GoneononeplusPMCphase] = bode(PMCfilter/(1+PMCfilter), 2*pi*PMCIMON_x);
GoneononeplusPMCamp = squeeze(GoneononeplusPMCamp);
GoneononeplusPMCphase = squeeze(GoneononeplusPMCphase);
GoneononeplusPMCfilterzpk = GoneononeplusPMCamp.*exp(i.*deg2rad(GoneononeplusPMCphase));
GoneononeplusPMCmagzpk = abs(GoneononeplusPMCfilterzpk);
GoneononeplusPMCphasezpk = angle(GoneononeplusPMCfilterzpk);

[PMCLPamp, PMCLPphase] = bode(PMCcavLP, 2*pi*PMCIMON_x);
PMCLPamp = squeeze(PMCLPamp);
PMCLPphase = squeeze(PMCLPphase);
PMCLPfilterzpk = PMCLPamp.*exp(i.*deg2rad(PMCLPphase));
PMCLPOLmagzpk = abs(PMCLPfilterzpk);
PMCLPOLphasezpk = angle(PMCLPfilterzpk);

[PMCHPamp, PMCHPphase] = bode(PMCcavHP, 2*pi*PMCIMON_x);
PMCHPamp = squeeze(PMCHPamp);
PMCHPphase = squeeze(PMCHPphase);
PMCHPfilterzpk = PMCHPamp.*exp(i.*deg2rad(PMCHPphase));
PMCHPOLmagzpk = abs(PMCHPfilterzpk);
PMCHPOLphasezpk = angle(PMCHPfilterzpk);

GononeplusPMC_G = GoneononeplusPMCmagzpk; % G/(1+G)
PMC_RLG = GononeplusPMC_G;
oneononeplusPMC_G = oneononeplusPMCmagzpk; % 1/(1+G)
PMC_CLG = oneononeplusPMC_G;
oneplusPMC_G = oneplusPMCmagzpk; %1+G
PMC_G = PMCOLmagzpk; %G
PMC_CM = PMCCMmagzpk;
PMC_A = PMCAmagzpk;
PMC_S = PMCSmagzpk;
PMC_LP = PMCLPOLmagzpk;
PMC_HP = PMCHPOLmagzpk;



%% Plot
figure(230);
subplot(2,1,1);
semilogx(PMCTF_freq, 20*log10(PMCOLmag),...
    SHGIMON_x, 20*log10(PMCOLmagzpk),'-.','LineWidth', 1.5, 'Color', [0.9290 0.6940 0.1250]);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
ylim([-80,60])
xlim([1e2,1e4])
legend('Measured', 'Modeled', 'Modeled G/(1+G)')
title('PMC TF March 11, 2024');
grid on;
subplot(2,1,2);
semilogx(PMCTF_freq, rad2deg(PMCOLphase),...
    SHGIMON_x, rad2deg(PMCOLphasezpk),'-.','LineWidth', 1.5, 'Color', [0.9290 0.6940 0.1250]);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
xlim([1e2,1e4])
grid on;


%% phi PMC


dNPMCsens = ones(size(PMCIMON_x)).*PMCsens; %V/sqrt(Hz)
dNPMC =sqrt((PMCIMON_y./PMC_CLG).^2-dNPMCsens.^2)./(PMC_LP.*PMC_S);
dNPMCinloop = dNPMC.*PMC_CLG.*PMCHPOLmagzpk;
%philaserinSHG =  sqrt((philaser.*SHG_HP.*SHG_RLG).^2 + (philaser.*SHG_LP).^2) ; %philaser comes in rad
PMCsensinloop = dNPMCsens.*(PMC_RLG).*PMC_HP./(PMC_LP.*PMC_S);

%philaserinSHG shouldn't be included if we're after the noise contribution to phi_sqz. It's already included in LO_IMON 
%dNSHG is not included because we can't say for sure we measured any length
%noise.
phiPMC = sqrt( (2.*PMCsensinloop).^2+ (2.*dNPMCinloop).^2); 

phiPMC_LOCLG = phiPMC.*LO_CLG./2;
%phipump_LOCLG1k = phipump.*LO_1kCLG./2;

%% rms
flipdiffsens = flip(diff(PMCIMON_x));
flipPSDsens = flip((2.*PMCsensinloop).^2);
meansquaresens = cumsum(flipPSDsens(1:end-1).*flipdiffsens);
PMCsensinloop_rms = flip(sqrt(meansquaresens));


flipdiffmockctrl = flip(diff(PMCIMON_x));
flipPSDmockctrl = flip((2.*dNPMCinloop).^2);
meansquaremockctrl = cumsum(flipPSDmockctrl(1:end-1).*flipdiffmockctrl);
dNPMCinloop_rms = flip(sqrt(meansquaremockctrl));
% 
% flipdiffrms1 = flip(diff(SHGIMON_x));
% flipPSDrms1 = flip((2.*philaserinSHG).^2);
% meansquarerms1 = cumsum(flipPSDrms1(1:end-1).*flipdiffrms1);
% philaserinSHG_rms = flip(sqrt(meansquarerms1));

flipdiffrms2 = flip(diff(PMCIMON_x));
flipPSDrms2 = flip(phiPMC.^2);
meansquarerms2 = cumsum(flipPSDrms2(1:end-1).*flipdiffrms2);
phiPMC_rms = flip(sqrt(meansquarerms2));

flipdiffrms3 = flip(diff(PMCIMON_x));
flipPSDrms3 = flip(phiPMC_LOCLG.^2);
meansquarerms3 = cumsum(flipPSDrms3(1:end-1).*flipdiffrms3);
phiPMC_LOCLGrms = flip(sqrt(meansquarerms3));


%phipump_rms = sqrt(philaserinSHG_rms.^2 + SHGsensinloop_rms.^2 );% + dNSHGinloop_rms.^2);
%% color
% https://htmlcolorcodes.com
% Convert color code to 1-by-3 RGB array (0~1 each)
str = '#C2921B';
darkyellow = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;

str = '#759E14';
green1 = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;

str = '#9E6614';
brown = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;

str = '#C29119';
darkyellow = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;


%% plot phi PMC

figure(8001)

SHGctrlplot = loglog(PMCIMON_x, 1e3.*dNPMCinloop, 'LineWidth', 2, 'color', [0.9290 0.6940 0.1250]); 

%SHGctrlplot.Color(4) = 0.5;

grid on
hold on
%philaserinSHGplot = loglog(SHG_x, 1e3.*2.*philaserinSHG, 'LineWidth', 2, 'Color', [0.6350 0.0780 0.1840]); 
SHGsensplot = loglog(PMCIMON_x, 1e3.*PMCsensinloop, 'LineWidth', 5, 'Color', brown); 
phipumpplot = loglog(PMCIMON_x, 1e3.*phiPMC, 'LineWidth',  3, 'Color', darkyellow); 
loglog(PMCIMON_x, 1e3.*phiPMC_LOCLG, 'LineWidth',  3, 'Color', [0.4940 0.1840 0.5560]); 


SHGctrlplotrmsplot = loglog(PMCIMON_x(1:end-1), 1e3.*dNPMCinloop_rms, '--', 'LineWidth',  2, 'Color', [0.9290 0.6940 0.1250]); 
%philaserinSHG_rmsplot = loglog(SHGIMON_x(1:end-1), 1e3.*philaserinSHG_rms, '--', 'LineWidth',  2, 'Color', [0.6350 0.0780 0.1840]); 
SHGsensinloop_rmsplot = loglog(PMCIMON_x(1:end-1), 1e3.*PMCsensinloop_rms, '--', 'LineWidth',  2, 'Color', brown); 

phipumprmsplot = loglog(PMCIMON_x(1:end-1), 1e3.*phiPMC_rms, '--', 'LineWidth',  3, 'Color', darkyellow); 
loglog(PMCIMON_x(1:end-1), 1e3.*phiPMC_LOCLGrms, '--', 'LineWidth',  3, 'Color', [0.4940 0.1840 0.5560]); 



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


legend({'\deltaN_{PMC}*PMC\_CLG*PMC\_HP',... 
    '\deltaN_{sensPMC}*PMC\_RLG*PMC\_HP', '\phi_{PMC}', '\phi_{PMC}*LO\_CLG'} , 'FontSize',14, 'Location', 'Northeast');

set(gca,'FontSize',18);
hTitle = title('PMC Loop march 11, 2024');
set(hTitle,'FontSize',16);
ylim([1e-6,1e4])
xlim([10,1e4])

%grid minor
hold off




