% Define model
opt = adVirgo1

% Mirror indexes
nEMX = getDriveIndex(opt, 'EMX');
nEMY = getDriveIndex(opt, 'EMY');
nPRM = getDriveIndex(opt, 'PRM');
nSRM = getDriveIndex(opt, 'SRM');
nBS  = getDriveIndex(opt, 'BS');

% Probe indexes
nXP_DC   = getProbeNum(opt, 'XP_DC');
nYP_DC   = getProbeNum(opt, 'YP_DC');
nASY_DC  = getProbeNum(opt, 'ASY_DC');
nSYM_DC  = getProbeNum(opt, 'SYM_DC');
nPOBS_DC = getProbeNum(opt, 'POBS_DC');

%%%%%%%%%%%%%%%%%%%%%%%%% DARM sweep (set PRM and SRM transmission to 1)
pos_start = zeros(opt.Ndrive, 1);
pos_end = zeros(opt.Ndrive, 1);

pos_start(nEMX) = -0.3e-6;
pos_end(nEMX)   =  0.3e-6;

[pos, sigDC, fDC] = sweepLinear(opt, pos_start, pos_end, 5000);

nTRA = getFieldOut(opt, 'EMX', 'bk');
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 4, :))).^2, 'k-')
hold on
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 3, :))).^2, 'r-')
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 1, :))).^2, 'g-')
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 2, :))).^2, 'b-')
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 5, :))).^2, 'r--')
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 7, :))).^2, 'g--')
semilogy(pos(nEMX,:), abs(squeeze(fDC(nTRA, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Arm cavity length detuning [m]');
ylabel('Transmitted power [W]');

%%%%%%%%%%%%%%%%%%%%%%%%% PRCL sweep (set SRM transmission to 1)
pos_start = zeros(opt.Ndrive, 1);
pos_end = zeros(opt.Ndrive, 1);

pos_start(nPRM) = -0.3e-6;
pos_end(nPRM)   =  0.3e-6;

[pos, sigDC, fDC] = sweepLinear(opt, pos_start, pos_end, 1000);

nPOBS = getFieldOut(opt, 'BS', 'piB');
figure();
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 4, :))).^2, 'k-')
hold on
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 3, :))).^2, 'r-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 1, :))).^2, 'g-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 2, :))).^2, 'b-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 5, :))).^2, 'r--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 7, :))).^2, 'g--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOBS, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Power recycling cavity length detuning [m]');
ylabel('Beam Splitter pick-off [W]');

nPOY = getFieldOut(opt, 'IMY_AR', 'frA');
figure();
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 4, :))).^2, 'k-')
hold on
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 3, :))).^2, 'r-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 1, :))).^2, 'g-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 2, :))).^2, 'b-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 5, :))).^2, 'r--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 7, :))).^2, 'g--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOY, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Power recycling cavity length detuning [m]');
ylabel('IMY pick-off [W]');

nPOX = getFieldOut(opt, 'IMX_AR', 'frA');
figure();
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 4, :))).^2, 'k-')
hold on
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 3, :))).^2, 'r-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 1, :))).^2, 'g-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 2, :))).^2, 'b-')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 5, :))).^2, 'r--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 7, :))).^2, 'g--')
semilogy(pos(nPRM,:), abs(squeeze(fDC(nPOX, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Power recycling cavity length detuning [m]');
ylabel('IMX pick-off [W]');

%%%%%%%%%%%%%%%%%%%%%%%%% MICH sweep (set PRM and SRM transmission to 1)
pos_start = zeros(opt.Ndrive, 1);
pos_end = zeros(opt.Ndrive, 1);

pos_start(nBS) = -0.03e-6;
pos_end(nBS)   =  0.03e-6;

[pos, sigDC, fDC] = sweepLinear(opt, pos_start, pos_end, 1000);

nASY = getFieldOut(opt, 'BS', 'bkB');
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 4, :))).^2, 'k-')
hold on
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 3, :))).^2, 'r-')
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 1, :))).^2, 'g-')
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 2, :))).^2, 'b-')
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 5, :))).^2, 'r--')
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 7, :))).^2, 'g--')
semilogy(pos(nBS,:), abs(squeeze(fDC(nASY, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Beam splitter detuning [m]');
ylabel('Anti-symmetric port power [W]');

%%%%%%%%%%%%%%%%%%%%%%%%% SRC sweep 
pos_start = zeros(opt.Ndrive, 1);
pos_end = zeros(opt.Ndrive, 1);

pos_start(nSRM) =  0.5*5.3153e-07 - 3e-7;
pos_end(nSRM)   =  0.5*5.3153e-07 + 3e-7;

[pos, sigDC, fDC] = sweepLinear(opt, pos_start, pos_end, 1000);

nASY = getFieldOut(opt, 'SRM', 'bk');
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 4, :))).^2, 'k-')
hold on
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 3, :))).^2, 'r-')
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 1, :))).^2, 'g-')
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 2, :))).^2, 'b-')
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 5, :))).^2, 'r--')
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 7, :))).^2, 'g--')
semilogy(pos(nSRM,:) - mean(pos(nSRM,:)), abs(squeeze(fDC(nASY, 6, :))).^2, 'b--')
legend('CAR', 'SB1', 'SB2', 'SB3')
grid on
xlabel('Signal recycling cavity detuning [m]');
ylabel('Anti-symmetric port power [W]');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Compute TF around operating point
f = logspace(-2,4,1000);

