% lock a DRFPM
%
% [t, z, md, mp] = lockDRFPM(z0, nMax)
%
% Example:
% z0 = rand(1, 8); z0(5:6) = 0
% [t, z, md, mp] = lockDRFPM(z0 * 5e-7, 2000);

function [t, z, md, mp] = lockDRFPM(z0, nMax)

  opt = optDRFPM;
  
  nIn = 2;
  fMod = 24.482e6;
  gMod = 0.4;
  order = 4;

  % detector specification
  dSpec = [1, 2, 1,  0			% REFL
           2, 2, 1,  0			% AS
           1, 1, 1,  0			% POP
           2, 1, 1,  0			% POS
           5, 2, 1,  0.25		% POX
           6, 2, 1, -0.16		% POY
           7, 2, 1, -1.32		% TRX
           8, 2, 1, -1.73];		% TRY
  
  % initialize records
  z0 = reshape(z0, 1, 8);
  zNow = z0;
  [mdNow, mpNow] = dcDemod(opt, nIn, fMod, gMod, order, dSpec, zNow);

  md = zeros(nMax, size(mdNow, 2));
  mp = zeros(nMax, size(mpNow, 2));
  z  = zeros(nMax, size(zNow, 2));
  t  = (0:nMax-1) / 1000;

  z(1, :) = zNow;
  md(1, :) = mdNow;
  mp(1, :) = mpNow;

  % main loop
  for n = 2:nMax
    % feedback to ETMs with POX and POY
    zNow(7) = zNow(7) - 2e-9 * real(mdNow(5)) / mpNow(7);
    zNow(8) = zNow(8) - 2e-9 * real(mdNow(6)) / mpNow(8);

    % feed back BS with AS - REFL
    zNow(3) = zNow(3) - 6e-8 * (mpNow(1) - mpNow(2));
    zNow(4) = zNow(3);
    
    %zNow(1) = zNow(1) + 4e-9 * (mpNow(1) + mpNow(2) - 0.4);
    zNow(1) = zNow(1) - 4e-9 * (mpNow(5) + mpNow(6) - 0.4);
    zNow(2) = zNow(2) - 4e-9 * (mpNow(5) - mpNow(6));
    
    % compute new fields
    [mdNow, mpNow] = dcDemod(opt, nIn, fMod, gMod, order, dSpec, zNow);
    
    % accumulate results
    z(n, :) = zNow;
    md(n, :) = mdNow;
    mp(n, :) = mpNow;
  end

  % plot result
  subplot(2, 1, 1)
  plot(t, mp(:, [1 2 5 6 7 8]))
  legend('REF', 'ASY', 'POX', 'POY', 'TRX', 'TRY')
  grid on
  
  subplot(2, 1, 2)
  plot(t, z(:, [1 2 3 7 8]) * 1e9)
  legend('PR', 'SR', 'BS', 'EX', 'EY')
  xlabel('time (s)')
  ylabel('position (nm)')
  grid on
