%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Generate data compatible with Jerome's stand-alone main().
%          from the ps8000.
% AUTHOR:  Rob McEwen
% DATE:    01/03/19
% NOTE:    The column ordering below in the dataJ(j+30... statement is 
%          dependant upon the channel-to-beacon mapping that is contained in
%          grid1.m.  This should be coded to be read out out grid1.m
% 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%    /*--- get data */
%    fscanf(fpt_data,"%lf ",&mission_time);
%    fscanf(fpt_data,"%lf ",&pitch);
%    fscanf(fpt_data,"%lf ",&yaw);
%    fscanf(fpt_data,"%lf ",&lbl_in.ping_time);
%    fscanf(fpt_data,"%lf ",&depth);
%    fscanf(fpt_data,"%lf ",&speed);
%    for (i=1; i<=lbl_in.number_of_beacon; i++)
%      fscanf(fpt_data,"%lf ",&lbl_in.tof[i]);
%
  dataJ = 0*ones(length(dataNav),10);
  dataJ         = [dataNav(:,[1 mTheta mPsi 1 mDepth 1 1 1 1 1 1])];
  dataJ(:,4)    = 0*dataJ(:,4);
  dataJ(:,6:11) = 0*dataJ(:,6:11);
  length(dataPS8000)
  for i = 1:length(dataPS8000),
    if(mod(i,10) == 0), i,end
    j = 1;
    while(dataNav(j,1) < dataPS8000(i,1) ), j = j+1; end
    dataJ(j,4) = dataPS8000(i,1);
    %
    % The j+30 below places the responses 5 seconds after the ping.  We are
    % selecting beacons 1:5 from dataPS8000.  The columns of dataPS8000
    % correspond to channel numbers, plus one.  We will ignore channel 3,
    % which is the interrogation channel.
    %
    %  beacon # 1    2    3    4   5
    %  channel# 2    4    5    6   7
    %  Freq     8.5 9.5 10.0 10.5 11.0
    %  Column#  7    8    9   10  11
    %
    dataJ(j+30,7:11) = dataPS8000(i, [3 5 6 7 8]);
  end
  %
  % Hardcode the speed here.  Change this to be read in from the DVL.
  %
  dataJ(:,6) = 1.5*ones(length(dataJ),1);
  %
  % The j+30 above causes dataJ to be too long in the final loop iteration.
  %
  dataJ = dataJ(1:length(dataNav),:);
  