% Frequency-Domain Advanced LIGO Interferometer Model
%  computes frequency dependent IFO response matrix
% m = freqDRFPM(p, f);
%
% [AS_Q; REFL_Q; POB_Q; AS_I; REFL_I; POB_I] = ...
%  m * [ETMx; ETMy; ITMx; ITMy; PR; SR; BS; dFreq]
%
% Giving a third argument 'DOF' will cause the response matrix to be
% in the canonical degree-of-freedom basis
% m = freqIFO(p, f, 'DOF');
% [AS_Q; REFL_Q; POB_Q; AS_I; REFL_I; POB_I] = ...
%  m * [DARM; CARM; MICH; PRCL; SRCL; dFreq]
% where DARM = Lx - Ly, CARM = Lx + Ly, MICH = lx - ly,
%  PRCL  = lx + ly, SRCL  = sx + sy
%
% % ==== Parameter Struct (and possible values)
% % == Optical Path Lengths between HR Surfaces
% p.len.IXEX = 3995;			% X arm length [m]
% p.len.IYEY = 3995;			% Y arm length [m]
% p.len.PRBS = 4.397;			% PR to BS optical path length [m]
% p.len.SRBS = 4.397;			% SR to BS optical path length [m]
% p.len.BSIX = 4.9765;			% BS to ITMX optical path length [m]
% p.len.BSIY = 4.5975;			% BS to ITMY optical path length [m]
%
% % == Optical Parameters
% p.mEX.T = 20e-6;			% HR power transmission
% p.mEX.L = 20e-6;			% HR power loss
% p.mEX.RB = 100e-6;			% AR power reflection
% p.mEX.LB = 100e-6;			% AR and bulk transmission loss
%
% p.mEY.T = 20e-6;
% p.mEY.L = 20e-6;
% p.mEY.RB = 100e-6;
% p.mEY.LB = 100e-6;

% p.mIX.T = 0.03;
% p.mIX.L = 100e-6;
% p.mIX.RB = 100e-6;
% p.mIX.LB = 100e-6;
%
% p.mIY.T = 0.03;
% p.mIY.L = 100e-6;
% p.mIY.RB = 100e-6;
% p.mIY.LB = 100e-6;
%
% p.mPR.T = 0.03;
% p.mPR.L = 100e-6;
% p.mPR.RB = 100e-6;
% p.mPR.LB = 100e-6;
%
% p.mSR.T = 0.1;
% p.mSR.L = 100e-6;
% p.mSR.RB = 100e-6;
% p.mSR.LB = 100e-6;
%
% p.mBS.T = 0.5;
% p.mBS.L = 0.0;
% p.mBS.RB = 0.0;
% p.mBS.LB = 0.0;
%
% % == Input Beam
% p.lambda = 1064;			% wave length [nm]
% p.ampMod = 0.44;			% input modulation depth
% p.freqMod = 24.482e6;			% input modulation frequency (Hz)
%
% % == Demod Phases
% p.phiAS   = 0;			% AS port demod phase   [degree]
% p.phiREFL = 0;			% REFL port demod phase [degree]
% p.phiPOB  = 0;			% POB port demod phase  [degree]
%
% This function can be called with up to 5 output arguments as
% [m, pm, mCR, mP1, mP2] = freqIFO(p, f);
% in which case
% m   - is the total response matrx, as before
% pm  - a modified parameter struct
% mCR - is the carrier signal matrix, in the COMP basis
% mP1 - is the first SB signal matrix, in the COMP basis
% mP2 - is the second SB signal matrix, in the COMP basis

% ================================================================
% Some relationships that may help determine arm
% cavity parameters from measured data:
%  rRT = rITM * rETM			% arm round-trip reflectivity
%  tau = -2 * lArm / (c * log(rRT))	% arm decay time
%  fCP = 1 / (2 * pi * tau)		% arm cavity pole frequency
%
% solving for rRT gives...
%  rRT = exp(-4 * pi * lArm * fCP / c)
% ================================================================

% ================================================================
% To Do?
% Include lower SBs (pM1 and pM2)
%  pM1 = freqProp(p, -p.freqMod);
%  pM2 = freqProp(p, -2 * p.freqMod);
% Include some aIn asymmetry between P1 and M1
%  This will make pP1 ~= conj(pM1), which will result in offsets
%  in output signals, which creates a hell of a mess.  (Mirrors
%  should be offset to zero the signal offsets, this involves
%  control loops...the mess grows.)
% Add higher-modes...at least the even ones
%  This too will introduce offsets, and a nasty mess with them.
% ================================================================

