%% Build the model
clear all
close all

baseDir = pwd;
baseDir = baseDir(1:47);

addpath(baseDir)
addpath(genpath([baseDir,'/Optickle-Optickle2']));
addpath(genpath([baseDir,'/tools']));
addpath(genpath([baseDir,'/parameters']));


%% --- Output ---

p = paramLCGT;
        
% no MZI
p.userMZ = 0;
% show it's PRFPMI
   
% input power
p.Pin = 10;

% Modulation index (klog 9505)
p.g1 = 1i*0.22;
p.g2 = 1i*0.23;

% REFL power equally split to PD1 (f1) and PD2 (f2)
p.REFLBS1_R = 0.5;
p.REFLBS2_R = 1;

% Derive demod phases
doplot = 0;
determineDemodPhase(doplot, baseDir, p);
demodPhaseKAGRA

%%%%%%%%%%%%%%%
%   if DRMI   %
%%%%%%%%%%%%%%%
% p.ETMXLhr=0;
% p.ETMXThr=1;
% p.ETMYLhr=0;
% p.ETMYThr=1;  
%%%%%%%%%%%%%%%

%% run Optickle

[opt,n,l,pr,dr] = modelLCGT(p);

% ====== output ====================
% opt: Optickle model
% n: a structure containing nodes.
% l: a structure containing links.
% pr: a structure containing probes.



%% Check power at each field, each probe
%  [fDC, sigDC] = opt.tickle([], []);
%  fprintf('DC fields (fDC matrix):\n');
%  showfDC(opt, fDC);
%  fprintf('\nProbes (sigDC matrix):\n');
%  showsigDC(opt, sigDC);


%% Sweep optic and check fields
% Usage
%    [pos, sigDC, fDC] = sweepLinear(opt, posStart, posEnd, Npos)

% ======= Sweep Parameters ========
% Range
range = p.lambda/2;
% sweep point number
N = 200;

% ====== Field numbers =======
nCarrier = find(opt.vFrf == 0, 1);
nf1_up = find(opt.vFrf == p.fmod1, 1);
nf1_lo = find(opt.vFrf == -p.fmod1, 1);
nf2_up = find(opt.vFrf == p.fmod2, 1);
nf2_lo = find(opt.vFrf == -p.fmod2, 1);
nDC = [nCarrier nf1_up nf1_lo nf2_up nf2_lo];



% %% ======== PRM Sweep ========
% pos = zeros(opt.Ndrive, 1);
% pos(dr.PRM) = range;
% 
% % ======== Run ===============
% 
% [pos, sigDC, fDC] = sweepLinear(opt, -pos, pos, N);
% 
% % ======== Plot ==============
% % PRM sweep
% field = squeeze(fDC(l.PRMtoPR2, nDC,:));
% figure(1)
% semilogy(pos(dr.PRM, :)/p.lambda*2*180, abs(field).^2)
% title('PRC Cavity Sweep')
% xlabel('Tuning [degree]')
% ylabel('Field Power [W]')
% legend('CR','+f1','-f1','+f2','-f2')
% grid on
% xlim([-180, 180])
% 
% % ==== % PRG
% E_prm = squeeze(fDC(l.PRMtoPR2, nDC,N/2));
% E_prin = squeeze(fDC(l.PRMbk, nDC,N/2));
% PRG = abs(E_prm.^2/E_prin.^2);
% 
% %% ======== XARM Sweep ========
% pos = zeros(opt.Ndrive, 1);
% pos(dr.ETMX) =  p.lambda/2;
% 
% % ======== Run ================
% [pos, sigDC, fDC] = sweepLinear(opt, -pos, pos, N);
% 
% % ======== Plot ===============
% % PRM sweep
% field = squeeze(fDC(l.ITMXtoETMX, nDC,:));
% figure(2)
% semilogy(pos(dr.ETMX, :)/p.lambda*2*180, abs(field).^2,'.')
% title('Cavity Sweep')
% xlabel('Tuning [degree]')
% ylabel('Field Power [W]')
% legend('CR','+f1','-f1','+f2','-f2')
% grid on
% %xlim([-180, 180])


