%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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;
     figure(lbl_nav);
 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, 'Color','m' );
%        '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

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',position)
 
 subplot(3,1,1)
 plot( dataK(:,1), dataK(:,mLatK)*180/pi)
 ylabel('Latitude,Degrees')
 grid on
 title('INS Position and Speed Versus Time')
 
 subplot(3,1,2)
 plot( dataK(:,1), dataK(:,mLonK)*180/pi)
 ylabel('Longitude,Degrees')
 grid on

 subplot(3,1,3)
 magV = sqrt( dataK(:,mVbodyxK).^2 + ...
	      dataK(:,mVbodyyK).^2 + ...
	      dataK(:,mVbodyzK).^2 );
 plot( dataK(:,1), magV)
 ylabel('Transport Speed, m/s')
 grid on
 xlabel('Seconds')

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', 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;
