%% Check Third Harmonics Demodulation

%% Path setup

baseDir = pwd;
addpath(genpath([baseDir,'/Optickle']));
addpath(genpath([baseDir,'/tools']));

resultDir = [pwd,'/results/LockAcquisition/'];

%% Units constants
nm=1e-9;
pm=1e-12;
MHz=1e6;
ppm=1e-6;

%% Read parameters
p=paramLCGT();

%% Construct an Optickle model
[opt,n,l,pr,dr] = modelLCGT(p);

% == Extract some useful model information ==
drvNames=getDriveNames(opt);
Ndrive=opt.Ndrive;
Nlink=opt.Nlink;
lambda=opt.lambda;
c=opt.c;

%% Optimize the demodulation phase
determineDemodPhase(0, 0, p);

%% Load the optimized values
demodPhaseLCGT;

%% Construct the Optickle model again
[opt,n,l,pr,dr] = modelLCGT(p);

% == Extract some useful model information ==
drvNames=getDriveNames(opt);
Ndrive=opt.Ndrive;
Nlink=opt.Nlink;
lambda=opt.lambda;
c=opt.c;

%% ==== PRM Sweep =====
Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);
pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'PRM'),:) = dl;
[fDC, sigDC]=sweep(opt,pos);

%
h1=figure(1);
plot(dl, squeeze(sigDC(pr.REFL_2ThI,:)),dl, squeeze(sigDC(pr.REFL_1ThI,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('PRC Sweep','FontSize',16);
    grid on
    legend('REFL_2ThI','REFL_1ThI');

%% ==== MICH Sweep =====
Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);
pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'ETMX'),:) = dl;
pos(getDriveNumbers(opt,'ETMY'),:) = -dl;
pos(getDriveNumbers(opt,'ITMX'),:) = -dl;
pos(getDriveNumbers(opt,'ITMY'),:) = dl;
[fDC, sigDC]=sweep(opt,pos);

%
h1=figure(1);
plot(dl, squeeze(sigDC(pr.REFL_1ThQ,:)),dl, squeeze(sigDC(pr.REFL_1ThI,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('MICH Sweep','FontSize',16);
    grid on
    legend('REFL_1ThQ','REFL_1ThI');

%% ==== SRM Sweep =====
Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);
pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'SRM'),:) = dl;
[fDC, sigDC]=sweep(opt,pos);

%
h1=figure(1);
plot(dl, squeeze(sigDC(pr.REFL_1ThI,:)),dl, squeeze(sigDC(pr.REFL_1ThQ,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('SRC Sweep','FontSize',16);
    grid on
    legend('REFL_1ThI','REFL_1ThQ');

%% Tickle
[fDC, sigDC, sigAC, mMech, noiseAC, noiseMech]=tickle(opt,[],100);

%% Sensing Matrix
probeNames = {'REFL_1ThI','REFL_1ThQ','REFL_2ThI','POP_1ThI'};
cPrbNames = probeNames;
mPrb = eye(length(probeNames));
driveNames = {'ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM'};
cDrvNames = {'DARM','CARM','MICH','PRC','SRC'};
mDrv = [1,-1,0,0,0,0,0; %DARM
        1,1,0,0,0,0,0;  %CARM
        1,-1,-1,1,0,0,0;%MICH
        0,0,0,0,0,1,0;  %PRC
        0,0,0,0,0,0,1]; %SRC

%%
h1=figure(1);
%plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      %mDrv, cDrvNames, noiseAC);
           
plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);                     
h2=figure(2);    
plotSensingMatrixWithSign(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);

%% ==== SRC Arm detune =====


h1=figure(1);
hold on;
NarmStep = 10;
dlarm = linspace(2*nm,0,NarmStep);

Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);

M =[dl(:)];

for ii = 1:NarmStep

pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'ETMX'),:) = dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'ETMY'),:) = dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'SRM'),:) = dl;
[fDC, sigDC]=sweep(opt,pos);

%
errorSig = squeeze(sigDC(pr.REFL_1ThI,:));
plot(dl, errorSig, 'Color', hsv2rgb(ii/NarmStep,1,1));
    
M = [M,errorSig(:)];

end

xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('SRC Sweep','FontSize',16);
    grid on

fid = fopen([resultDir, 'THD_SRC_CARM_Sweep.dat'],'w');
fprintf(fid,'#');
fclose(fid);

dlmwrite([resultDir, 'THD_SRC_CARM_Sweep.dat'], dlarm, '-append');

dlmwrite([resultDir, 'THD_SRC_CARM_Sweep.dat'],M, '-append');

%% ==== PRC Arm detune =====


h1=figure(1);
hold on;
NarmStep = 10;
dlarm = linspace(2*nm,0,NarmStep);

Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);

M =[dl(:)];

for ii = 1:NarmStep
    
pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'ETMX'),:) = dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'ETMY'),:) = dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'PRM'),:) = dl;


[fDC, sigDC]=sweep(opt,pos);

%
errorSig = squeeze(sigDC(pr.REFL_2ThI,:));
plot(dl, errorSig,'Color', hsv2rgb(ii/NarmStep,1,1));
M = [M,errorSig(:)];

end

xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('PRC Sweep','FontSize',16);
    grid on

fid = fopen([resultDir, 'THD_PRC_CARM_Sweep.dat'],'w');
fprintf(fid,'#');
fclose(fid);

dlmwrite([resultDir, 'THD_PRC_CARM_Sweep.dat'], dlarm, '-append');

dlmwrite([resultDir, 'THD_PRC_CARM_Sweep.dat'],M, '-append');

%% ==== MICH Arm detune =====


h1=figure(1);
hold on;
NarmStep = 15;
dlarm = linspace(2*nm,0,NarmStep);

Nstep = 100;
range=20*nm;
dl=linspace(-range,range,Nstep);

M =[dl(:)];
for ii = 1:NarmStep
    
pos=zeros(Ndrive, Nstep);
pos(getDriveNumbers(opt,'ETMX'),:) = dl+dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'ETMY'),:) = -dl+dlarm(ii)*ones(1,Nstep);
pos(getDriveNumbers(opt,'ITMX'),:) = -dl;
pos(getDriveNumbers(opt,'ITMY'),:) = dl;
[fDC, sigDC]=sweep(opt,pos);

%

errorSig = squeeze(sigDC(pr.REFL_1ThQ,:));
plot(dl, errorSig,'Color', hsv2rgb(ii/NarmStep,1,1));
M = [M,errorSig(:)];

end
xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('MICH Sweep','FontSize',16);
    grid on
    
    
fid = fopen([resultDir, 'THD_MICH_CARM_Sweep.dat'],'w');
fprintf(fid,'#');
fclose(fid);

dlmwrite([resultDir, 'THD_MICH_CARM_Sweep.dat'], dlarm, '-append');

dlmwrite([resultDir, 'THD_MICH_CARM_Sweep.dat'],M, '-append');