% set RSE operating point
pos = zeros(opt.Ndrive, 1);
pos(nSRM) = 2.6576e-07 + 0.07/pi * 1e-6;

[fDC, sigDC, sigAC, mMech] = tickle(opt, pos, f);

nXP_SB1_P   = getProbeNum(opt, 'XP_SB1_P');
nXP_SB1_Q   = getProbeNum(opt, 'XP_SB1_Q');
nXP_SB2_P   = getProbeNum(opt, 'XP_SB2_P');
nXP_SB2_Q   = getProbeNum(opt, 'XP_SB2_Q');
nXP_SB3_P   = getProbeNum(opt, 'XP_SB3_P');
nXP_SB3_Q   = getProbeNum(opt, 'XP_SB3_Q');

nYP_SB1_P   = getProbeNum(opt, 'YP_SB1_P');
nYP_SB1_Q   = getProbeNum(opt, 'YP_SB1_Q');
nYP_SB2_P   = getProbeNum(opt, 'YP_SB2_P');
nYP_SB2_Q   = getProbeNum(opt, 'YP_SB2_Q');
nYP_SB3_P   = getProbeNum(opt, 'YP_SB3_P');
nYP_SB3_Q   = getProbeNum(opt, 'YP_SB3_Q');

nSYM_SB1_P   = getProbeNum(opt, 'SYM_SB1_P');
nSYM_SB1_Q   = getProbeNum(opt, 'SYM_SB1_Q');
nSYM_SB2_P   = getProbeNum(opt, 'SYM_SB2_P');
nSYM_SB2_Q   = getProbeNum(opt, 'SYM_SB2_Q');
nSYM_SB3_P   = getProbeNum(opt, 'SYM_SB3_P');
nSYM_SB3_Q   = getProbeNum(opt, 'SYM_SB3_Q');

nASY_SB1_P   = getProbeNum(opt, 'ASY_SB1_P');
nASY_SB1_Q   = getProbeNum(opt, 'ASY_SB1_Q');
nASY_SB2_P   = getProbeNum(opt, 'ASY_SB2_P');
nASY_SB2_Q   = getProbeNum(opt, 'ASY_SB2_Q');
nASY_SB3_P   = getProbeNum(opt, 'ASY_SB3_P');
nASY_SB3_Q   = getProbeNum(opt, 'ASY_SB3_Q');

nPOBS_SB1_P   = getProbeNum(opt, 'POBS_SB1_P');
nPOBS_SB1_Q   = getProbeNum(opt, 'POBS_SB1_Q');
nPOBS_SB2_P   = getProbeNum(opt, 'POBS_SB2_P');
nPOBS_SB2_Q   = getProbeNum(opt, 'POBS_SB2_Q');
nPOBS_SB3_P   = getProbeNum(opt, 'POBS_SB3_P');
nPOBS_SB3_Q   = getProbeNum(opt, 'POBS_SB3_Q');


loglog(f, abs(getTF(sigAC, nASY_SB1_P, nEMX) - getTF(sigAC, nASY_SB1_P, nEMY) ))
