%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot Kearfott and Winfrog navigation data.
% AUTHOR:  Rob McEwen
% DATE:    03/3/6
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
  clear filter_north;  %Prevent it from trying to plot Lbl data.
  clear mGpsValid;     %Prevent it from trying to plot Gps data.
  eval(['cd ', pathNames(pathIndx).missions{1}]) 
  pwd
  navplot
  figure(lbl_nav);
  fontsize = 10;
  fontsize = 14;
  linewidth = 1.5;
  set(gca,'fontsize',fontsize);
  if( loadDvl )
    line( dvlDist(:,2)*m2km, dvlDist(:,1)*m2km, 'color','k','linewidth',linewidth)
    lstr(length(lstr)+1,1) = {'Dvl Dead Reckoning'};
  end
%
% Title for the Nav Paper:
%
  if( pathIndx == ThreeYoyosPNIndx ) 
    title([' Data Set: 2001.297.03-05']);
    lstr(1) = {'INS/DVL Real-Time Posn'};
  end
%
% OK, here's the scoop.  Mstart and Mend are the relative start and stop times
% (startTime subtracted off) of each Kearfott data file.  This code assumes these 
% include (are a super set of) the in-water missions.  startTime is the QNX time
% that the first in-water mission started.  So, if there is pre-deployment
% Kearfott deck data, loadNavData will assign it negative (relative) time values.
%
% MstartIndx and MendIndx are the indices of the start and stop of each Kearfott
% data segment in the concatenated array.  Remember that each segement is
% separated by two NaNs. 
%
% poleDown is an array where 1 means in-water mission, and 0 means deck data.
%
% Zone is an array of the utm zone at the start of each mission.
%
% *ALL* of these arrays should have dimension equal to the number of Kearfott
% log files.  This should also be the same as the number of elements in 
% pathNames(pathIndx).missions
%
% First, find the UTM zone of the first mission, and plot all of the data in
% that zone. 
%
  zone = utmzone(dataK(1,mLatK)/d2r, dataK(1,mLonK)/d2r)
% axesm('utm','zone',zone);
  mstruct = defaultm('utm');
  mstruct.zone = zone;
  mstruct  = defaultm(mstruct);
%
% This indexes all of the arrays above.
%
  deckIndx = find(poleDown == 0);  
%
% Plot Kearfott pre and post-mission data.  
%
  clmap = colormap;
  for( i=1:length(deckIndx) )
    indx = [MstartIndx(deckIndx(i)):MendIndx(deckIndx(i))];
   %
   % Plot all missions in the zone of the first (in-water) mission.  Notify
   % the user if the zone changes.
   %
   [easting northing]=mfwdtran(mstruct,...
     dataK(indx, mLatK)/d2r, dataK(indx, mLonK)/d2r);
%  line( easting-posy0, northing-posx0, 'color',clmap(20,:),'linewidth',linewidth)
%  lstr(length(lstr)+1,1) = {'Keafott on Deck'};
   gpsIndx = find( bitand( dataK(indx, mMonK), 2^6) );
%
%  If indx is empty, the corresponding legend doesn't print, thus
%  offsetting all the markers by one.  So, add at least one point.
%
   if( ~isempty(gpsIndx) & 0) 
     line( (easting(gpsIndx)-posy0)*m2km, (northing(gpsIndx)-posx0)*m2km, ...
	 'color','m','linewidth',linewidth,...
	 'linestyle','none','marker','x')
     lstr(length(lstr)+1,1) = {'INS/DVL on Deck w/GPS'};
   end
   line( (easting-posy0)*m2km, (northing-posx0)*m2km, 'color','m',...
       'linewidth',linewidth)
   lstr(length(lstr)+1,1) = {'Vehicle on Deck'};
 end
%
% Plot Don's renav data for the 297.03 set:
%
 if( exist( 'posEst' ) == 1 & exist('utmzone') == 2 )
   zone = utmzone(posEst(1,mLatD), posEst(1,mLonD))    %Units of degrees
