%
% PURPOSE: Plot docking data.
%
% LOG:     Feb 25 2019, rsm initial.
%
lstr={};
lrDatPtr=[];
lrUsblPtr=[];
%global titleStr;
titleStr = [' Data Set: ', replace(DirName2,'_','-'), ' / ', replace(filepath,'_','-')];
if exist( 'position' ) ~= 1
  MonitorPositions = get(0,'MonitorPositions');
  position = MonitorPositions(end,:);
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end

position = [0 0 .979 .935];


if exist( 'lrNavFigPtr' ) ~= 1
  lrNavFigPtr= figure;
  set( lrNavFigPtr, 'Name', 'LR Nav', 'DeleteFcn', 'clear lrNavFigPtr' );
  set(gcf,'units','normalized')
  set(gcf,'position',position)
  if(exist('fontSize') == 1) 
    lrNavFontsize=fontSize;
  else
    lrNavFontsize=20;
  end
  lrNavFontweight='normal';
  set(gca,'fontsize',lrNavFontsize,'fontweigh',lrNavFontweight)
  set(gcf,'defaultaxesfontsize',lrNavFontsize,'defaulttextfontsize', ...
      lrNavFontsize)
  lrNavAxesPtr = gca;
else
 figure(lrNavFigPtr);
end

% initNav=[Navigation.longitude.value(1)*cos(38.6*180/pi)*pi/180*rE, Navigation.latitude.value(1)*pi/180*rE];

