function runFP()
% Example code for simple Fabry-Perot cavity
% Initiated by: Yuta Michimura

%% Clear everything
close all
clear all

%% Define some parameters
p.modelName = 'exampleFP'; % name of this model
p.Pin = 1;           % input power [W]
p.fmod1 = 15e6;      % modulation frequency [Hz]
p.g1 = 0.1i;         % modulation depth (imaginary number for phase modulation)
p.ITMChr = 1/1900;   % 1/RoC radius of curvature for ITM [1/m]
p.ITMThr = 0.05;     % power transmisson of ITM
p.ETMChr = 1/1900;   % 1/RoC radius of curvature for ETM [1/m]
p.ETMThr = 0.001;    % power transmisson of ETM
p.Larm = 3e3;        % cavity length [m]

%% Construct optickle model and set some parameters, lists
opt = Optickle([-p.fmod1,0,p.fmod1]);

% Add laser source
%  [opt, sn] = addSource(opt, name, vArf, z0, z)
%  vArf - amplitudes of each RF component (Nrf x 1)
%  z0 - beam range = (waist size)^2 * pi / lambda
%  z - distance to waist (negative if beam is converging)
opt.addSource('Laser', [0,p.Pin,0]);

% Add RFmodulator (EOM)
%  [opt, sn] = addRFmodulator(opt, name, fMod, aMod)
%  name - name of this optic
%  fMod - modulation frequency
%  aMod - modulation index (imaginary for phase, real for amplitude)
opt.addRFmodulator('Mod1', p.fmod1, p.g1);

% Add mirrors
%  [opt, sn] = addMirror(opt, name, aio, Chr, Thr, Lhr, Rar, Lmd, Nmd)
%  aio - angle of incidence (in degrees)
%  Chr - curvature of HR surface (Chr = 1 / radius of curvature)
%  Thr - power transmission of HR suface
%  Lhr - power loss on reflection from HR surface
%  Rar - power reflection of AR surface
%  Nmd - refractive index of medium (1.45 for fused silica, SiO2)
%  Lmd - power loss in medium (one pass)
opt.addMirror('ITM',0, p.ITMChr, p.ITMThr, 0, 0, 0, 1.754);
opt.addMirror('ETM',0, p.ETMChr, p.ETMThr, 0, 0, 0, 1.754);

% Add links
opt.addLink('Laser', 'out', 'Mod1', 'in',0);
opt.addLink('Mod1', 'out', 'ITM', 'bk',0);
opt.addLink('ITM', 'fr', 'ETM', 'fr',p.Larm);
opt.addLink('ETM', 'fr', 'ITM', 'fr',p.Larm);

% Add REFL probes
opt.addSink('REFL',0);
opt.addLink('ITM', 'bk', 'REFL','in',0);
%  opt = addReadout(opt, name, fphi, names)
%  name - sink name (also used as base name for probes in the readout)
%  fphi - demod frequency and phase (Nx2)
%  names - optional cell array of probe suffixes (instead of 1:N)
opt.addReadout('REFL', [p.fmod1,0]);

% Add TRANS probes
opt.addSink('TRANS',0);
opt.addLink('ETM', 'bk', 'TRANS','in',0);
%  [opt, snProbe] = addProbeIn(opt, name, snOpt, nameIn, freq, phase);
%  name - probe name
%  snOpt - the serial number or name of the source optic
%  nameIn - the number or name of the input port
%  freq - demodulation/RF frequency
%  phase - demodulation/RF phase offset (degrees)
opt.addProbeIn('TRANS_DC', 'TRANS', 'in', 0,0);