%  axesm('utm','zone',zone);
   mstruct = defaultm('utm');
   mstruct.zone = zone;
   mstruct.geoid = almanac('earth','geoid','m','grs80');
   mstruct  = defaultm(mstruct);
   [easting northing]=mfwdtran(mstruct,...
       posEst(:,mLatD), posEst(:,mLonD));              %In Degrees
   clmap = colormap;
   %80,100 apparent from first plot.  I don't know what causes this error.
   %line( (easting-posy0-80)*m2km, (northing-posx0-100)*m2km, 'color',clmap(30,:),...
   if 0
   line( (easting-posy0-80)*m2km, (northing-posx0-100)*m2km, 'color','g',...
       'linewidth',linewidth)
   end
   line( (easting-posy0)*m2km, (northing-posx0)*m2km, 'color','g',...
       'linewidth',linewidth)
   lstr(length(lstr)+1,1) = {'Post-Processed'};
 end
%
% Ship's position at the start and end of each mission:
%
  markerArray = ['+','o','*','x','s','d','^','v','>','<','p','h'];
  cmap = colormap;
  
  for( i = 1:length(tusblrStartIndx) )
    if( tusblrStartIndx(i) > 0 )
      j = mod(2*i-1, length(markerArray)); %Don't overrun the marker array indx
      line( (usblr(tusblrStartIndx(i),shipE)-posy0)*m2km,...
	    (usblr(tusblrStartIndx(i),shipN)-posx0)*m2km,...
	   'linestyle','none','marker',markerArray(j),...
	   'color','g','linewidth',linewidth);
      lstr(length(lstr)+1,1) = {['Mission ',num2str(i), ' Start']};
    end
    if( tusblrEndIndx(i) > 0 )
      j = mod(2*i, length(markerArray));  %Don't overrun the marker array indx
      if( j== 0 ) j=12;end
      line( (usblr(tusblrEndIndx(i),shipE)-posy0)*m2km,...
	    (usblr(tusblrEndIndx(i),shipN)-posx0)*m2km,...
	   'linestyle','none','marker',markerArray(j),...
	   'color','r','linewidth',linewidth);
      lstr(length(lstr)+1,1) = {['Mission ',num2str(i), ' End']};
    end
    %
    % Plot Usbl data. First 900, or all of it:
    %
    if( strcmp(pathNames(pathIndx).plotusbl, 'none' ))
      indx1 = [];
    elseif( strcmp(pathNames(pathIndx).plotusbl, 'begin' ))
      indx1 = find(  tusblr - startTime >  Mstart(i) ...
	           & tusblr - startTime <= Mstart(i) + 900 );
    elseif( strcmp(pathNames(pathIndx).plotusbl, 'all' ))
      indx1 = find(  tusblr - startTime >  Mstart(i) ...
	           & tusblr - startTime <= Mend(i));
    else
      disp(sprintf(' Unrecognized Usbl plot option, %s.\n', ...
	  pathNames(pathIndx).plotusbl));
    end
    %disp(sprintf(' indx1 has length %d\n', length(indx1)));	     
    %
    % Eliminate outliers:
    %
    indx2 = find( abs(usblr(:,vehN)-posx0) < 40000 );
    indx = intersect(indx1,indx2);
    %disp(sprintf(' indx has length %d\n', length(indx)));	     
    if( ~isempty(indx) & poleDown(i) )		
      j=mod(10*i, length(cmap) );   %index into cmap below
      line( (usblr(indx,vehE)-posy0)*m2km, (usblr(indx,vehN)-posx0)*m2km,...
	  'linestyle','.','linewidth',linewidth,'color',cmap(j,:))
      lstr(length(lstr)+1,1) = {['Mission ', num2str(i), ' Usbl']};
    end
  end
  legend(lstr,-1)  %to the right of the plot
  %legend(lstr,0)
  zoom on;

  return

  
  
  
  
  
  
  
  
  
  
  
  
% Mark mission start and stop times on the ship's track.  Since
% Winfrog was started after the first mission, we'll begin with the ship's
% position when the vehicle was sighted after the first mission.
%
  qnxtime = (   datenum('24-Oct-2001 12:18:30') ...
              - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxtime+2);
  figure(lbl_nav);
  line(usblr(indx(1),shipE)-posy0,...
       usblr(indx(1),shipN)-posx0,'linestyle','x','color','r','linewidth',2)
   lstr(length(lstr)+1,1) = {'1^{st} Mission End'};
%
% Ship's position at the start of the second mission:
%
  qnxtime = (   datenum('24-Oct-2001 13:26:30') ...
              - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxtime+2);
  figure(lbl_nav);
  line(usblr(indx(1),shipE)-posy0,...
       usblr(indx(1),shipN)-posx0,'linestyle','x','color','c','linewidth',2)
  lstr(length(lstr)+1,1) = {'2^{nd} Mission Start'};
%
% Plot Usbl data:
%
  qnxend = (   datenum('24-Oct-2001 13:39:30') ...
             - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxend);
  line(usblr(indx,vehE)-posy0, usblr(indx,vehN)-posx0, ...
      'linestyle','.','color','k')
  lstr(length(lstr)+1,1) = {'2^{nd} Mission Usbl'};
%
% Ship's position when the vehicle was sighted after the second mission:
%
   qnxtime = (   datenum('24-Oct-2001 14:59:00') ...
              - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxtime+2);
  figure(lbl_nav);
  line(usblr(indx(1),shipE)-posy0,...
       usblr(indx(1),shipN)-posx0,'linestyle','x','color','r','linewidth',2)
  lstr(length(lstr)+1,1) = {'2^{nd} Mission End'};
%
% Ship's position at the start of the third mission:
%
   qnxtime = (   datenum('24-Oct-2001 14:59:41') ...
              - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxtime+2);
  figure(lbl_nav);
  line(usblr(indx(1),shipE)-posy0,...
       usblr(indx(1),shipN)-posx0,'linestyle','x','color','c','linewidth',2)
  lstr(length(lstr)+1,1) = {'3^{rd} Mission Start'};
%
% Plot Usbl data:
%
  qnxend = (   datenum('24-Oct-2001 15:12:00') ...
             - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxend);
  line(usblr(indx,vehE)-posy0, usblr(indx,vehN)-posx0, ...
      'linestyle','.','color','m')
  lstr(length(lstr)+1,1) = {'3^{rd} Mission Usbl'};
%
% Ship's position when the vehicle was sighted after the third mission:
%
   qnxtime = (   datenum('24-Oct-2001 16:10:45') ...
              - datenum('1-Jan-1970 00:00:00' )  )*24*3600;
  indx = find(tusblr >= qnxtime & tusblr <= qnxtime+2);
  if isempty(indx) disp(sprintf(' Not Found.\n'));end
  figure(lbl_nav);
  line(usblr(indx(1),shipE)-posy0,...
       usblr(indx(1),shipN)-posx0,'linestyle','x','color','r','linewidth',2)
  lstr(length(lstr)+1,1) = {'3^{rd} Mission End'};
   
  legend(lstr,-1)
%
% Convert from dataX(1,1) to GMT:
%  
% datestr(  (dataKEnd(1,1) + startTime 
%            + datenum('1-Jan-1970 00:00:00')*86400)/86400  )
  
  
  
  return   
   
   
   
% Plot the vehicle position
%
%figure(3);
%line(515185-posy0, 8971376-posx0,'linestyle','x','color','r','linewidth',2)
%
% Plot usbl data
%
figure(3);
line(usbl(8700:9400,7)-posy0,usbl(8700:9400,6)-posx0,'linestyle','x')



 %2001.297.03 - the unattainable waypoint
 line(523294-posy0, 8984074-posx0,'linestyle','x','color','r','linewidth',2)
 lstr(length(lstr)+1,1) = {'Waypoint'};
