%
% Constants:
  k = .1;           %Kalman gain
%
% Assume the origin of the inertially-fixed frame is at the dock.

  [ eastings northings] = deg2utm( c.latitude.value*180/pi, c.longitude.value*180/pi );
%
% Ground velocity from Dvl:
  vel = c.platform_velocity_wrt_ground.value;
%
% Time.  This is the main computation cycle of the MVC, about .4 seconds:
  time = c.time.time;
%  
% How to check that c.time.value is just c.time.time converted to Unix time:
% norm(c.time.time' - datenum(datetime(c.time.value,'convertfrom','posixtime')'))
%
% Now relate everything to Mvc time.  Mvc time has the fastest sampling rate, so
% there cannot be two dvl or dat measurements in one Mvc interval.
%
  npts = length(time);
  dvl = NaN(3,npts);
  dat = NaN(2,npts);
  xhat = NaN(2,npts);
  xbar = NaN(2,npts);
  filtDat = NaN(2,npts);
  
  j = 1;
  for( i = 2:npts)
    if( c.platform_velocity_wrt_ground.time(j) <= time(i) && ...
          c.platform_velocity_wrt_ground.time(j) > time(i-1) )
      dvl(:,i) = c.platform_velocity_wrt_ground.value(:,j);
      if( j < length(c.platform_velocity_wrt_ground.time) )
        j=j+1;
      else
        break;
      end
    end
  end
%
% T_N_B is a direction cosine matrix that takes a vector from the vehicle frame B
% (B for body) to the Newtonian frame N, which in this case is the same as NED.
  j = 1;
  for( i = 2:npts)
    if( c.platform_orientation_matrix.time(j) <= time(i) && ...
          c.platform_orientation_matrix.time(j) > time(i-1) )
      T_N_B(:,:,i) =  c.platform_orientation_matrix.value(:,:,j)';
      if( j < length(c.platform_orientation_matrix.time) )
        j=j+1;
      else
        break;
      end
    end
  end

%
% Recoordinatize dvl from vehicle (or FSK) into NED:
  for( i = 1:npts )
    dvl(:,i) = T_N_B(:,:,i)*dvl(:,i);
  end
%
% Grid the Dat to Mvc time.  There is more code here to handle outliers and non-monotonic cases.
  cc = 86400;
  jj = 1;
  j = 1;
  for( i = 2:npts)
    if( j > length(c.TrackAcousticContact.heading_to_contact.time) - 1 )
      fprintf('j at end of array.  j = %d, i = %d\n',j,i);
      break;
    end
    if( j>1 && (c.TrackAcousticContact.heading_to_contact.time(j) < ...
                c.TrackAcousticContact.heading_to_contact.time(j-1)))
      %
      % Just skip it.
      fprintf('Time is non-monotonic at j= %d.\n',j);
      while( c.TrackAcousticContact.heading_to_contact.time(j) < time(i-1) )
        fprintf('Incrementing j = %d.\n',j);
        j=j+1;
        if( j > length(c.TrackAcousticContact.heading_to_contact.time) )
          break;
        end
      end
    end
    if( c.TrackAcousticContact.heading_to_contact.time(j) <= time(i) && ...
          c.TrackAcousticContact.heading_to_contact.time(j) > time(i-1) )
      dat(1,i) = c.TrackAcousticContact.range_to_contact.value(j);
      dat(2,i) = c.TrackAcousticContact.heading_to_contact.value(j);
      if( c.TrackAcousticContact.heading_to_contact.time(j+1) <= time(i) && ...
          c.TrackAcousticContact.heading_to_contact.time(j+1) > time(i-1) )
         %
         % If there are multiple contacts within one time(i-1) to time(i) interval, take the average:
         if( j+1 > length(c.TrackAcousticContact.heading_to_contact.time) )
           fprintf('j+1 at end of array.  j = %d, i = %d\n',j,i);
           break;
         end
         while( c.TrackAcousticContact.heading_to_contact.time(j+1) <= time(i) && ...
               c.TrackAcousticContact.heading_to_contact.time(j+1) > time(i-1) )
           dat(1,i) = dat(1,i) + c.TrackAcousticContact.range_to_contact.value(j+1);
           dat(2,i) = dat(2,i) + c.TrackAcousticContact.heading_to_contact.value(j+1);
           j=j+1;
           jj=jj+1;
           %fprintf('Got here, jj = %d\n',jj);
           if( j+1 > length(c.TrackAcousticContact.heading_to_contact.time) )
             fprintf('j+1 at end of array.  j = %d, i = %d\n',j,i);
             break;
           end
         end
         dat(:,i) = dat(:,i)./jj;
         jj = 1;
      end
      if( j <= length(c.TrackAcousticContact.heading_to_contact.time) )
        %fprintf('j is in the interval; i=%d, j=%d.\n',i,j);
        j=j+1;
      else
        fprintf('j at end of array.  j = %d, i = %d\n',j,i);
        break;
      end
    else
      %fprintf('tac.time(%d) is not in the interval (time(%d),time(%d)\n',j,i-1,i);
      %fprintf('%f  %f\n', c.TrackAcousticContact.heading_to_contact.time(j)*cc, time(i)*cc);
    end
  end