% Add REFL ASC probes
opt.addMirror('REFLSplit', 45, 0, 0.5, 0, 0, 0, 1.45);
opt.addSink('REFLA',0);
opt.addSink('REFLB',0);
opt.addGouyPhase('GouyREFLA', 0);
opt.addGouyPhase('GouyREFLB', pi/2);
opt.addLink('REFL', 'out', 'REFLSplit', 'fr',0);
opt.addLink('REFLSplit', 'fr', 'GouyREFLA', 'in',0);
opt.addLink('GouyREFLA', 'out','REFLA', 'in', 0);
opt.addLink('REFLSplit', 'bk', 'GouyREFLB', 'in',0);
opt.addLink('GouyREFLB', 'out','REFLB', 'in', 0);
opt.addReadout('REFLA', [p.fmod1,0]);
opt.addReadout('REFLB', [p.fmod1,0]);

% tell Optickle to use this cavity basis
opt.setCavityBasis('ITM', 'ETM');

% some parameters
lambda = opt.lambda(1);

% DOF definitions
probeNames={'REFL_I', 'REFL_Q','TRANS_DC'};  % probes of interest
probeNamesASC={'REFLA_I', 'REFLA_Q','REFLB_I', 'REFLB_Q'};  % probes of interest for ASC
driveNames = {'ITM','ETM'};  % drives of interest


%% Draw a graph for optickle model
opt2dot(opt, [p.modelName,'.pdf']); % GraphViz needs to be installed
    
%% 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 ETM in length (sweep function)
% define which mirror to sweep by how much
sweepDOF = 'ETM';
range = lambda/2;
pos = zeros(opt.Ndrive, 1001);
x = linspace(range,-range,1001);
pos(opt.getDriveNum(sweepDOF),:) = x;

% do the sweep
[fDC, sigDC] = opt.sweep(pos);

% plot
figure()
for ii=1:length(probeNames)
    plot(x,sigDC(opt.getProbeNum(probeNames(ii)),:));
    hold on
end
legend(probeNames,'interpreter', 'none')
xlabel([sweepDOF,' displacement [m]']);
ylabel('signal [W]');
xlim([-range,range])
grid on

saveas(gcf,[p.modelName,sweepDOF,'sweep.png'])

%% Length response (tickle function)
freq = logspace(-2,8,1000);
[fDC, sigDC, sigAC] = opt.tickle([], freq);

