%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot vehicle position in UTM.
% AUTHOR:  rsm
% DATE:    00/8/20
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% Check for LBL data:
%
% if exist('nfix') ~= 1
%   return
% end
%
% Check to see if "review" has been run.  If not, initialize below.
%
% if( exist('position') ~=1 )
 if(1)
   global titleStr;
   global vehicleName;
   vehicleName = 'Dorado';
   titleStr = [' Data Set: ', DirName];
   units;      %Put unit conversions on the stack
 end
 if( exist('linewidth') ~=1 ) linewidth = 1; end

 if( exist('position') ~=1 ) 
   npposition = [4 32 815 658];
 else
   npposition = position;
 end
 
 if( exist('plotSwPublish') == 0 ) plotSwPublish=0;end
 
 
%
% Plot distances in either km or meters.
%
 if( exist('pmeters') ~=1 ) pmeters = 0; end
 distUnits = 1/1000;
 if( pmeters == 1 ) distUnits = 1; end
 
%plotSwKft = 1; 
 
 if exist( 'nav' ) ~= 1
     nav = figure;
 end     
 figure(nav);
 clf;
 set( nav, 'Name', 'Navigation Plot', 'DeleteFcn', 'clear nav' );
 set(gcf,'position',npposition)
 if( exist('fontsize') == 0), fontsize = 14; end
 if( plotSwPublish ) fontsize = 30; end
 if( fontsize ==12 ), linewidth = .5;end
 fontweight = 'normal';
 set(gca,'fontsize',fontsize,'fontweigh',fontweight)
 set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

 
 posx0 = dataNav(1,mPos_x);
 posy0 = dataNav(1,mPos_y);

 % posx0 = 0;
 % posy0 = 0;
 
 posx  = dataNav(:,mPos_x) - posx0;
 posy  = dataNav(:,mPos_y) - posy0;
 if( exist( 'plotSwDepth' ) == 1 && plotSwDepth > 0)
   depthIndx = find(dataNav(:,mDepth) >= plotSwDepth );
   fprintf('Trimming all navigation data at depths less than %.1f\n',...
           plotSwDepth);
 else
   depthIndx = [1:length(dataNav)]';
   plotSwDepth = 0;
 end

 if( exist( 'plotSwStart' ) == 1 & exist( 'plotSwStop' ) == 1 &...
   plotSwStop > plotSwStart)
   depthIndx = find( dataNav(:,1) >= plotSwStart & dataNav(:,1) <= plotSwStop );
   fprintf('Trimming all navigation data outside interval (%.1f, %.1f) sec.\n',...
           plotSwStart, plotSwStop);
 else
   depthIndx = [1:length(dataNav)]';
 end
 %
 lstr = {};
 if( exist('plotSwLatLon') ~= 1 | exist('defaultm') ~= 2 ) plotSwLatLon = 0;end
 if( plotSwLatLon == 1 & exist('defaultm') ~= 2 ) 
   plotSwLatLon = 0;
   disp(sprintf(['Setting plotSwLatLon back to zero.  '...
	 'The mapping TB is not present.\n']))
 end
 %
 % Hack out bad data for the Nav paper.  Sorry!
 %
 if(    ~strncmp(DirName,'2001.290',8) ) %...