% fakeUtm     = [Navigation.longitude.value'*cos(38.6*180/pi)*pi/180*rE-initNav(1), ...
%               Navigation.latitude.value'*pi/180*rE-initNav(2)];
% fakeUtmTime = Navigation.longitude.time';

vehicleDataAvail = 0;
%if( exist( 'longitude' ) )
%
% Check to see if we've got vehicle data.  If so, plot it.
if( exist( 'data' ) ==1 && isfield( data, 'longitude') )
  vehicleDataAvail = 1;
  fprintf('Plotting vehicle data.\n');
  navTime = data.longitude.time';

  if( strcmp(data.longitude.units,'n/a') )
    unitsConv = 180/pi;
  elseif( strcmp(data.longitude.units,'arcdeg') )
    unitsConv = 1;
  else
    fprintf('Abort.  lat/long in unrecognized units.\n');
    return
  end
  [eastings northings zone]  = geo2utm(data.longitude.value'*unitsConv, data.latitude.value'*unitsConv);

  if( strcmp(data.longitude_fix.units,'n/a') )
    unitsConv = 180/pi;
  elseif( strcmp(data.longitude_fix.units,'arcdeg') )
    unitsConv = 1;
  else
    fprintf('Abort.  longitude_fix in unrecognized units.\n');
    return
  end
  [eastingsFix1 northingsFix1 zoneFix] = geo2utm(data.longitude_fix.value'*unitsConv, ...
      data.latitude_fix.value'*unitsConv);

  nFix = length(eastingsFix1);
  nNaN = floor(nFix/2);
  nFixA = nFix+nNaN;
  eastingsFix  = NaN(nFixA,1);
  northingsFix = NaN(nFixA,1);
  indxTemp = [1:nFixA]';
  %
  % Skip every third element (Twirl the gun, drop it in the holster)
  indxF = find(mod(indxTemp,3));
  eastingsFix(indxF)  = eastingsFix1; 
  northingsFix(indxF) = northingsFix1; 
  %
  pfTime  = data.platform_orientation.time';
elseif( exist( 'ta' ) )
  %
  % The ta struct is only created by replay in the LR world.
  fprintf('Plotting replay data.\n');
  navTime = ta.data(:,ta.navTime);
  eastings = ta.data(:,ta.navE);
  northings = ta.data(:,ta.navN);
else
  fprintf('No data found.  Assuming orignal vehicle data (not replay) is in struct ''data''.\n');
  return
end
indxnan = find( ~isnan(eastings) & ~isnan(northings) & northings>0 & eastings>0);
eastings0  = eastings(indxnan(1));
northings0 = northings(indxnan(1));

%eastings0  = 229901;
%northings0 = 3811142;

utmProj = [eastings-eastings0 northings-northings0];

%
% Now check to see if this was a DAT run or a whoi USBL run.  Plot appropriately.
if( exist('DAT') && isfield(DAT,'homing_target_range') )
  datTime = DAT.homing_target_range.time';
  datHits = NaN(length(pfTime),2);
  datIndx = closest(datTime,pfTime);

  range = NaN(size(pfTime));
  az    = NaN(size(pfTime));
  range(datIndx) = DAT.homing_target_range.value';
  az(datIndx)    = DAT.azimuth_vehicleCoordinates.value';

  utmIndx = closest(navTime, pfTime);
  psi     = platform_orientation.value';

  datHits(datIndx,:) = utmProj(datIndx,:) + [sin(psi(datIndx)+az(datIndx)).*range(datIndx) cos(psi(datIndx)+az(datIndx)).*range(datIndx)];

  npts = length(datIndx);
  fprintf(' Number of good hits:  %d\n', npts)
  %cax=[min(posxUsbl) max(posxUsbl)]; 
  beaconColor = 'hitNumber';
  beaconColor = 'range';
  if( strcmp( beaconColor, 'range' ) )
    cax=[min(range(datIndx)) max(range(datIndx))]; 
    cax=[0 90]; 
    cmp = colormap(jet(128));
    rng=linspace(cax(1),cax(2),length(cmp));
  elseif( strcmp( beaconColor, 'hitNumber' ) )
    cax=[1 npts]; 
    cmp = colormap(jet(npts));
  end
  hold on;
  for i=1:length(datIndx)
    %[mn kk]=min(abs(rng-posxUsbl(i)));
    if( strcmp( beaconColor, 'range' ) ) [mn kk]=min(abs(rng-range(datIndx(i))));
    elseif( strcmp( beaconColor, 'hitNumber' ) ) kk=npts-i+1;
    end
    lrDatPtr=plot(datHits(datIndx(i),1), datHits(datIndx(i),2), 'kd','color',cmp(kk,:),'markersize',7);
    lstr(length(lstr)+1,1) = {'DAT Hits'};
    %  plot(usblE(i)*distUnits,usblN(i)*distUnits,...
    %      'k+','color',cmp(kk,:),'markersize',7);
  end
end

if( exist('data') ==1 && isfield(data,'acoustic_contact_range') && 0 )
  %
  % Using the whoi line-array USBL:
  acouTime = data.acoustic_contact_range.time';
  acouHits = NaN(length(pfTime),2);
  acouIndx = closest(acouTime,pfTime);

  range = NaN(size(pfTime));
  az    = NaN(size(pfTime));
  range(acouIndx) = data.acoustic_contact_range.value';
  az(acouIndx)    = atan2(data.acoustic_contact_direction_vehicle_frame.value(2)',...
     data.acoustic_contact_direction_vehicle_frame.value(1)');

%  az1(acouIndx(1:end-1)) = data.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value;
%  az1(acouIndx(end)) = NaN;
%  az1=az1';
  
  utmIndx = closest(navTime, pfTime);
  psi     = data.platform_orientation.value';

 acouHits(acouIndx,:) = utmProj(acouIndx,:) + [sin(psi(acouIndx)+az(acouIndx)).*range(acouIndx) cos(psi(acouIndx)+az(acouIndx)).*range(acouIndx)];
%  acouHits(acouIndx,:) = utmProj(acouIndx,:) + [sin(psi(acouIndx)+az1(acouIndx)).*range(acouIndx) cos(psi(acouIndx)+az1(acouIndx)).*range(acouIndx)];

  npts = length(acouIndx);
  fprintf(' Number of good hits:  %d\n', npts)
  %cax=[min(posxUsbl) max(posxUsbl)]; 
  beaconColor = 'hitNumber';
  beaconColor = 'range';
  if( strcmp( beaconColor, 'range' ) )
    cax=[min(range(acouIndx)) max(range(acouIndx))]; 
    cax=[0 90]; 
    cmp = colormap(jet(128));
    rng=linspace(cax(1),cax(2),length(cmp));
  elseif( strcmp( beaconColor, 'hitNumber' ) )
    cax=[1 npts]; 
    cmp = colormap(jet(npts));
  end
  hold on;
  for i=1:length(acouIndx)
    %[mn kk]=min(abs(rng-posxUsbl(i)));
    if( strcmp( beaconColor, 'range' ) ) [mn kk]=min(abs(rng-range(acouIndx(i))));
    elseif( strcmp( beaconColor, 'hitNumber' ) ) kk=npts-i+1;
    end
    lrUsblPtr(i)=plot(acouHits(acouIndx(i),1), acouHits(acouIndx(i),2), 'kd','color',cmp(kk,:),'markersize',7);
    %lstr(length(lstr)+1,1) = {'ACOU Hits'};
    %  plot(usblE(i)*distUnits,usblN(i)*distUnits,...
    %      'k+','color',cmp(kk,:),'markersize',7);
  end
end

axis equal

lrNavPtr=plot(utmProj(indxnan,1), utmProj(indxnan,2),'c-','linewidth',3);
lstr(length(lstr)+1,1) = {'Navigated Position'};
hold on
if( vehicleDataAvail)
  lrGpsPtr=plot(eastingsFix-eastings0, ...
      northingsFix-northings0,'mx','linewidth',3);
      %northingsFix-northings0,'m--x','linewidth',3)
  lstr(length(lstr)+1,1) = {'GPS'};
end

if( exist( 'plotSwContours' ) == 0 ) plotSwContours=1;end
if( exist('plg') & plotSwContours )
  xlims = get(gca,'xlim');
  ylims = get(gca,'ylim');
  %imagesc(plg.utmProjx-posy0,plg.utmProjy-posx0,plg.z);
  indxxlim(1) = indxfn2(xlims(1),plg.eastings-eastings0);
  indxxlim(2) = indxfn2(xlims(2),plg.eastings-eastings0);
  indxylim(1) = indxfn2(ylims(1),plg.northings-northings0);
  indxylim(2) = indxfn2(ylims(2),plg.northings-northings0);
  decinp=1;
  if(exist('deci')) 
    fprintf('Navplot:  Setting decinp to %d\n',deci);
  else
    maxEl = 2.8e6;
    %
    % Compute decimation factor deci:
    deci = ceil( numel(plg.z)/maxEl);
    %
    % Round deci to the nearest multiple of 5:
    if( deci > 5 ) deci = 5*round(deci/5); end
  end
  decinp=deci;
  Levels=10;

  if( exist('loc') )
    if( strncmpi('Portuguese',loc,3) )
      Levels = [-105:1:-50];
    elseif( strncmpi('MC2800',loc,5) )
    elseif( strncmpi('axial',loc,3) )
      decinp=10;
    elseif( strncmpi('MC1850',loc,8) )
      Levels = [-2100:4:1600]';
      decinp=2;
    elseif( strncmpi('Wall',loc,4) )
      Levels = [-1100:10:0]';
      Levels = [-1100:20:0]';
      Levels = [-1100:30:0]';
      deci=3;    %Not used here.  Set up for plotSurf.
      decinp=4;
    elseif( strncmpi('MC1850-2',loc,8) )
      Levels = [-1100:4:0]';
      decinp=2;
      % elseif( strncmpi('axial',loc,3) )
      %   Levels = [-1800:4:1000]';
      %   decinp=2;
    elseif( strncmpi('plateau',loc,3) )
    else
      fprintf('Navplot: Unrecognized location %s\n',loc)
    end
  end
  
  % [cc,h]=contourf(downsample(plg.eastings-eastings0, decinp),...
  %                 downsample(plg.northings-northings0, decinp),...
  %                 downsample(downsample(plg.z,decinp)',decinp)',...
  %                 Levels);
  offsetX = -50;
  offsetY =  20;
  offsetX = 0;
  offsetY = 0;
  indxE=indxxlim(1):decinp:indxxlim(2);
  indxN=indxylim(1):decinp:indxylim(2);
  [cc,hh]=contourf(plg.eastings(indxE)-eastings0-offsetY,...
      plg.northings(indxN)-northings0-offsetX,...
      plg.z(indxN,indxE),...
      Levels);
  if( 1 )
    clabel(cc,hh);
  else
    % Trying to fix the excess precision problem with the labels.
    % Couldn't quite get it, yet.
    %
    % 2014b doesn't support text handles with the hh argument.  But
    % apparently removing the hh also removes all the other
    % "metadata" like fontsize, angle, location, etc.
    texth=clabel(cc);

    for( i=2:2:length(texth) )
      if(0)
	% First try:
	isobath    =  get(texth(i),'UserData');
	isobathStr =  num2str(isobath,'%.1f');
	set(texth(i),'String',isobathStr);
	isobathStr    =  get(texth(i),'UserData');
      end
      % Second try (has same result)
      isobathStr = get(texth(i),'String');
      isobath    = str2double(isobathStr);
      isobathStr =  num2str(isobath,'%.1f');
      set(texth(i),'String',isobathStr);
    end
    %
    % Last word.  There is a downloadable fix:
    % http://www.mathworks.com/support/bugreports/1114747
    %
    % Supposedly this is fixed in R2015b 8.6.
  end
  axis equal
  hold on
  lstr(length(lstr)+1,1) = {'Contours'};
  %
  % Do it again to put it on top:
  %lrNavPtr=plot(utmProj(:,1), utmProj(:,2),'c','linewidth',3);  set(gca,'xlim',xlims);
  %lstr(length(lstr)+1,1) = {'Navigated Position'};
  %uistack(lrNavPtr,'top');
  set(gca,'ylim',ylims);
end
grid on;
axis equal
%set(gca,'ylim',[-2000 1000]);
%set(gca,'xlim',[-1000 2000]);

if( exist('ta') )
  %
  % We have Trn data on the stack.  Plot Trn's estimated location:
  trnp = plot(ta.data(:,ta.mmseE)-eastings0,ta.data(:,ta.mmseN)-northings0,'m+-','linewidth',3);
  lstr(length(lstr)+1,1) = {'Trn'};
end

xlabel('Meters, East')
ylabel('Meters, North')

legend(lstr);

if( ~isempty(titleStr) ) title(titleStr,'interpreter','none'); end

uistack(lrNavPtr,'top');
if( vehicleDataAvail)     uistack(lrGpsPtr,'top'); end
if( ~isempty(lrDatPtr) )  uistack(lrDatPtr,'top'); end
if( ~isempty(lrUsblPtr) ) 
  for( i=1:length(lrUsblPtr) )
    uistack(lrUsblPtr(i),'top'); 
  end
end


%plot(datHits(:,1), datHits(:,2),'kx')


%str='Mission 201306061439 201306062133';
%title(str)
