function makeTrajFile(methodStr, runRkwinTraj, ecefPos)
% Create a trajectory file. 
% Inputs: 
% 
% methodStr - method with which to generate the trajectory
%   'dynamic' - Generate an rkwsTraj.pos from the rtk engine (rkwin)
%               and use this as a reference trajectory.
%   'static'  - Take the mean position from the rkwsTraj.pos 
%               file as our static position
%   'survey'  - Use the externally supplied surveyed 
%               position ecefPos
%   'mpos'    - Generate a trajectory file in the correct format from 
%               the mPOS trajectory. 
%   'ats'     - Generate a trajectory file in the correct format from 
%               the ATS trajectory. 
% 
% runRkwinTraj - do we need to run rkwin to generate a trajectory file
%   0 - don't run rkwin. Generate an error if the file doesn't exist.
%   1 - run rkwin if necessary, and if the file doesn't exist. This is
%       the default. 
%   2 - force rkwin to run
%
% ecefPos - Surveyed position of the rover in ECEF 
%           coordinates ([X,Y,Z])
%
% Vivek Nadkarni
% Trimble
% August 5, 2004

%% Load configuration file
load gieTestSetupData;

%% Initialization
DYNAMIC = 1;
STATIC = 2;
SURVEY = 3;
MPOS = 4; 
ATS = 5; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Input Checking 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Zero or one input
if (nargin < 1)
  methodStr = 'dynamic';
end

if (strcmp(methodStr, 'survey'))
  method = SURVEY;
elseif (strcmp(methodStr, 'static'))
  method = STATIC;
elseif (strcmp(methodStr, 'mpos'))
  method = MPOS; 
elseif (strcmp(methodStr, 'ats'))
  method = ATS; 
else % (strcmp(methodStr, 'dynamic'))
  method = DYNAMIC; 
end

% Check second input
if (nargin < 2)
  % Default to generating the file if necessary. 
  runRkwinTraj = 1;
end

if ((method == DYNAMIC) || (method == STATIC))
  % If the file doesn't exist, generate it unless forbidden to.
  if (exist([directoryname 'rkwsTraj.pos'], 'file') == 0)
    if (runRkwinTraj == 0)
      % We are forbidden from running rkwin. Throw an error. 
      error('rkwsTraj.pos file does not exist.');
    elseif (runRkwinTraj == 1)
      % rkwsTraj.pos file does not exist. We have to generate it.
      runRkwinTraj = 2;
    end
    % If runRkwinTraj == 2, we are being forced to generate the 
    % file. This is fine, because the file doesn't exist.
  end
  % If runRkwinTraj == 2, we are being forced to generate
  % the file. So regenerate this file. 
else % (method == SURVEY)
  if (runRkwinTraj == 1)
    % We don't need to generate the file. 
    runRkwinTraj = 0;
  end
  % If runRkwinTraj == 2, we are being forced to generate
  % the file. So generate it even though we don't use it. 
end

% Check third input
if (nargin < 3)
  if (method == SURVEY)
    error('The method ''survey'' requires an external position.');
  end
elseif (nargin == 3)
  if (length(ecefPos) ~= 3)
    error('ECEF position should be a 3-vector [X,Y,Z]'); 
  end
elseif (nargin > 3)
  error('Too many inputs.');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% End of Input Checking 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% External Trajectory files. 
% First test for using external trajectory files. If we are using
% external trajectory files, then write them and return from the
% function. 

if (method == MPOS)
  makeMposTrajFile; 

  % Copy the mpos Trajectory to the truth Trajectory
  copyfile( ...
    [directoryname 'mposTraj.txt'], ... 
    [directoryname 'truthTraj.txt']);
  
  return;
end