figure();
resp = sigAC(opt.getProbeNum('REFL_I'), opt.getDriveNum('ETM'),:);
resp = squeeze(resp);
subplot(2,1,1);
loglog(freq,abs(resp));
ylabel('Abs [W/m]');
title('ETM tickle, REFL_I response','interpreter', 'none');
grid on;
subplot(2,1,2);
semilogx(freq,angle(resp)*180/pi');
xlabel('Frequency [Hz]');
ylabel('Phase [deg]');
grid on;
ylim([-180,180])
set(gca,'YTick',linspace(-180,180,7));

%% Length sensing matrix (tickle function)
[fDC, sigDC, sigAC] = opt.tickle([], 10);
iDrv = cellfun(@opt.getDriveNum,driveNames);
iPrb = cellfun(@opt.getProbeNum,probeNames);
mSens = sigAC(iPrb,iDrv)

% use KAGRA tools to show sensing matrix better
fprintf('Length Sensing Matrix [W/m]:\n');
showSensingMatrix(opt,sigAC,probeNames,driveNames);

%% Optical spring (setMechTF, tickle function)
% define displacement mechanical transfer functions for ITM and ETM
ITMmass = 1;   % mass of ITM [kg]
ETMmass = 0.001;   % mass of ETM [kg]
ITMfres = 1;   % resonant frequency of ITM [Hz]
ETMfres = 1;   % resonant frequency of ETM [Hz]
mechTFposITM = zpk([],2*pi*ITMfres*[-0.1+1i, -0.1-1i],1/ITMmass);    % mechanical transfer function for ITM [m/N]
mechTFposETM = zpk([],2*pi*ITMfres*[-0.1+1i, -0.1-1i],1/ETMmass);    % mechanical transfer function for ETM [m/N]

% set mechanical transfer functions in length
opt = setMechTF(opt, 'ITM', mechTFposITM);
opt = setMechTF(opt, 'ETM', mechTFposETM);

freq = logspace(-2,3,1000);
[fDC, sigDC, sigAC, mMech] = opt.tickle([0,0,0,-lambda/64,0],freq);   % detuned
mMechETM = mMech(opt.getDriveNum('ETM'),opt.getDriveNum('ETM'),:);
mMechETM = abs(squeeze(mMechETM));  % optomechanical modification

% plot mechanical/optomechanical transfer functions (change detuning and ETM/ITM mass ratio, and see how optomechanical TF change)
[mag,phase] = bode(mechTFposETM,2*pi*freq);
mechETM = squeeze(mag);  % original mechanical transfer function
figure()
loglog(freq,mechETM)
hold on
loglog(freq,mechETM.*mMechETM)
legend('ETM pos mechanical','ETM pos optomechanical')
grid on
xlabel('Frequency [Hz]');
ylabel('Gain [m/N]');


%% ========== Alignment Sensing and Control ==========
%% Alignment sensing matrix (tickle01/tickle10 function)
% set Gouy phase
gouy = 0;
% gouy = 1.0659;  % optimal
opt.setGouyPhase('GouyREFLA', gouy);
opt.setGouyPhase('GouyREFLB', gouy+pi/2);

[sigAC] = opt.tickle01([], 10);

% use KAGRA tools to show sensing matrix better
fprintf('Alignment Sensing Matrix [W/rad]:\n');
showSensingMatrix(opt,sigAC,probeNamesASC,driveNames);

% show in hard/soft mode basis (change the Gouy phase, and see how it diagonalizes the sensing matrix)
dofNames = {'HARD','SOFT'};   % DOF of interest
gITM=1-p.Larm*p.ITMChr;
gETM=1-p.Larm*p.ETMChr;
r=2/((gITM-gETM)+sqrt((gITM-gETM)^2+4));
mDrv = [r,-1; % HARD  Conversion matrix for optic motion to DOF motion
        1,r]; % SOFT
fprintf('Alignment Sensing Matrix [W/rad]:\n');
showSensingMatrix(opt,sigAC,probeNamesASC,driveNames,dofNames,mDrv);

%% Angular optical spring (setMechTF, tickle01/tickle10 function)
% define amgular mechanical transfer functions for ITM and ETM
ITMinertia = 1;   % moment of inertia of ITM [kg*m^2]
ETMinertia = 0.00002;   % moment of inertia of ETM [kg*m^2]
ITMfres = 1;   % resonant frequency of ITM [Hz]
ETMfres = 1;   % resonant frequency of ETM [Hz]
mechTFyawITM = zpk([],2*pi*ITMfres*[-0.1+1i, -0.1-1i],1/ITMinertia);    % mechanical transfer function for ITM [rad/(N*m)]
mechTFyawETM = zpk([],2*pi*ITMfres*[-0.1+1i, -0.1-1i],1/ETMinertia);    % mechanical transfer function for ETM [rad/(N*m)]

% set mechanical transfer functions in length
opt = setMechTF(opt, 'ITM', mechTFyawITM,3);
opt = setMechTF(opt, 'ETM', mechTFyawETM,3);

freq = logspace(-2,3,1000);
[sigAC, mMech] = opt.tickle10([],freq);   % tickle01 for pitch, tickle10 for yaw
mMechETM = mMech(opt.getDriveNum('ETM'),opt.getDriveNum('ETM'),:);
mMechETM = abs(squeeze(mMechETM));  % optomechanical modification

% plot mechanical/optomechanical transfer functions (change RoC and ETM/ITM inertia ratio, and see how optomechanical TF change)
[mag,phase] = bode(mechTFyawETM,2*pi*freq);
mechETM = squeeze(mag);  % original mechanical transfer function
figure()
loglog(freq,mechETM)
hold on
loglog(freq,mechETM.*mMechETM)
legend('ETM yaw mechanical','ETM yaw optomechanical')
grid on
xlabel('Frequency [Hz]');
ylabel('Gain [rad/(N*m)]');