%   &   ~strncmp(DirName,'2001.286',8) )
   if( plotSwLatLon )
     %
     % Hardcode the zone for the Monterey bay.  Later, figure out a way to
     % read this from workspace.cfg.  (Or from the GPS data if it's valid).
     %
     zone = '10S';    %Monterey Bay
     clear mstruct tmpLat tmpLon latLim lonLim;
     mstruct = defaultm('utm');
     mstruct.zone = zone;
     mstruct.geoid = almanac('earth','geoid','m','grs80'); %WGS84
     [mstruct errorStr] = defaultm(mstruct);
     if( errorStr ) disp(sprintf(errorStr));end
     axesm(mstruct);
     %
     % In my notation Pos_x are the Northings, and Pos_y are the Eastings.
     %
     disp(sprintf('Beginning dataNav minvtran.\n'));
     [tmpLat tmpLon]=minvtran( mstruct, dataNav(:,mPos_y), dataNav(:,mPos_x));
     plotm( tmpLat, tmpLon, 'linewidth', linewidth ); 
     minlat = min(tmpLat);
     maxlat = max(tmpLat);
     minlon = min(tmpLon);
     maxlon = max(tmpLon);
     meridiansc = abs(maxlon-minlon)/3;
     parallelsc = abs(maxlat-minlat)/5;
     mlabel on; 
     plabel on;
     setm(gca,'LabelFormat','signed')     
     setm(gca,'maplatlimit',[minlat maxlat],...
	 'maplonlimit',[minlon maxlon],...
	 'mlinelocation', meridiansc, 'plinelocation', parallelsc,...
	 'mlabellocation', meridiansc, 'plabellocation', parallelsc,...
	 'mlabelround', -3, 'plabelround', -3);
     gridm on;
     grid off;
     set(gca,'Xgrid','off','Ygrid','off');
     showaxes;
     %framem on; 
     %xlabel( 'East Longitude, Degrees (Decimal)' )
     %ylabel( 'Latitude, Degrees (Decimal)' )
   else
     %plot( posy*distUnits, posx*distUnits,'linewidth', linewidth);
     plot( posy(depthIndx)*distUnits, posx(depthIndx)*distUnits,'b:',...
           'linewidth',3);
     hold on
     axis equal
     xlims = get(gca,'xlim');
     ylims = get(gca,'ylim');

     if( exist('loc') &&  strncmpi('Wall',loc,4) )
       ylims(1) = ylims(1) - 500;
     end
     
     
     lstr = {'Navigated Position'};
     if( exist( 'plotSwContours' ) == 0 ) plotSwContours=1;end
     if( exist('plg') & plotSwContours )
         %imagesc(plg.utmx-posy0,plg.utmy-posx0,plg.z);
         indxxlim(1) = indxfn2(xlims(1),plg.eastings-posy0);
         indxxlim(2) = indxfn2(xlims(2),plg.eastings-posy0);
         indxylim(1) = indxfn2(ylims(1),plg.northings-posx0);
         indxylim(2) = indxfn2(ylims(2),plg.northings-posx0);
         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('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('plateau',loc,3) )
	   else
	     fprintf('Navplot: Unrecognized location %s\n',loc)
	   end
	 end
	 
         if( 0 )
	   %
	   % This might be useful later...
	   %
	   if( recognized )
	     switch(missionFileName)
	      case{'SimWCakeSimple.cfg','SimVIsland.cfg'}
	      case('Flat4c.cfg')
	       Levels = 5;
	      case{'PortOALow2.cfg'}
	      case{'PortOA30.cfg'}
	      case{'SimMC1850.cfg','20150827_IcebergTesting_m1v4_rob.cfg'}
	       Levels = [-2100:4:1600]';
	       decinp=2;
	      case{'SimMC2800.cfg'}
	      case{'Transect.cfg'}
	      otherwise
	       fprintf('Unrecognized scenario.\n')
	     end
	   end
	 end
	 
	 
	 % [cc,h]=contourf(downsample(plg.eastings-posy0, decinp),...
         %                 downsample(plg.northings-posx0, decinp),...
         %                 downsample(downsample(plg.z,decinp)',decinp)',...
         %                 Levels);
         indxE=indxxlim(1):decinp:indxxlim(2);
         indxN=indxylim(1):decinp:indxylim(2);
         [cc,hh]=contourf(plg.eastings(indxE)-posy0,...
                          plg.northings(indxN)-posx0,...
                          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'};
         plot( posy(depthIndx)*distUnits, posx(depthIndx)*distUnits,'b:',...
               'linewidth',3);
         lstr(length(lstr)+1,1) = {'Navigated Position'};
         set(gca,'xlim',xlims);
         set(gca,'ylim',ylims);
     end
     %plot( posy(depthIndx)*distUnits, posx(depthIndx)*distUnits,...
     %      'linewidth', linewidth,'linestyle','.');
     %lstr(length(lstr)+1,1) = {'Navigated Position 2'};
     grid on;
 
    %set(gca,'fontsize',12)

     %xlabel( 'Relative UTM East, km')
     %ylabel( 'Relative UTM North, km')
     if( pmeters )
       xlabel('Meters, East')
       ylabel('Meters, North')
     else
       xlabel('Kilometers, East')
       ylabel('Kilometers, North')
     end
   end
 end
 %set(gca,'xticklabel',cellstr(num2str(get(gca,'xtick')','%d')))
 %set(gca,'yticklabel',cellstr(num2str(get(gca,'ytick')','%d')))

 if( exist( 'dataTC' ) == 1 )
   indxTC=find(dataTC(:,propRpm)<=5);
   posxAborted = interp1(dataNav(:,1), posx, dataTC(indxTC,1));
   posyAborted = interp1(dataNav(:,1), posy, dataTC(indxTC,1));
   depthAborted = interp1(dataNav(:,1), dataNav(:,mDepth), dataTC(indxTC,1));

   idx2 = find(depthAborted > 5);
   
   if( ~isempty(indxTC) )
     plot( distUnits*posyAborted(idx2), distUnits*posxAborted(idx2), 'h','markersize',10,...
           'markeredgecolor','r', 'markerfacecolor','r')
     lstr(length(lstr)+1,1) = {'Prop Off'};
   end
 end
 
 if exist( 'dataSim' ) == 1
   line( (dataSim(:,sPos_y)-posy0)*distUnits, ...
         (dataSim(:,sPos_x)-posx0)*distUnits, ...
         'linewidth', linewidth, 'Color','g' );
   lstr(length(lstr)+1,1) = {'Simulation Truth'};
 end
%
% GPS Ship location, after processing by cvtwf.  Note the the coversion 
% to UTM was performed by Winfrog.
%
 if exist( 'usblr' ) == 1
   line( (usblr(:,shipE)-posy0)*distUnits, ...
         (usblr(:,shipN)-posx0)*distUnits, ...
         'linewidth', linewidth, 'Color',[1 .62 .40] );  %Copper color
   lstr(length(lstr)+1,1) = {'Ship GPS Posn'};
   if( exist('utmzone') == 2 )

   end
 end
%
% Plot GPS data.  GpsLog.cc doesn't log the hemisphere, so by implication it
% is West for the Monterey bay.  Thus the sign flip on the longitude term
% below. 
%
 if exist( 'mGpsValid' ) == 1
   if( plotSwLatLon )
%    line( -gps(:,longitude), gps(:,latitude), ...
 %         'linewidth', linewidth, 'linestyle','x','Color','m' );
     linem(  gps(:,latitude)*180/pi, -gps(:,longitude)*180/pi,...
           'linewidth', linewidth, 'linestyle','x','Color','m' );
   else
     plot( (dataNav( find(dataNav(:,mGpsValid)),  mGpsEast ) - posy0)*distUnits, ...
           (dataNav( find(dataNav(:,mGpsValid)),  mGpsNorth) - posx0)*distUnits, ...
           'mx','linewidth', linewidth);
%          'linewidth', linewidth, 'marker','x','Color','m' );
%          'linewidth', linewidth, 'linestyle','x','Color','m' );
   end
   if( ~isempty(find(dataNav(:,mGpsValid))) ) 
       lstr(length(lstr)+1,1) = {'GPS'};
   end
 end

if( exist('cam')==1 && 1)
  camx = interp1(dataNav(:,1), posx, cam.data(:,1));
  camy = interp1(dataNav(:,1), posy, cam.data(:,1));
  hold on
  camhand = plot(camy*distUnits,camx*distUnits,'kp', 'linewidth',linewidth);
  if( exist('numOn')~=1 ) numOn=0;end
  if( numOn )
    camhand2= text(camy*distUnits+.3,camx*distUnits+.3, ...
                   num2str(cam.data(:,cam.number)+1407,'%3d\n'));
    zoom on
%                   num2str(cam.data(:,cam.number)+897,'%3d\n'));  %First mission
  end
  lstr(length(lstr)+1,1) = {'Photos'};
end

if( exist('gulper')==1 )
  gulpx = interp1(dataNav(:,1), posx, gulper.data(:,1));
  gulpy = interp1(dataNav(:,1), posy, gulper.data(:,1));
  hold on
% gulphand = plot(gulpy*distUnits,gulpx*distUnits,'kh','linewidth',linewidth+2);
% gulphand = plot(gulpy*distUnits,gulpx*distUnits,'kh','linewidth',3);
  gulphand = plot(gulpy*distUnits,gulpx*distUnits,'k^','markersize',15);
  lstr(length(lstr)+1,1) = {'Gulps'};
end

if( exist('plotSwTa') == 0 ) plotSwTa = 0; end
if( exist('ta')==1 & plotSwTa)
  plot( ta.data(:,ta.mmseE)-posy0,  ta.data(:,ta.mmseN)-posx0,...
        'linewidth', linewidth,'linestyle','--');
  set(gca,'ylim',ylims)
  lstr(length(lstr)+1,1) = {'Terrain Relative'};
end

if( exist('dvlSide')==1 )
  dvlSidex   = interp1(dataNav(:,1), posx, dvlSide.data(:,1));
  dvlSidey   = interp1(dataNav(:,1), posy, dvlSide.data(:,1));
  dvlSidePsi = interp1(dataNav(:,1), dataNav(:,mPsi), dvlSide.data(:,1));
  %
  % Now add the range measurement, taking only vehicle heading into
  % account.  We'll add a full 3D DC matrix later if necessary.
  dvlSidex = dvlSidex + sin(dvlSidePsi).*dvlSide.data(:,dvlSide.Range);
  dvlSidey = dvlSidey - cos(dvlSidePsi).*dvlSide.data(:,dvlSide.Range);
  hold on
  plot(dvlSidey, dvlSidex, 'gx');
  lstr(length(lstr)+1,1) = {'Side-Looking Dvl Range'};
end
%
% Plot the commanded trackline from waypoints in the .cfg file.  This
% comes through syslog, and sometimes gets chopped up, so it doesn't
% always work.
%
 if( exist( 'dataWP' ) == 1 & min(size(dataWP)) > 1 )
   if( plotSwLatLon )
     [latW lonW]=minvtran( mstruct, dataWP(:,3), dataWP(:,2));
     linem( latW, lonW,...
	 'color','r','linewidth', linewidth)
     lstr(length(lstr)+1,1) = {'Trackline'};
   else
     plot( (dataWP(:,3)-posy0)*distUnits, (dataWP(:,2)-posx0)*distUnits, ...
	 'r','linewidth', linewidth)
     lstr(length(lstr)+1,1) = {'Trackline'};
     plot( (dataWP(:,3)-posy0)*distUnits, (dataWP(:,2)-posx0)*distUnits, ...
	 'gd','linewidth', linewidth)
     lstr(length(lstr)+1,1) = {'Waypoints'};
     sizeDataWP = size(dataWP);
     if( sizeDataWP(2) > 3 )
         plot( (dataWP(:,5)-posy0)*distUnits, (dataWP(:,4)-posx0)*distUnits, ...
               'k-','linewidth', linewidth)
         lstr(length(lstr)+1,1) = {'Starboard Corridor Edge'};
         plot( (dataWP(:,7)-posy0)*distUnits, (dataWP(:,6)-posx0)*distUnits, ...
               'k-','linewidth', linewidth)
         lstr(length(lstr)+1,1) = {'Port Corridor Edge'};
     end
   end
 end

 %
 % Switch between wall and bottom following:
 % 
 if( exist('idt')==1 && ~isfield(idt,'vertMt'))
    idt.vertMt = 1;
 end 
 
 
if( exist('idt')==1 && isfield(idt,'data') && idt.vertMt == 0)
  % 
  % nbeams is the total number of beams, assumed to be even.
  % npbeams is the number to plot. npbeams=1 means plot only one out of
  % the 120.
  %
  % Again, this has to plot last otherwise the legend will be scrambled.
  %
  if( ~isfield(idt,'nbeams') )  
      if( idt.data(1,idt.NumberOfBeams) ~= 0 )
          idt.nbeams = idt.data(1,idt.NumberOfBeams); 
      else
          idt.nbeams = 120; 
          fprintf(['navplot.m:  idt.NumberOfBeams is zero. Setting ' ...
                   'nbeams to %d\n'], idt.nbeams);
      end
  end
  if( ~isfield(idt,'npbeams') ) idt.npbeams = 119;end
  idt.x   = interp1(dataNav(:,1), posx(:), idt.data(:,1));
  idt.y   = interp1(dataNav(:,1), posy(:), idt.data(:,1));
  idt.northings=idt.x;
  idt.eastings=idt.y;
  %
  % This removes the 0-2pi jumps from dataNav(:,mPsi) and converts it to a
  % continuous curve.  Otherwise the subsequent interp1 finds intermedate
  % values between 0 and 2pi at the jump points, which was creating false
  % (x,y) locations in the re-computation of idt.x  and y further below.
  %
  offset = 0;
  psiNoJump = nan(length(dataNav(:,mPsi)),1);
  for( i=1:length(dataNav(:,mPsi))-1)
      psiNoJump(i,1) = dataNav(i,mPsi) + offset;
      psiDiff = dataNav(i+1,mPsi)-dataNav(i,mPsi);
      if( abs(psiDiff) > 2*pi -.1)
          offset = offset - 2*pi*sign(psiDiff);
      end
  end
  psiNoJump(i+1,1)= dataNav(i+1,mPsi) + offset;

  idt.Psi = interp1(dataNav(:,1), psiNoJump, idt.data(:,1));
  if(sum(isnan(idt.Psi)))
      fprintf('Warning: idt.Psi has length %d and contains %d nans.\n',length(idt.Psi), ...
              sum(isnan(idt.Psi)));
  end
  %idt.Psi = interp1(dataNav(:,1), dataNav(:,mPsi), idt.data(:,1));
  monoIndx = find(diff(dataDC(:,1))>0);
  idt.Psic= interp1(dataDC(monoIndx,1), dataDC(monoIndx,cPsi), idt.data(:,1));
  %
  % Index into idt.data
  idt.BeamRange=idt.BeamRange1+[0:idt.nbeams-1];
  %
  beamIndx   = floor(idt.nbeams/(idt.npbeams+1)*[1:idt.npbeams]);
  %beamAngles = (beamIndx-60)*idt.nbeams/(idt.nbeams-1);
  %beamAngles = (beamIndx*idt.nbeams/(idt.nbeams-1) - 60)*pi/180;  %Degrees
  beamIndx30 = find(beamIndx>30); %Drop out beams below 30 degrees.
 %beamIndx = beamIndx(beamIndx30);
  beamAngles = ([0:idt.nbeams-1]*idt.nbeams/(idt.nbeams-1) - 60)*pi/180;  %Degrees
  beamAngles = ([0:idt.nbeams-1] - 60)*pi/180;  %Degrees
  if(1)
      for( i=1:length(idt.x) )
          idt.x(i,1:length(beamIndx)) = idt.x(i) + sin(idt.Psi(i) + beamAngles(beamIndx)).*...
              idt.data(i,idt.BeamRange(beamIndx));
          idt.y(i,1:length(beamIndx)) = idt.y(i) - cos(idt.Psi(i) + beamAngles(beamIndx)).*...
              idt.data(i,idt.BeamRange(beamIndx));
      end
  else
      for( i=1:length(depthIndx) )
      idt.x(depthIndx(i),1:length(beamIndx)) = idt.x(depthIndx(i)) + sin(idt.Psi(depthIndx(i)) + ...
        beamAngles(beamIndx)).*idt.data(depthIndx(i), idt.BeamRange(beamIndx));
      idt.y(depthIndx(i),1:length(beamIndx)) = idt.y(depthIndx(i)) - cos(idt.Psi(depthIndx(i)) + ...
        beamAngles(beamIndx)).*idt.data(depthIndx(i),idt.BeamRange(beamIndx));
      end
  
  end
  hold on
  %plot(idt.y(depthIndx), idt.x(depthIndx), 'x');
  plot(idt.y, idt.x, 'x');
  if(length(beamIndx) < 7 )
      for( i=1:length(beamIndx))
          lblStr = sprintf('DeltaT Beam %d at %.1f Deg.',beamIndx(i), ...
                           beamAngles(beamIndx(i))*180/pi);
          %lstr(length(lstr)+1,1) = { 'Decimated side-Looking Imagenex Ranges; '};
          lstr(length(lstr)+1,1) = { lblStr };
      end
  end
  rmfield(idt,'x');
  rmfield(idt,'y');
  %rmfield(idt,'Psi');
  if(exist('plotSwRays')==0) plotSwRays=0;end
  if( norm(dataDC(:,rangeToWall)) ~= 0 && plotSwRays);
      ctrlBeamIndx = find(dataDC(:,rangeToWall) ~= 0);
      idt.xdc   = interp1(dataNav(:,1), posx(:), dataDC(ctrlBeamIndx,1));
      idt.ydc   = interp1(dataNav(:,1), posy(:), dataDC(ctrlBeamIndx,1));
      idt.Psidc = interp1(dataNav(:,1), dataNav(:,mPsi), dataDC(ctrlBeamIndx,1));
      idt.ctrlx = idt.xdc + sin(idt.Psidc + ...
                                (dataDC(ctrlBeamIndx,SideBeamIndex)-60)*pi/180).*...
          dataDC(ctrlBeamIndx,rangeToWall);
      idt.ctrly = idt.ydc - cos(idt.Psidc + ...
                                (dataDC(ctrlBeamIndx,SideBeamIndex)-60)*pi/180).*...
          dataDC(ctrlBeamIndx,rangeToWall);
      plot(idt.ydc, idt.xdc,'kd')
      plot(idt.ctrly, idt.ctrlx,'rd')
      for( i=1:length(ctrlBeamIndx))
          plot([idt.ydc(i) idt.ctrly(i)],[idt.xdc(i) idt.ctrlx(i)],'g');
      end
  end
end

 if exist( 'lblgrid' ) == 1
   line( (lblgrid(eastIndx,:)-posy0)*distUnits, (lblgrid(northIndx,:)-posx0)*distUnits,...
         'linestyle','x','color','r','linewidth', linewidth)
   for i=1:size(lblgrid,2)
   text( lblgrid(eastIndx,i)-posy0, lblgrid(northIndx,i)-posx0,...
         [' B',num2str(i)] )
   end
   lstr(length(lstr)+1,1) = {'Trackline'};
 end
%
% Plot estimated position from lbl
%
 lblscale = 50;
 if ( exist('filter_north') & ...
      norm(dataNav(:,filter_north)) ~= 0 & plotSwLBL == 1),
   if( dataNav(1,filter_east) ~= dataNav(1,mPos_y) )
     disp(' The initial lbl filter position is not the same as nav.')
   end
   %
   % Replace posx0 & posy0 with initial position of the filter.
   % This prevents the plot scaling from being too large when the
   % lbl isn't initialized, but also doesn't make it obvious.
   %
   %line(dataNav(:,filter_east)  - posy0,...
   %    dataNav(:,filter_north) - posx0,...
   %    'linewidth', linewidth, 'Color','c' );
   line( (dataNav(:,filter_east)  - dataNav(1,filter_east))*distUnits,...
         (dataNav(:,filter_north) - dataNav(1,filter_north))*distUnits,...
        'linewidth', linewidth, 'Color','c' )
    lstr(length(lstr)+1,1) = {'Lbl Filter'};

   line( (dataNav(find(dataNav(:,efix) ~= 0),efix)  - posy0)*distUnits,...
         (dataNav(find(dataNav(:,nfix) ~= 0),nfix)  - posx0)*distUnits,...
        'linewidth', linewidth, 'linestyle','x','Color','k' );
   %
   % Plot the velocity of the water wrto the bottom:
   %
   fixIndx = find(dataNav(:,efix) ~= 0);
   for i=1:length(fixIndx);
     epos = dataNav(fixIndx(i),filter_east ) - posy0;
     npos = dataNav(fixIndx(i),filter_north) - posx0;
     evel = epos + lblscale*dataNav(fixIndx(i),east_current);
     nvel = npos + lblscale*dataNav(fixIndx(i),north_current);
     line([epos;evel]*distUnits,[npos;nvel]*distUnits,'color','r');
   end
 end
%
% Plot Usbl hits
%
if( exist('usbllog')==1 )
  statusIndx = find(usbllog(:,sonardyneStatus)==1);
  if( length(statusIndx) ~= 0 )
    usblStatDC=interp1(usbllog(:,1), usbllog(:,sonardyneStatus), dataDC(:,1) );

    PsiUsbl = interp1(dataNav(:,1), dataNav(:,mPsi), usbllog(:,1) );
    PsiUsbl= PsiUsbl( find(usbllog(:,sonardyneStatus)==1));

    posxUsbl= interp1(dataNav(:,1), posx, usbllog(:,1) );
    posxUsbl= posxUsbl(find(usbllog(:,sonardyneStatus)==1));

    posyUsbl= interp1(dataNav(:,1), posy, usbllog(:,1) );
    posyUsbl= posyUsbl(find(usbllog(:,sonardyneStatus)==1));
    maxRange = 3000;
    if( exist('targetEasting') == 1 )
      %
      % First plot the raw (from the Usbl) data:
      usblX  = usbllog( find(usbllog(:,sonardyneStatus)==1), sonardyneX );
      usblY  = usbllog( find(usbllog(:,sonardyneStatus)==1), sonardyneY );
      usblR  = usbllog( find(usbllog(:,sonardyneStatus)==1), sonardyneRange );
      usblTime= usbllog( find(usbllog(:,sonardyneStatus)==1), 1 );

      usblN  = usblX .* cos(PsiUsbl) - usblY .* sin(PsiUsbl) + posxUsbl; 
      usblE  = usblX .* sin(PsiUsbl) + usblY .* cos(PsiUsbl) + posyUsbl;

      usblE( find(abs(usblE)>maxRange) ) = NaN;
      usblN( find(abs(usblN)>maxRange) ) = NaN;
      usblR( find(abs(usblR)>maxRange) ) = NaN;
      %
      % Now plot the filtered data from Dynamic Control:
      targE  = dataDC( :,  targetEasting  ) - posy0;
      targN  = dataDC( :,  targetNorthing ) - posx0;
      if( exist('newEasting') ) 
	newE  = dataDC( :,  newEasting  ) - posy0;
	newN  = dataDC( :,  newNorthing ) - posx0;
      end
      %    
      %   Remove outliers:

      targE( find(abs(targE)>maxRange) ) = NaN;
      targN( find(abs(targN)>maxRange) ) = NaN;

      if( exist('newEasting') ) 
	newE( find(abs(newE)>maxRange) ) = NaN;
	newN( find(abs(newN)>maxRange) ) = NaN;
      end

      %'x','Color','k','markersize',7 );
      hold  on	
      plot( targE*distUnits, targN*distUnits,...
	  'linewidth', 2, 'linestyle','x','Color','k' );
	  %'linewidth', linewidth, 'linestyle','x','Color','k' );
      lstr(length(lstr)+1,1) = {'Median-Filtered Usbl'};
      if( 0 )
        %
        % Overplot the first point in red as it is usually the programmed beacon
        % location. 
        iindx=find( ~isnan(targE));
        if( ~isempty(iindx) )
          plot( targE(iindx(1))*distUnits, targN(iindx(1))*distUnits,...
                'linewidth', 2, 'linestyle','x','Color','r' );
          lstr(length(lstr)+1,1) = {'Beacon Location'};
        end
      end

      if( 0 )
      plot( newE*distUnits, newN*distUnits,...
	  'linewidth', linewidth, 'linestyle','+','Color','k' );
      end
      %
      %	line( usblE*distUnits, usblN*distUnits,...
      %	    'linewidth', linewidth, 'linestyle','o','Color','c' );
      %	lstr(length(lstr)+1,1) = {'Raw Usbl'};
      %
      % Assume that posxUsbl, posyUsbl, usblN and usblE are the same length. 
      npts = length(posxUsbl);
      fprintf(' Number of good hits:  %d\n', npts)
      %cax=[min(posxUsbl) max(posxUsbl)]; 
      beaconColor = 'range';
      beaconColor = 'hitNumber';
      if( strcmp( beaconColor, 'range' ) )
        cax=[min(usblR) max(usblR)]; 
        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:npts
        %[mn kk]=min(abs(rng-posxUsbl(i)));
        if( strcmp( beaconColor, 'range' ) ) [mn kk]=min(abs(rng-usblR(i)));
        elseif( strcmp( beaconColor, 'hitNumber' ) ) kk=npts-i+1;
        end
        plot(posyUsbl(i)*distUnits,posxUsbl(i)*distUnits,...
             'kd','color',cmp(kk,:),'markersize',7);
        plot(usblE(i)*distUnits,usblN(i)*distUnits,...
             'k+','color',cmp(kk,:),'markersize',7);
      end
      %
      % Plotting in a loop kills the labeling. So, 
      % THE ABOVE LOOP HAS TO COME LAST OR IT SCRAMBLES THE LEGENDS.
      %lstr(length(lstr)+1,1) = {'Vehicle Location'};
      %lstr(length(lstr)+1,1) = {'Raw Usbl'};
    end
  end
end 
% title(['Location in Inertial North-East-Down Frame;',...
%       'Initial UTM Coord North = ', num2str(posx0),' ',...
%       'East = ', num2str(posy0),' ', titleStr])

% title(strvcat(['Initial UTM Coord North = ', num2str(posx0),' ',...
%                'East = ', num2str(posy0)], ...
%               ['Dead Reckoning-Blue; GPS-Magenta; Sim-Green ',...
%               'Lbl filter-Cyan; Lbl fix-Black'],...
%               titleStr))

 if(~plotSwPublish) 
     titlehand=title(strvcat(['Initial UTM Coord:  North = ', num2str(posx0,'%.1f'),' ',...
                         'East = ', num2str(posy0,'%.1f')], titleStr)); 
     set(titlehand,'fontsize',16)
 end

%set( gca, 'DataAspectRatio', [1 1 .25] );
 axis equal;

%legend(lstr,'location','southeast')
%legend(lstr,'location','bestOutside')
 if(~plotSwPublish) legend(lstr,'location','best'), end
 zoom on;
 hold off;
 grid on
 

 % Next plot:
 
 
 if( (plotSwKft == 1) & (exist('dataK') == 1) )
 pause(.5)
 if exist( 'navTimeFig' ) ~= 1
     navTimeFig = figure;
     figure(navTimeFig);
 else
     figure(navTimeFig);
 end

 set( navTimeFig, 'Name', 'Position Versus Time', 'DeleteFcn', ...
		  'clear navTimeFig' );
 set(gcf,'position',npposition)

 fmtstr = '.';
 fmtstr = '-';
 
 subplot(4,1,1)
 plot( dataK(:,1), dataK(:,mLatK)*180/pi,fmtstr)
 ylabel('Latitude,Degrees')
 grid on
 title(['INS Position and Speed Versus Time', titleStr])
 
 subplot(4,1,2)
 plot( dataK(:,1), dataK(:,mLonK)*180/pi,fmtstr)
 ylabel('Longitude,Degrees')
 grid on

 subplot(4,1,3)
%
% For backward compatibility; 2004/12/1.
%
  if( exist('mVbodyxK') == 1 )
    mVnavxK = mVbodyxK;
    mVnavyK = mVbodyyK;
    mVnavzK = mVbodyzK;
  end
 magV = sqrt( dataK(:,mVnavxK).^2 + ...
	      dataK(:,mVnavyK).^2 + ...
	      dataK(:,mVnavzK).^2 );
 plot( dataK(:,1), magV,fmtstr)
 ylabel('Transport Speed, m/s')
 grid on
 
 subplot(4,1,4)
 plot( dataK(:,1), dataK(:,mMonK ),'color','m','linestyle',fmtstr)
 ylabel('INS Monitor Byte')
 grid on 
 xlabel('Seconds')

 figure(nav);
 
end
 return












if exist( 'do_3d_plot' ) ~= 1, do_3d_plot = 0; end
%if exist( 'm_pos_n' ) == 1
%    pos = [mPos_x mPos_y];
%elseif exist( 'm_lbl_est_north' ) == 1
%    pos = [m_lbl_est_north m_lbl_est_east];
%else
%    pos = [m_dr_north m_dr_east];
%end

    pos = [mPos_x mPos_y];

    posx0 = dataNav(1,pos(1));
    posy0 = dataNav(1,pos(2));

    posx  = dataNav(:,pos(1)) - posx0;
    posy  = dataNav(:,pos(2)) - posy0;

if exist('lbl_fixstat') == 1
    ind = find( data(:,lbl_fixstat) == 1 );
elseif exist('m_lbl_fixstat') == 1
    ind = find( data(:,m_lbl_fixstat) == 1 );
else
    ind = [];
end

legendStr = 'Dead Reckoning';

if do_3d_plot == 0

    plot( posy, posx, 'linewidth', linewidth );
%       data(:,m_dr_east), data(:,m_dr_north) );
%   title( ['Estimated Position (b), DR (g): ' titleStr] );
    hold on;
    if ~isempty(ind)
        plot( data(ind,m_lbl_eastfix), ...
            data(ind,m_lbl_northfix), 'ro' );
        Xb = o2_xpndr( start );
        o2_xpndr( Xb(:,1:2) );
    end
    if exist( 'c_wpt_e' ) == 1
        line( data(:,c_wpt_e), data(:,c_wpt_n), 'linewidth', linewidth );
    end
    if exist( 'dataSim' ) == 1
        line( dataSim(:,sPos_y)-posy0, ...
              dataSim(:,sPos_x)-posx0, ...
              'linewidth', linewidth, 'Color','g' );
        legendStr = [legendStr ,' Simulation Truth'];
    end
    if exist( 'gps' ) == 1 
        line( dataNav(   find(dataNav(:,mGpsValid)),  mGpsEast ) - posy0, ...
              dataNav(   find(dataNav(:,mGpsValid)),  mGpsNorth) - posx0, ...
              'linewidth', linewidth, 'linestyle','.','Color','m' );
        legendStr = [legendStr ,' GPS Fixes'];
    end

if 0
    wpts= [4067605 589268;
           4067655 589218;
           4067555 589218;
           4067605 589268];
    line( wpts(:,2),wpts(:,1),'color','r','linewidth', linewidth)
    legendStr = [legendStr ,' Trackline'];
end

    grid on
%    legend( legendStr );
%    legend('Deadreckoning','Simulation Truth','GPS Fixes','Trackline')
    legend('Deadreckoning','Simulation Truth','GPS Fixes')
%    legend('Dead Reckoned','Simulation Truth')
    title(['Location in Inertial North-East-Down Frame;',...
           'Initial UTM Coord North = ', num2str(posx0),' ',...
           'East = ', num2str(posy0),' ', titleStr])
else

    plot3( data(:,pos(2)), data(:,pos(1)), data(:,m_depth) );
    set( gca, 'ZDir', 'reverse' );

    if ~isempty(ind)
        Xb = o2_xpndr( start );
        o2_xpndr( Xb(:,1:2) );
    end
end

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

set( gca, 'DataAspectRatio', [1 1 .25] );

zoom on;
hold off;