if (method == ATS)
  makeAtsTrajFile; 
  
  % Copy the ats Trajectory to the truth Trajectory
  copyfile( ...
    [directoryname 'atsTraj.txt'], ... 
    [directoryname 'truthTraj.txt']);
  
  % Later, we may want to replace this copy with a better algorithm
  % to choose the truth trajectory. We will want to always compute
  % all trajectory files, and use the best one available as the truth
  % trajectory. 
    
  return;
end

%% Run rkwin if we need to, and select good points
if ((method == DYNAMIC) || (method == STATIC))

  % Generate rkwsTraj.pos file using rkwin.exe
  if (runRkwinTraj)
    origDir = pwd;
    cd(directoryname);
    system('run_rkwin_traj.bat');
    cd(origDir);
    disp('rkwin trajectory generation executed');
  end
  load([directoryname 'rkwsTraj.pos']);

  %% Generate position file
  rkwsTime = rkwsTraj(:,1)*1000; % in milliseconds
  dX = rkwsTraj(:,5);
  dY = rkwsTraj(:,6);
  dZ = rkwsTraj(:,7);
  ambStatus = rkwsTraj(:,16);
  clear rkwsTraj;
  
  %% Select section with good RTK initialization status
  % /****************************************************************
  %  * INITIALIZATION STATUS REPORTING
  %  ****************************************************************/
  % typedef enum {
  %   Init_None=0,       /* 0: Initialisation status unknown */
  %   Init_NewFloat,     /* 1: transitioned from none to float */
  %   Init_HeldFloat,    /* 2: Remained in Float mode */
  %   Init_NewInit,      /* 3: Transitioned from float to fix */
  %   Init_HeldInit,     /* 4: Remained Fully initialised */
  %   Init_PartInit,     /* 5: New initialisation of >= 1 sv(s) */
  %   Init_Suspect,      /* 6: Init. suspect because of mismatch */
  %   Init_Lost_HighRMS, /* 7: Lost init due to high rms condition */
  %   Init_Lost_Mismatch,/* 8: Lost init due to init. mismatch */
  %   Init_Lost_BadRef   /* 9: Lost init due to bad ref sv */
  % } RkInit_e;
  Init_HeldInit = 4;
  seg = find ( ambStatus == Init_HeldInit );
end %% ((method == DYNAMIC) || (method == STATIC))

%% Set up positions
if (method == DYNAMIC)
  % Create X,Y,Z positions
  X = dX(seg) + surveyPos(BASE,1);
  Y = dY(seg) + surveyPos(BASE,2);
  Z = dZ(seg) + surveyPos(BASE,3);
elseif (method == STATIC)
  % If we want static positions, find the mean position
  % of the trajectory.
  X = mean(dX(seg)) + surveyPos(BASE,1);
  Y = mean(dY(seg)) + surveyPos(BASE,2);
  Z = mean(dZ(seg)) + surveyPos(BASE,3);
else % (method == SURVEY)
  % Set up positions for the surveyed case
  X = ecefPos(1);
  Y = ecefPos(2);
  Z = ecefPos(3);
end

%% Set up time tags
if (method == DYNAMIC)
  % For the dynamic case, we use timetags from rkwin. 
  timetags = rkwsTime(seg);
else % ((method == SURVEY) || (method == STATIC))
  % For the static or surveyed case, we use timetags from the
  % reference SV list. 
  
  % Load ref SV file and extract timetags
  svFileName = [directoryname 'refSvList.txt'];
  load(svFileName);
  timetags = refSvList(:,1);
end

%% Create position matrix
% Now that we have the timetags and the positions, we can
% create the truth Trajectory matrix.  The matrix will contain
% [timetags, X, Y, Z]; 

% Initialize storage space. 
truthTrajectory = ones(length(timetags), 4); 

% Assign the truth Trajectory. Here timetags will always be a vector
% and X,Y and Z will either be the static position (scalars), or 
% each be column vectors containing the X, Y and Z components of the
% trajectory. 
truthTrajectory(:,1) = timetags;
truthTrajectory(:,2) = X;
truthTrajectory(:,3) = Y;
truthTrajectory(:,4) = Z;

