%% SHG transfer function

SHGTF_amp = load('SHG_TFamp_7dBgain.TXT');
SHGTF_phase = load('SHG_TFph_7dBgain.TXT');
SHGTF_ampx = SHGTF_amp(:,1); %freq
SHGTF_ampy = SHGTF_amp(:,2); %amp
SHGTF_phy = SHGTF_phase(:,2); %phase

SHGTFamp = 10.^(SHGTF_ampy/20); %dB to power ratio
SHGcmplx = SHGTFamp.*exp(i.*deg2rad(SHGTF_phy)); 
SHGOLmag = abs(SHGcmplx);
SHGOLphase = angle(SHGcmplx);

%% model
Vpp = 79e-3; %Oct28 2019
SHGFWHM = 51.449e6;
SHGFSR = 3e9;
SHGcal = Vpp/SHGFWHM; %V/Hz

SHGcommongain = 2.24; %7dB

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

%% SHG stuff

cavLP = zpk(-2*pi*[],-2.*pi.*[SHGFWHM./2], 2.*pi.*SHGFWHM/2 ); 
cavHP = zpk(-2*pi*[0],-2.*pi.*[SHGFWHM./2], 1 ); 
PZTpole = zpk(-2*pi*[],-2.*pi.*[7e3 7e3], 2.*pi.*7e3.*2.*pi.*7e3 ); 

RFamp = 27.3;
IQdemodboardgain = 5.785;
SHGPZTcal = 60/SHGFSR; %V/Hz

SHGCM = SHGcommongain.*CM_boost1.*SLOW_boost.*pztnotch.*SLOW_filters;

SHGfilter = SHGcal.*SHGCM.*PZTdriver.*(1./SHGPZTcal);

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

[SHGamp, SHGphase] = bode(SHGfilter, 2*pi*x);
SHGamp = squeeze(SHGamp);
SHGphase = squeeze(SHGphase);
SHGfilterzpk = SHGamp.*exp(i.*deg2rad(SHGphase));
SHGOLmagzpk = abs(SHGfilterzpk);
SHGOLphasezpk = angle(SHGfilterzpk);



%% Plot
figure(230);
subplot(2,1,1);
semilogx(SHGTF_ampx, 20*log10(SHGOLmag),...
    x, 20*log10(SHGOLmagzpk),'--','LineWidth', 1.5, 'Color', [0.4660 0.6740 0.1880]);
xlabel('Frequency, Hz'), ylabel('Gain, dB');
ylim([-80,60])
xlim([1e1,1e5])
legend('Measured', 'Modeled')
title('SHG TF Oct 28, 2019');
grid on;
subplot(2,1,2);
semilogx(SHGTF_ampx, rad2deg(SHGOLphase),...
    x, rad2deg(SHGOLphasezpk), '--','LineWidth', 1.5, 'Color', [0.4660 0.6740 0.1880]);
xlabel('Frequency, Hz'), ylabel('Phase, deg.');
xlim([1e1,1e5])
grid on;

