%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot vehicle position in UTM.
% AUTHOR:  rsm
% DATE:    00/8/20
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% Check for LBL data:
%
% if exist('nfix') ~= 1
%   return
% end

 if exist( 'lbl_nav' ) ~= 1
     lbl_nav = figure;
 else
     figure(lbl_nav);
 end

 set( lbl_nav, 'Name', 'LBL Navigation Plot', 'DeleteFcn', 'clear lbl_nav' );
 set(gcf,'position',position)

 legendStr = ' ''Dead Reckoning'' ';

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

 posx  = dataNav(:,mPos_x) - posx0;
 posy  = dataNav(:,mPos_y) - posy0;
 
 plot( posy, posx, '.','linewidth', 2 );
 
 if exist( 'dataSim' ) == 1
   line( dataSim(:,sPos_y)-posy0, ...
	 dataSim(:,sPos_x)-posx0, ...
	 'linewidth', 2, 'Color','g' );
 legendStr = [legendStr ,', '' Simulation Truth'' '];
 end
 
 if exist( 'mGpsValid' ) == 1
   line( dataNav(   find(dataNav(:,mGpsValid)),  mGpsEast ) - posy0, ...
	 dataNav(   find(dataNav(:,mGpsValid)),  mGpsNorth) - posx0, ...
	 'linewidth', 2, 'linestyle','x','Color','m' );
 legendStr = [legendStr ,', '' GPS Fixes'' '];
 end
 
 if exist( 'dataWP' ) == 1
   line( dataWP(:,3)-posy0, dataWP(:,2)-posx0 ,'color','r','linewidth',2)
   %legendStr = [legendStr ,' Trackline'];
 end

 if exist( 'lblgrid' ) == 1
   line( lblgrid(eastIndx,:)-posy0, lblgrid(northIndx,:)-posx0,...
	 'linestyle','x','color','r','linewidth',2)
   for i=1:size(lblgrid,2)
   text( lblgrid(eastIndx,i)-posy0, lblgrid(northIndx,i)-posx0,...
	 [' B',num2str(i)] )
   end	  
   %legendStr = [legendStr ,' Trackline'];
 end
%
% Plot estimated position from lbl
%
 lblscale = 50;
 if ( exist('filter_north') & ...
      norm(dataNav(:,filter_north)) ~= 0),
   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', 2, 'Color','c' );
   line(dataNav(:,filter_east)  - dataNav(1,filter_east),...
	dataNav(:,filter_north) - dataNav(1,filter_north),...
	'linewidth', 2, 'Color','c' );

   line(dataNav(find(dataNav(:,efix) ~= 0),efix)  - posy0,...
	dataNav(find(dataNav(:,nfix) ~= 0),nfix)  - posx0,...
	'linewidth', 2, '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],[npos;nvel],'color','r');
   end
 end
%
% Dang. I SURE wish I could make this work.
% legend(legendStr)
%

% 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))

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

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

 zoom on;
 hold off;
 grid on
 
 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', 2 );
%	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', 2 );
    end
    if exist( 'dataSim' ) == 1
	line( dataSim(:,sPos_y)-posy0, ...
	      dataSim(:,sPos_x)-posx0, ...
              'linewidth', 2, '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', 2, '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',2)
    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;
