
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

if( 0 )
if isnan(end_time)
    start_time = start_time_.Position(1)
    end_time = end_time_.Position(1)
end
end

if( isnan(L(i,2)) && isnan(L(i,3)) )
  fprintf('Insufficient data for approach %d\n', i);
  return;
end


idNav = c.latitude.time >= start_time & c.latitude.time <= end_time;
idTAC = c.TrackAcousticContact.range_to_contact.time >= start_time & c.TrackAcousticContact.range_to_contact.time <= end_time;
idTACs = c.TrackAcousticContact.contact_latitude_lowpass.time >= start_time & c.TrackAcousticContact.contact_latitude_lowpass.time <= end_time;

% Plot as offsets from the dock position
% ca

  lstr={};
  if exist( 'sprdFigPtr' ) ~= 1
    sprdFigPtr= figure;
    set( sprdFigPtr, 'Name', ['Navplot Animation approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr' );
    set(gcf,'position',position)
    sprdFontsize=20;
    sprdFontweight='normal';
    set(gca,'fontsize',sprdFontsize,'fontweigh',sprdFontweight)
    set(gcf,'defaultaxesfontsize',sprdFontsize,'defaulttextfontsize', ...
	    sprdFontsize)
    sprdAxesPtr = gca;
  else
    figure(sprdFigPtr);
    set( sprdFigPtr, 'Name', ['Navplot Animation approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr' );
    clf
  end
%
% Let the games begin:

hold on;
axis equal

hit_t    =  c.TrackAcousticContact.horizontal_range_to_contact_lowpass.time(idTACs);
range_t  =  c.TrackAcousticContact.horizontal_range_to_contact_lowpass.value(idTACs);
heading_t=  c.TrackAcousticContact.heading_to_contact_lowpass.value(idTACs);

veh_at_hit_x =  interp1( c.longitude.time(idNav), x_nav(idNav), hit_t);
veh_at_hit_y =  interp1( c.latitude.time(idNav), y_nav(idNav),  hit_t);

dock_x_median = dock_x_est_median(idTACs);
dock_y_median = dock_y_est_median(idTACs);

dock_x = dock_x_est(idTAC);
dock_y = dock_y_est(idTAC);

mksize = 10^2;

%
% Select range subset:  ranges > 10 m and < 40 m
minRange=1;
maxRange=40;
indx_tg   = range_t >= minRange & range_t <= maxRange;
hit_t_sub = hit_t(indx_tg);

if( isempty(hit_t_sub) ) 
  fprintf('No valid ranges from %d to %d meters.\n', minRange, maxRange);
  return;
end

startNavTime = min(hit_t_sub);
stopNavTime  = max(hit_t);

idNav_tg  = c.latitude.time >= startNavTime & c.latitude.time <= stopNavTime;

onlyTG = 1;

if( onlyTG )
  veh_x_sub         = x_nav(idNav_tg);
  veh_y_sub         = y_nav(idNav_tg);
  range_t_sub       = range_t(indx_tg);
  heading_t_sub     = heading_t(indx_tg);
  hit_t_sub         = hit_t(indx_tg);
  veh_at_hit_x_sub  = veh_at_hit_x(indx_tg);
  veh_at_hit_y_sub  = veh_at_hit_y(indx_tg);
  dock_x_median_sub = dock_x_median(indx_tg);
  dock_y_median_sub = dock_y_median(indx_tg);
  dock_x_sub        = dock_x(indx_tg);
  dock_y_sub        = dock_y(indx_tg);
  npts              = length(hit_t_sub);
else
  veh_x_sub         = x_nav(idNav);
  veh_y_sub         = y_nav(idNav);
  range_t_sub       = range_t;
  heading_t_sub     = heading_t;
  hit_t_sub         = hit_t;
  veh_at_hit_x_sub  = veh_at_hit_x;
  veh_at_hit_y_sub  = veh_at_hit_y;
  dock_x_median_sub = dock_x_median;
  dock_y_median_sub = dock_y_median;
  dock_x_sub        = dock_x;
  dock_y_sub        = dock_y;
  npts              = length(hit_t);
end

%scatter(x_nav(idNav) - origin_x, y_nav(idNav) - origin_y, 15, 'filled');

if( isempty(veh_x_sub) ) 
  fprintf('No valid navigation between %d and %d seconds.\n', startNavTime, ...
      stopNavTime);
  return
end

origin_x = veh_x_sub(1);
origin_y = veh_y_sub(1);

scatter(veh_x_sub - origin_x, veh_y_sub - origin_y, 15, 'b','filled');

%scatter(dock_x_median_sub - origin_x, dock_y_median_sub - origin_y, mksize, jet(length(range_t_sub)), 'filled','s');
  
%scatter(veh_at_hit_x_sub - origin_x, veh_at_hit_y_sub - origin_y, mksize, jet(length(range_t_sub)), 'filled','d');


hitColor = jet(length(range_t_sub));

  if(exist('p1')) clear p1, end
  if(exist('p2')) clear p2, end
  if(exist('p3')) clear p3, end
  if(exist('p4')) clear p4, end
  rangeFlag=0;

  stats = [];
  xteStats = [];
  
for( ii=1:npts )
  
  if(exist('p1')) set(p1,'visible','off'), end
  if(exist('p2')) set(p2,'visible','off'), end
  if(exist('p3')) set(p3,'visible','off'), end
  if(exist('p4')) set(p4,'visible','off'), end
  if(range_t_sub(ii) < 10 && rangeFlag == 0)
    if(exist('p1')) set(p1,'visible','on'), end
    if(exist('p3')) set(p3,'visible','on'), end
    rangeFlag=1;
  end
  
  p1=  plot(dock_x_median_sub(ii) - origin_x, dock_y_median_sub(ii) - origin_y, 's', 'color', ...
      hitColor(ii,:),'markersize',10);

  p2= plot(veh_at_hit_x_sub(ii) - origin_x, veh_at_hit_y_sub(ii) - origin_y, 'd', 'color',...
      hitColor(ii,:),'markersize',10);

  mdim = 4;     % median dimension - 1.

  if( ii > mdim )
    p3     = plot(dock_x_sub(ii-mdim:ii) - origin_x, dock_y_sub(ii-mdim:ii) - origin_y, ...
             'x', 'color', 'k');
    r      = sqrt((dock_x_sub(ii-mdim:ii)-dock_x_median_sub(ii)).^2 + (dock_y_sub(ii-mdim:ii)-dock_y_median_sub(ii)).^2);
    stats  = [stats; mean(r) median(r) std(r)];
    xte    = -sin(heading_t_sub(ii))*(dock_x_sub(ii-mdim:ii)-dock_x_median_sub(ii))...
             +cos(heading_t_sub(ii))*(dock_y_sub(ii-mdim:ii)-dock_y_median_sub(ii));
    xteStats = [xteStats; mean(xte) median(xte) std(xte,1)];
  end

  p4= plot([veh_at_hit_x_sub(ii) dock_x_median_sub(ii)] - origin_x,...
      [veh_at_hit_y_sub(ii) dock_y_median_sub(ii)] - origin_y, ...
      'k','linewidth',1);
  
  b2d(ii) = atan2( dock_x_median_sub(ii)-veh_at_hit_x_sub(ii), ...
      dock_y_median_sub(ii)-veh_at_hit_y_sub(ii) );

  axis equal
  
  pause(.3);
  %pause(.1);

  if(ii == npts)
    %if(exist('p1')) set(p1,'visible','off'), end
    %if(exist('p2')) set(p2,'visible','off'), end
    %if(exist('p3')) set(p3,'visible','off'), end
    if(exist('p3')) set(p3,'color','r'), end
  end
  
  
end


hold on

if( 0 )
  for( ii=1:npts )
    plot([veh_at_hit_x_sub(ii) dock_x_median_sub(ii)] - origin_x,...
        [veh_at_hit_y_sub(ii) dock_y_median_sub(ii)] - origin_y,  'k','linewidth',1);
  end
end

%scatter(dock_x - dock_x, dock_y - dock_y,75,'>k','filled');
%colormap(brewermap(128,'*RdPu'))
%cb = colorbar;
%cb.Label.String = 'Range (m)'; %'Elapsed time (sec)';
%caxis([0 100])
% set(gca, 'DataAspectRatio',[1 1 1]);
title([ dirName, ' Approach # ', num2str(i), '; ', num2str(npts),...
      ' Median-Filtered DAT Fixes Total; ', num2str(length(hit_t_sub)), ' here.' ]);
xlabel('Eastings')
ylabel('Northings')
legend('Vehicle Position','Projected Dock Position', 'Corresponding Vehicle Position');
axis equal

dataPath = '/Users/rob/lr/vehiclelogs/tethys/';
fname = [dataPath,dirName,'A',num2str(APPROACH)];
save(fname,'stats');

if exist( 'sprdFigPtr2' ) ~= 1
  sprdFigPtr2= figure;
  set( sprdFigPtr2, 'Name', ['Statistics for approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr2' );
  set(gcf,'position',position)
  sprdFontweight='normal';
  set(gca,'fontsize',sprdFontsize,'fontweigh',sprdFontweight)
  set(gcf,'defaultaxesfontsize',sprdFontsize,'defaulttextfontsize', ...
      sprdFontsize)
  sprdAxesPtr = gca;
else
  figure(sprdFigPtr2);
  set( sprdFigPtr2, 'Name', ['Statistics for approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr2' );
  clf
end

plot(range_t_sub(mdim+1:end), stats,'--o');
grid on;
set(gca,'xdir','reverse')
title([ dirName, ' Approach # ', num2str(i), '; Distance of DAT Fixes From Median. ', ...
      num2str(npts),' DAT Fixes.']);
xlabel('Range, Meters')
ylabel('Meters')
legend('Mean','Median', 'Standard Deviation','Max DAT Angular Error Spec.');

DatErr = .3*pi/180;     %+/- .3 degrees.

if exist( 'sprdFigPtr3' ) ~= 1
  sprdFigPtr3= figure;
  set( sprdFigPtr3, 'Name', ['Statistics for approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr3' );
  set(gcf,'position',position)
  sprdFontweight='normal';
  set(gca,'fontsize',sprdFontsize,'fontweigh',sprdFontweight)
  set(gcf,'defaultaxesfontsize',sprdFontsize,'defaulttextfontsize', ...
      sprdFontsize)
  sprdAxesPtr = gca;
else
  figure(sprdFigPtr3);
  set( sprdFigPtr3, 'Name', ['Statistics for approach # ',num2str(i)], 'DeleteFcn', 'clear sprdFigPtr3' );
  clf
end

plot(range_t_sub(mdim+1:end), xteStats,'--o');
hold on
plot(range_t_sub(mdim+1:end), range_t_sub(mdim+1:end)*DatErr,'m');
plot(range_t_sub(mdim+1:end), -range_t_sub(mdim+1:end)*DatErr,'m');
grid on;
set(gca,'xdir','reverse')
title([ dirName, ' Approach # ', num2str(i), '; Lateral Distance of DAT Fixes From Median. ', ...
      num2str(npts),' DAT Fixes.']);
xlabel('Range, Meters')
ylabel('Meters')
legend('Mean','Median', 'Standard Deviation','Max DAT Angular Error Spec.');





%indxR = c.TrackAcousticContact.range_to_contact.value <= 11 &  c.TrackAcousticContact.range_to_contact.value >= 9;