function [m, pm, mCR, mP1, mP2] = freqDRFPM(p, f, varargin)

  % ==== Optical Parameters
  % things with units
  pm.c = 299792458;			% speed of light [m/s]
  pm.k = 2 * pi / p.lambda;		% wave number [1/nm]

  pm.len = p.len;
  pm.oEX = optAmp(p.mEX);
  pm.oEY = optAmp(p.mEY);
  pm.oIX = optAmp(p.mIX);
  pm.oIY = optAmp(p.mIY);
  pm.oPR = optAmp(p.mPR);
  pm.oSR = optAmp(p.mSR);
  pm.oBS = optAmp(p.mBS);

  % ==== DC Fields
  pCR = freqProp(p, 0);
  pP1 = freqProp(p, p.freqMod);
  pP2 = freqProp(p, 2 * p.freqMod);

  % ==== Signals from Various Sources
  % add DC to f vector, for reference in demodulation
  ff = [f(:); 0];

  % compute signals
  % (assumes upper and lower audio SBs are equivalent)
  pf = freqProp(pm, ff);
  mCR = sigMatrix(pCR, pf);

  pf = freqProp(pm, ff + p.freqMod);
  mP1 = sigMatrix(pP1, pf);

  pf = freqProp(pm, ff + 2 * p.freqMod);
  mP2 = sigMatrix(pP2, pf);

  m = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% optAmp
% compute amplitude properties for an optic

function o = optAmp(m)

 LBt = (1 - m.RB) * (1 - m.LB);        % total loss on AR to HR transmission

 o.r = sqrt(1 - m.T - m.L);                  % HR reflectivity, front
 o.b = -o.r * LBt;                           % HR reflectivity, back
 o.t = sqrt(m.T * LBt);                      % optic transmissivity

 o.ap = sqrt(m.RB);                          % AR reflectivity, prompt
 o.ab = o.ap * o.r * sqrt((1 - m.LB) * LBT); % AR reflectivity, internal back
 o.af = -o.ap * sqrt((1 - m.LB) * m.T);      % AR reflectivity, internal front

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% freqProp
% compute frequency dependent IFO properties
%
% The return struct contains a 13 x 13 transfer matrix
% for field propagation from point to point in the IFO.
% The 13 field locations are
%  1) ETMx - from ITMx
%  2) ETMy - from ITMy
%  3) ITMx - from ETMx
%  4) ITMx - from BS
%  5) ITMy - from ETMy
%  6) ITMy - from BS
%  7) RM   - from BS
%  8) RM   - from REFL port
%  9) BS   - from ITMx
% 10) BS   - from ITMy
% 11) BS   - from RM
% 12) REFL - output
% 13) AS   - output
%
% A few of the elements are picked out specifically
% as "cavity gains".  They are
%  gX = X-Arm Cavity Gain
%  gY = Y-Arm Cavity Gain
%  gP = Power Recycling Cavity Gain
%
% ps = freqProp(pm, f)