%% Save output file 
trajFileName = [directoryname 'rtkTraj.txt'];

fid = fopen(trajFileName,'w'); 
fprintf(fid, '%d %f %f %f\r\n', truthTrajectory.'); 
fclose(fid); 

% Copy the rtk Trajectory to the truth Trajectory
copyfile( ...
  [directoryname 'rtkTraj.txt'], ...
  [directoryname 'truthTraj.txt']);

return; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function makeMposTrajFile

% Get sample time, and start and end times from stored data
loadData; 

% Get the trajectory data from the mPOS. 
mposLLA = load([directoryname mposFileName]); 

sampleTime = mposLLA(:,1);

% lat = vehNavData(:,2); in degrees
% lon = vehNavData(:,3); in degrees
% alt = vehNavData(:,4); in meters
% ell = [longitude, latitude, height]; in [radians, radians, meters]
DEG2RAD = pi/180; 
ell = [ DEG2RAD*mposLLA(:,3), ...
        DEG2RAD*mposLLA(:,2), ...
        mposLLA(:,4) + 0.20602 ];
warning('add mPOS center of nav. to antenna phase center correction'); 

% Get wgs84 ellipsoid parameters
WGS84 = sWGS84;

% Compute ECEF coordinates from long, lat, height. 
xyz = ell2xyz(ell,WGS84.a,WGS84.finv);

% Create a set of points onto which to interpolate the position data.
timetags = startTime:Ts:endTime; 

% Interpolate the xyz positions onto uniform time samples. 
X = interp1(sampleTime, xyz(:,1), timetags); 
Y = interp1(sampleTime, xyz(:,2), timetags); 
Z = interp1(sampleTime, xyz(:,3), timetags); 

% % Create matrix to output. 
% warning('******************');
% warning('subtracting antenna correction'); 
% warning('******************');
% 
% corrSgn = 1; % sign of the correction term
% 
% mposTrajectory(:,1) = timetags*1000; % convert timetags to milliseconds
% mposTrajectory(:,2) = X - corrSgn*19.763; % meters
% mposTrajectory(:,3) = Y - corrSgn*11.039; % meters
% mposTrajectory(:,4) = Z - corrSgn*0.293; % meters

%% Save output file 
mposFileName = [directoryname 'mposTraj.txt'];
fid = fopen(mposFileName,'w'); 
fprintf(fid, '%d %f %f %f\r\n', mposTrajectory.'); 
fclose(fid); 

return; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function makeAtsTrajFile

% Get sample time, and start and end times from stored data
loadData; 
% Ignore this for now. Later maybe use start and end times to chop
% the file. 

% Get the trajectory data from the ATS. 
atsTrajectory = load([directoryname atsFileName]); 

% Convert ATS time into milliseconds
atsTrajectory(:,1) = atsTrajectory(:,1)*1000;

% % Corrupting ATS trajectory file
% warning('Corrupting ATS trajectory file');
% atsTrajectory(:,2) = atsTrajectory(:,2) + 0.07; 
% 
% meanErrX = -5.2139e-002;
% meanErrY = 6.1370e-003;
% meanErrZ = -2.7939e-003;
% 
% Uncomment the following 3 lines so as not to corrupt
% the ATS trajectory file. 
meanErrX = 0;
meanErrY = 0;
meanErrZ = 0; 

atsTrajectory(:,2) = atsTrajectory(:,2) - meanErrX;
atsTrajectory(:,3) = atsTrajectory(:,3) - meanErrY;
atsTrajectory(:,4) = atsTrajectory(:,4) - meanErrZ;


%% Save output file 
atsFileName = [directoryname 'atsTraj.txt'];
fid = fopen(atsFileName,'w'); 
fprintf(fid, '%d %f %f %f\r\n', atsTrajectory.'); 
fclose(fid); 

return; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
