%
%    NAVPLOT - script to plot navigation data in one large window.
%
%    MIT SeaGrant Lab

%    Copyright (C) 1995-1998
%    Massachussetts Institute of Technology
%    Sea Grant AUV Laboratory
%    $Id: navplot.m,v 1.6 2000/06/21 21:25:54 rob Exp $	

 if exist( 'h_nav' ) ~= 1
     h_nav = figure;
     winpos( position(1,:) );
 else
     figure(h_nav);
 end

 set( h_nav, 'Name', 'Navigation Plot', 'DeleteFcn', 'clear h_nav' );

 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( '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 exist( 'dataWP' ) == 1
   line( dataWP(:,3)-posy0, dataWP(:,2)-posx0 ,'color','r','linewidth',2)
   %legendStr = [legendStr ,' Trackline'];
 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('Location in Inertial North-East-Down Frame;',...
	       ['Initial UTM Coord North = ', num2str(posx0),' ',...
		'East = ', num2str(posy0)], ...
	       'Dead Reckoning-Blue; GPS-Magenta; Sim-Green',...
	       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;