function ps = freqProp(pm, f)

  % ==== Phase Computations
  % == One-Way Phases
  dphi_dl = -2 * pi * i * f / pm.c;

  ps.pArmX = exp(dphi_dl * pm.lArmX);
  ps.pArmY = exp(dphi_dl * pm.lArmY);
  ps.pRMbs = exp(dphi_dl * pm.lRMbs);
  ps.pBSx = exp(dphi_dl * pm.lBSx);
  ps.pBSy = exp(dphi_dl * pm.lBSy);

  Nf = length(f);
  mc = zeros(13, 13, Nf);

  % via ETMx and ETMy
  mc(3, 1, :) = ps.pArmX * pm.rEX;	% from ITMx to ITMx via ETMx
  mc(5, 2, :) = ps.pArmY * pm.rEY;	% from ITMy to ITMy via ETMy

  % via ITMx
  mc(1, 3, :) = ps.pArmX * pm.rIX;	% from ETMx to ETMx
  mc(9, 3, :) = ps.pBSx * pm.tIX;	% from ETMx to BS
  mc(1, 4, :) = ps.pArmX * pm.tIX;	% from BS to ETMx
  mc(9, 4, :) = ps.pBSx * pm.bIX;	% from BS to BS

  % via ITMy
  mc( 2, 5, :) = ps.pArmY * pm.rIY;	% from ETMy to ETMy front
  mc(10, 5, :) = ps.pBSy * pm.tIY;	% from ETMy to BS
  mc( 2, 6, :) = ps.pArmY * pm.tIY;	% from BS to ETMy front
  mc(10, 6, :) = ps.pBSy * pm.bIY;	% from BS to BS

  % via RM
  mc(11, 7, :) = pm.rRM * ps.pRMbs;	% from BS to BS
  mc(12, 7, :) = pm.tRM;			% from BS to REFL
  mc(11, 8, :) = pm.tRM * ps.pRMbs;	% from REFL to BS
  mc(12, 8, :) = pm.bRM;			% from REFL to REFL

  % via BS
  mc( 7,  9, :) = pm.tBS * ps.pRMbs;	% from ITMx to RM
  mc(13,  9, :) = pm.bBS;		% from ITMx to AS
  mc( 7, 10, :) = pm.rBS * ps.pRMbs;	% from ITMy to RM
  mc(13, 10, :) = pm.tBS;		% from ITMy to AS
  mc( 4, 11, :) = pm.tBS * ps.pBSx;	% from RM to ITMx
  mc( 6, 11, :) = pm.rBS * ps.pBSy;	% from RM to ITMy

  % ==== Invert Propagation Matrix
  m = invprop(mc);
  ps.mc = mc;
  ps.m = m;

  % cavity gains
  ps.gX = m(1, 1, :) ./ (m(4, 1, :) .* mc(1, 4) + 1);
  ps.gY = m(2, 2, :) ./ (m(6, 2, :) .* mc(2, 6) + 1);
  ps.gP = m(7, 7, :);

  % composite reflectivites
  ps.rX = m( 9, 8, :) ./ m( 4, 8, :);
  ps.rY = m(10, 8, :) ./ m( 6, 8, :);
  ps.rM = m( 7, 8, :) ./ m(11, 8, :);
  ps.tM = m(13, 8, :) ./ m(11, 8, :);
  ps.rIFO = m(12, 8, :) ./ m(8, 8, :);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fieldAmp
% compute field amplitudes
%
% eAS and ePOB are in the cavity mode
% eREFL(1) - prompt reflection in the input mode
% eREFL(2) - leakage field in the cavity mode
% eREFL(3) - reject field in the (input - cavity) mode

function ps = fieldAmp(p, ps, eIn)

  if( size(ps.m, 3) ~= 1 )
    error('fieldAmp only accepts single frequency field matrices.')
  end

  ps.eIFO = ps.m(:, 8) * ps.aIn * eIn;

  % AS and POB
  ps.eAS = ps.eIFO(13);
  ps.ePOB = ps.eIFO(11);

  % REFL
  ps.eREFL = zeros(3, 1);
  ps.eREFL(1) = ps.mc(12, 8) * eIn;		% prompt reflection at REFL
  ps.eREFL(2) = ps.mc(12, 7) * ps.eIFO(7);	% leakage field to REFL
  ps.eREFL(3) = -(p.tRM^2 / (1 + p.rRM)) * ...	% anti-resonant
                 sqrt(1 - ps.aIn^2) * eIn;	% non-modematched


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% sigMatrix
% compute signal matrix for some frequency vector
%
% ps is the source field struct (from freqProp, with a single frequency)
% pf is the source field struct (from freqProp, with a frequency vector)
%
% Each matrix element is computed with a sum of terms of the form:
% m  = (propagation throughout IFO) .* (reflection to next field point) *
%      (source field at signal gerating reflector);
%
% This equation should be read from right to left (i.e., source, then
% reflection, then propagation).  Reflection from the "back" of an
% optic is given a negative sign in the summation.
%
% the return matrix is 3 dimentional with
% N frequencies
% 3 ports [AS; REFL; POB]
% 7 sources [ETMx; ETMy; ITMx; ITMy; RM; BS; dFreq]