%
% Run the loop.  xhat is the current estimate of the state.  Start with the first non-NaN Dvl
% measurement:
  iStart=find(~isnan(dvl(1,:)),1,'first');
  lastDvl = dvl(1:2,iStart);
  lastDvlTime = time(iStart);
%
% Navigation relative to the dock, assuming the dock is located at (end):
% dockLoc = [northings(end); eastings(end)];
  %dockLoc = [origin_y; origin_x];
  dockLoc = dockLocAll(1:2,end);
  xhat(:,iStart) = [northings(iStart) - dockLoc(1);eastings(iStart) - dockLoc(2)];
  useXbar = 0;
  for( i=iStart+1:length(time) )
    %
    % Do a process update:
    dt = (time(i) - lastDvlTime)*cc;
    if( useXbar )
      xbar(:,i) = xbar(:,i-1) + [dt 0;0 dt]*lastDvl;
    else
      xbar(:,i) = xhat(:,i-1) + [dt 0;0 dt]*lastDvl;
    end
    lambar = atan2(-xbar(2,i),-xbar(1,i));
    rbar   = norm(xbar(:,i));
    filtDat(:,i) = [rbar;lambar];
    %
    % If a new Dvl measurement came in, save it
    if( ~sum(isnan(dvl(:,i))))
      lastDvlTime = time(i);
      lastDvl = dvl(1:2,i);
    end
    %
    % If a Dat measurement came in, do a measurement update:
    if( ~sum(isnan(dat(:,i))))
      r = dat(1,i);
      lam = dat(2,i);
      sl = sin(lam);
      cl = cos(lam);
      xhat(:,i) = xbar(:,i) - [k 0;0 k]*[-sl  cl; cl sl]*[rbar*angle1minus2(lam,lambar); r - rbar];
      useXbar = 0;
    else
      %If no Dat measurement came in, then the best estimate of the state is xbar:
      lastDvlTime = time(i);
      useXbar = 1;
    end
  end
  
figure(2)
hold on
%iix=find(~isnan(xhat(1,:)),1,'last');
%plot(xbar(2,1:iix),xbar(1,1:iix),'g.')
plot(xbar(2,:),xbar(1,:),'g.')
plot(xhat(2,:),xhat(1,:),'rx')

%
% Check to see if proNav output exists.
if( ~exist('start_time') || ~exist('end_time') )
  return;
end
%
for( i = APPROACH )
  fprintf('Plotting approach # %d\n',i);
  
  start_time = T.start_time(i);
  end_time = T.dock_time(i);
  if isnan(end_time)
    end_time = T.miss_time(i);
  end

  indxAppr = time >= start_time & time <= end_time;
  rbarAppr = filtDat(1,indxAppr);
  lambarAppr = filtDat(2,indxAppr);
  xbarAppr = xbar(:,indxAppr);
  xhatAppr = xhat(:,indxAppr);
  timeAppr = time(indxAppr);

  %indxClose = rbarAppr <=40 & rbarAppr > 10;
  indxClose = rbarAppr <=40;
  timeClose = timeAppr(indxClose);
  xbarClose = xbarAppr(:,indxClose);
  xhatClose = xhatAppr(:,indxClose);
  
  rbarClose = rbarAppr(:,indxClose);
  lambarClose = lambarAppr(:,indxClose);

  origin = xbarClose(:,end);

  figure(4+i);
  hold on
  %scatter( xbarClose(2,:)-origin(2), xbarClose(1,:)-origin(1),'filled','g');
  %plot( xhatClose(2,:) - origin(2) ,xhatClose(1,:) - origin(1),'rx')
  s1=scatter( xbarClose(2,:), xbarClose(1,:),'filled','g','DisplayName','xbar');
  p1=plot( xhatClose(2,:), xhatClose(1,:),'rx','DisplayName','xhat');
  s2=scatter( xhatClose(2,:) + sin(lambarClose).*rbarClose, xhatClose(1,:) + ...
      cos(lambarClose).*rbarClose, 'kx','DisplayName','Dock Locations Projected from EKF');
  legend('show')
end