%% Run Tickle

[fDC, sigDC, sigAC, mMech, noiseAC, noiseMech] = tickle(opt, [], p.ftickle);

%% ====================== Sensing Matrix ===================================
% opt: Optickle instance.
% sigAC: Output of tickle. tickle has to be called with only one frequency
%        point. [Nprb*Ndrv]
% probeNames: A cell array of probe names.
% mPrb: A matrix of size [length(probeNames)*length(cPrbNames)].
%       This matrix is used to convert from normal probes to compound
%       probes.
% cPrbNames: A cell array of compound probe names.
% driveNames: A cell array of drive names.
% mDrv: A matrix of size [length(driveNames)*length(cDrvNames)].
%       This matrix is used to convert from normal drives to compound
%       drives.
% cDrvNames: A cell array of compound drive names.
% noiseAC (optional): noise matrix ( Nprb x Naf )

probeNames = {'POP_1Q','POP_2I','REFL_2I','AS_DC'};
mPrb = eye(length(probeNames));
cPrbNames = probeNames;
driveNames = {'ETMX','ETMY','ITMX','ITMY','BS','PRM'};
mDrv       = [0,       0,    0,     0,     1,   -1/sqrt(2);  %MICH
              0,       0,    0,     0,     0,   1;           %PRC
              1,       1,    0,     0,     0,   0;           %CARM
              1,      -1,    0,     0,     0,   0];           %DARM
  
cDrvNames = {'MICH','PRCL','CARM','DARM'};

[mSens, mSensNorm, mOfs, mOfsMeter] = getSensingMatrix(opt,sigAC,sigDC, probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames, noiseAC);


% Squeezing OMC DC
mSens(4,:) = abs(mSens(4,:)).* sign(cos(angle((mSens(4,:)))));


%%
resultDir = pwd;
NoteFile = fopen([resultDir, '/sensmat.txt'],'w');

fprintf(NoteFile, '\n-------- Sensing Matrix -------\n\n');
fprintf(NoteFile, '====== Magnitude [W/m] ======\n');
fprintf(NoteFile, '         MICH,     PRCL,     CARM,     DARM\n');
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{1},...
    abs(mSens(1,1)),abs(mSens(1,2)),abs(mSens(1,3)),abs(mSens(1,4)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{2},...
    abs(mSens(2,1)),abs(mSens(2,2)),abs(mSens(2,3)),abs(mSens(2,4)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{3},...
    abs(mSens(3,1)),abs(mSens(3,2)),abs(mSens(3,3)),abs(mSens(3,4)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n\n', probeNames{4},...
    abs(mSens(4,1)),abs(mSens(4,2)),abs(mSens(4,3)),abs(mSens(4,4)));


fprintf(NoteFile, '====== Phase [deg]  ======\n');
fprintf(NoteFile, '         MICH,     PRCL,     CARM,     DARM\n');
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{1},...
    angle(mSens(1,1))*180/pi,angle(mSens(1,2))*180/pi,angle(mSens(1,3))*180/pi,angle(mSens(1,4))*180/pi);
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{2},...
    angle(mSens(2,1))*180/pi,angle(mSens(2,2))*180/pi,angle(mSens(2,3))*180/pi,angle(mSens(2,4))*180/pi);
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{3},...
    angle(mSens(3,1))*180/pi,angle(mSens(3,2))*180/pi,angle(mSens(3,3))*180/pi,angle(mSens(3,4))*180/pi);
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g\n', probeNames{4},...
    angle(mSens(4,1))*180/pi,angle(mSens(4,2))*180/pi,angle(mSens(4,3))*180/pi,angle(mSens(4,4))*180/pi);



fclose(NoteFile);

%plotSensingMatrixWithSign(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);


plot_radar_chart(mSens, probeNames, cDrvNames,0)