% computeAtsMotion.m
% 
% Vivek Nadkarni
% Trimble
% November 16, 2004

function [atsTime, atsMotion] = computeAtsMotion(enablePlots);

loadCfg; 

if ~exist('enablePlots')
  enablePlots = 0; 
end

% ATS data
atsTraj = load([directoryname atsFileName]);
atsT = atsTraj(:,1);
atsX = atsTraj(:,2);
atsY = atsTraj(:,3);
atsZ = atsTraj(:,4);

atsDiffX = diff(atsX);
atsDiffY = diff(atsY);
atsDiffZ = diff(atsZ);
atsDiffT = atsT(2:end);

atsSpeed = sqrt(atsDiffX.^2 + atsDiffY.^2 + atsDiffZ.^2);
atsMotion = atsSpeed;
motionIdx = find(atsMotion >= 0.005);
atsMotion(motionIdx) = 0.05;

% motionIdx = find(atsMotion < 0.005);
% atsMotion(motionIdx) = 0;


% % Motion superimposed on phase.
% plotDdResids('phase', 0.5, 11);
% plot(atsDiffT-startTime, atsMotion, 'k');

if (enablePlots)
  newfigure;
  plot(atsDiffT, atsMotion);
  grid minor;
  title('ATS Motion');

  newfigure;
  plot(atsDiffT, atsSpeed);
  grid minor;
  title('ATS Speed');
end

atsTime = atsDiffT;

return; 
