%
%    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.1 2000/01/10 19:08:12 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' );

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];

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

if do_3d_plot == 0

    plot( dataNav(:,pos(2)), dataNav(:,pos(1)), 'linewidth', 3 );
%	data(:,m_dr_east), data(:,m_dr_north) );
%   title( ['Estimated Position (b), DR (g): ' run_name] );
    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', 3 );
    end

    if exist( 'dataSim' ) == 1
	line( dataSim(:,sPos_y), dataSim(:,sPos_x), ...
              'linewidth', 3, 'Color','g' );
%       legendStr = [legendStr ', Simulation Truth'];
    end
    
    grid on
%    legend( legendStr );
    legend('Dead Reckoned','Simulation Truth')
    title(['Location in Inertial North-East-Down Frame' run_name])
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;