function ms = sigMatrix(ps, pf)

  % init
  eIFO = ps.eIFO;				% source field vector
  mr = permute(pf.mc, [3 1 2]);			% reflection matrix
  mp = permute(pf.m, [3 1 2]);			% propagation matrix

  np = 13:-1:11;				% port indices, [AS; REFL; POB]
  ms = zeros(size(mp, 1), length(np), 7);	% signal matrix
  
  % compute signal matrix
  for n = 1:length(np)
    p = np(n);
    ms(:, n, 1) = mp(:, p, 3) .* mr(:, 3, 1) * eIFO(1);		% ETMx front
    ms(:, n, 2) = mp(:, p, 5) .* mr(:, 5, 2) * eIFO(2);		% ETMy front
    ms(:, n, 3) = mp(:, p, 1) .* mr(:, 1, 3) * eIFO(3) - ...	% ITMx front
                  mp(:, p, 9) .* mr(:, 9, 4) * eIFO(4);		% ITMx back
    ms(:, n, 4) = mp(:, p, 2) .* mr(:, 2, 5) * eIFO(5) - ...	% ITMy front
                  mp(:, p,10) .* mr(:,10, 6) * eIFO(6);		% ITMy back
    ms(:, n, 5) = mp(:, p,11) .* mr(:,11, 7) * eIFO(7) - ...	% RM front
                  mp(:, p,12) .* mr(:,12, 8) * eIFO(8);		% RM back
    ms(:, n, 6) =-mp(:, p,13) .* mr(:,13, 9) * eIFO(9) + ...	% BS from ITMx
                  mp(:, p, 7) .* mr(:, 7,10) * eIFO(10) + ...	% BS from ITMy
                  mp(:, p, 6) .* mr(:, 6,11) * eIFO(11);	% BS from RM
    ms(:, n, 7) = mp(:, p, 8) * eIFO(8);			% dFreq
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% demod
% demodulation signal between two fields
%
% aSig is the singal carrying field
%  aSig(end) is used as the RF phase reference
%  and must be generated with an audio frequency
%  of 0 (i.e., f(end) = 0 in call to freqProp).
%
% aLO is the local-oscillator
%
% df is sign of the demodulation
%  df = (fLO - fSig) / fDemod, where fLO
%  is the frequency of the local oscillator
%  field, fSig is the frequency of the signal
%  carrier (this does not include the audio
%  signal frequency), and fDemod is the
%  demodulation frequency.  If df is not -1 or
%  1, the demodulation results are zero.
%
% phiDemod is the demod phase in radians
%
% [aI, aQ] = demod(aSig, aLO, df, phi)

function [aI, aQ] = demod(aSig, aLO, df, phiDemod)

  % assuming f(end) == 0, phase of aSig(end) is RF phase,
  % remove this phase from aSig to leave audio signal
  aRF = exp(i * angle(aSig(end)));
  aAF = aSig(1:end-1) / aRF;

  % conjugate upper RF freq
  if( df == 1 )
    aLO = conj(aLO);
  elseif( df == -1 )
    aRF = conj(aRF);
    aAF = -aAF;
  else
    error('Third argument, df, must be -1 or 1.');
  end

  % make I and Q RF phases, then replace AF phase
  a = aLO * aRF * exp(i * phiDemod);
  aI = -real(a) * aAF;
  aQ =  imag(a) * aAF;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% inputTrans
% transform m into input basis mDOF

function mr = inputTrans(m, mDOF);

  if( ndims(mDOF) == 2 )
    mr = zeros(size(m, 1), size(m, 2), size(mDOF, 2));
    for n = 1:size(m, 1)
      mr(n, :, :) = shiftdim(shiftdim(m(n, :, :), 1) * mDOF, -1);
    end
  else
    mr = zeros(size(m, 1), size(m, 2), size(mDOF, 3));
    for n = 1:size(m, 1)
      mr(n, :, :) = shiftdim(shiftdim(m(n, :, :), 1) * ...
                          shiftdim(mDOF(n, :, :), 1), -1);
    end
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% invprop
% invert optical propagation matrix

function m = invprop(m)

  % consolidate warnings, if any
  warnState = warning;
  warning off
  lastwarn('') 

  % invert
  eyeN = eye(size(m, 2));
  for nf = 1:size(m, 3);
    m(:, :, nf) = inv(eyeN - m(:, :, nf));
  end

  % deal with warnings
  if( ~isempty(lastwarn) )
    warning('IFO propagation nearly singular.')
  end
  warning(warnState)
