% Noise budget  PLL test script
%
% $Id$

%% Add paths
clear all
close all
addpath('../');
findNbSVNroot;
addpath(genpath([NbSVNroot 'Common/Utils']));
addpath(genpath([NbSVNroot 'Dev/Utils/']));

%% Define some filters 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PLL%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Changing_unit = zpk([],[0],2*pi);
Sen = 1;
%Filt_Temp = 0;
Filt_PZT = zpk([-2*pi*5e3,-2*pi*5e3],[0,0],0.01);
%Act_Temp = 0
Act_PZT =  2e6;
%Gol = Sen*Filt_Temp*Act_Temp+Sen*Filt_PZT*Act_PZT;
Gol_PLL = Changing_unit*Sen*Filt_PZT*Act_PZT;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PDH%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%parameter%%%%%%%%%
c = 299792458;
L = 3000;
Finesse = 10;
lambda = 1.064e-6;
m2Hz = 2/lambda * c/2/L;
%%%%%%%%%%%%%%%%%%%%%%%%%

Phase2freq = zpk([0],[1e5,1e5],1/(2*pi)); 
Cavity_Pole = zpk([],[-2*pi*c/4/L/Finesse],2*pi*c/4/L/Finesse);
Sen_1 = Finesse/(c/2/L);
Sen_2 = 1/m2Hz;
Filt_GR = zpk([-2*pi*2.5e3],[0],0.1);

% RF
Filt_GR1 = zpk([0],[-10*2*pi],1);
Act_AOM = -1e6;
AOM = 1;

% Low frequency
% G_GrM_servo = -2e+2;            % F_MASS servo: gain
% pGrM_servo_1 = 2*pi*   0;       % F_MASS servo: pole
% zGrM_servo_1 = 2*pi*  10;       % F_MASS servo: zero
% pGrM_servo_2 = 2*pi*   0;       % F_MASS servo: pole
% zGrM_servo_2 = 2*pi*  10;       % F_MASS servo: zero
% zGrM_servo_3 = 2*pi*  10;       % F_MASS servo: zero
% pGrM_servo_3 = 2*pi* 100;       % F_MASS servo: pole
% pGrM_servo_4 = 2*pi* 5e+2;      % F_MASS servo: pole
% pGrM_servo_5 = 2*pi* 5e+2;      % F_MASS servo: pole
% 
% Filt_mass = zpk([-zGrM_servo_1 -zGrM_servo_2 -zGrM_servo_3],[-pGrM_servo_1 -pGrM_servo_2 -pGrM_servo_3 -pGrM_servo_4 -pGrM_servo_5],G_GrM_servo*pGrM_servo_4*pGrM_servo_5);
Filt_mass = 1;
num = [(-5.0e-6)*2*m2Hz*2*pi*2*pi];
den = [1 2*pi/10 4*pi*pi];
Act_mass = tf(num,den);


%Cavity
Changing_unit1 = c/L/532e-9;
num1 = [0.01*pi*pi];
den1 = [1 0.1*pi 0.01*pi*pi];

num2 = [4*pi*pi];
den2 = [1 2*pi/10 4*pi*pi];

inverted_pend = tf(num1,den1);
Pendulum =  tf(num2,den2);

Gol_PDH = Cavity_Pole*Sen_1*Filt_GR*(Filt_GR1*Act_AOM*AOM-Filt_mass*Act_mass);

%% Plot expected TF from 'in2' to 'out1'

%%%% to create a bode plot with the freqeuncy axis in Hz %%%%
set(cstprefs.tbxprefs,'FrequencyUnits','Hz')

figure(100)
[A,B,C,D] = linmod('TEST_PLL_PDH');
tfs = ss(A,B,C,D);
aaaa=tfs(1,2);
bode(aaaa)
grid on

%% Define some parameters and get live parts parameters
freq = logspace(-17,4,100000);
liveModel = 'TEST_PLL_PDH';
dof = 'TST';    % name of DOF to plot NB
 startTime = 1078250000;   % start GPS time
 durationTime = 512;
 IFO = 'H1';
 site = 'LHO';

% Try setting different NDS server if you couldn't get data
% setenv('LIGONDSIP','h1nds1:8088');
% mdv_config;

% load cached outputs
loadFunctionCache()

% get live parts parameters
%liveParts(liveModel, startTime, durationTime, freq)

%% Compute noises and save cache
% Compute noises
[noises, sys] = nbFromSimulink(liveModel, freq, 'dof', dof);

% save cached outputs
saveFunctionCache();

%% Make a quick NB plot
disp('Plotting noises')
nb = nbGroupNoises(liveModel, noises, sys);

% Get noise data from DAQ. Put NdNoiseSource block with DAQ channel
% specified. Put something (e.g. 1) in ASD parameter of that block.
%nb = nbAcquireData(liveModel, sys, nb, startTime, durationTime);

nb.sortModel();
matlabNoisePlot(nb);
%figure(1)
%grid on;
%axis tight;
%ylabel('Mag (m/sqrt(Hz))');
%xlabel('Frequency (Hz)');
%ylim([1e-22,1e-10]);
%xlim([1,1e4]);

figure(2);
grid on;
axis tight;
ylabel('Mag (m/sqrt(Hz))');
xlabel('Frequency (Hz)');
title('Displacement noises');
xlim([1,1e4]);
ylim([1e-22,1e-10]);

%% plot expected curve from calculation

%[mag,ph]=bode((Gol)/(1+Gol),2*pi*freq);
%loglog(freq,squeeze(mag),'b.')
%hold on

%[mag,ph]=bode(1/Sen,2*pi*freq);
%loglog(freq,freq./freq/Sen,'r.')