%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Deal with Winfrog USBL data
% AUTHOR:  Rob McEwen
% DATE:    99/12/20
% NOTES:   See the companion shell script cvtwf
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
 global titleStr;
 global vehicleName;
%
% If the data isn't on the stack, and does exist in the local directory, 
% load it.
%
  if ( (exist('usblTime') ~= 1) | (exist('usbl') ~= 1) ) , 
    if ( (exist('dataUsblTime') == 2) & (exist('dataUsbl') == 2) ) , 
      dataUsblTime;
      dataUsbl;
    else
      sprintf([' The Usbl data is not on the stack, '...
	    'and not in this directory.\n'])
      return
    end
  end

  if( exist('tusbl') ~=1 )
    disp(sprintf(' Begin time conversion.  Please wait...\n'))
%   tusbl = (datenum(usblTime) - datenum(usblTime(1,:)))*24*3600;
%   tusbl = (datenum(usblTime) )*24*3600;
%
%   This will compute seconds since 1 Jan 1970, to  match the
%   vehicle's QNX OS.  Matlab's datenum converts a date string to fractional
%   days since 1 Jan 0000, at 00:00:00.
%
    tusbl = (datenum(usblTime) - datenum('1-Jan-1970 00:00:00'))*24*3600;
    disp(sprintf(' End time conversion\n'))
  end

 if( exist('shipN') ~= 1 )
   %
   % NOTE: 03/3/14: If shipN already exists, assume that the Winfrog data was
   % parsed and read by loadNavData.  Otherwise, assume it was parsed by cvtwfq.
   %
   % Hand code the indirect addressing of the columns in usbl.  These 
   % columns correspond to the 'usbl' created by cvtwfq.  The 'usbl' created
   % in loadNavData has different column numbers.
   %
   shipN = 1;
   shipE = 2;
   shipH = 3;
   vehN  = 6;  %For the arctic
   vehE  = 7;
 end
%
% In case the user doesn't cut out a subset
%
 tusblr = tusbl;
 usblr  = usbl;

 %
 % Beware that the usbl data time vector doesn't have startTime
 % subtracted off as the others do, in case cvtwf is run stand-alone.
 %
 if (  (exist('tmin') == 1)   &   (exist('tmax') == 1 )  )
   disp(sprintf([' Synchronizing Usbl data start and stop times with ',...
	 'tmin & tmax.\n']))
   indx   = find(  (tusbl-tusbl(1) > tmin)  &  (tusbl-tusbl(1) < tmax)  );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 elseif exist('tmin') == 1
   disp(sprintf(' Synchronizing Usbl data start and stop times with tmin.\n'))
   indx   = find( tusbl-tusbl(1) > tmin );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 elseif exist('tmax') == 1
   disp(sprintf(' Synchronizing Usbl data start and stop times with tmax.\n'))
   indx   = find( tusbl-tusbl(1) < tmax );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 elseif( exist('startTime') == 1 & exist('dataNav') == 1 )
   disp(sprintf(' Synchronizing Usbl data start and stop times with dataN.\n'))
   indx   = find(  (tusbl > startTime)  &  ...
                   (tusbl < (dataNav(end,1)+startTime)  ) );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 end
 clear tmin tmax %indx

return 
%
% Massage the time vector for awhile.  Remove double points; take the
% back-difference.
%
 dtusbl  = diff(tusblr);
 indxAry = find(dtusbl ~= 0);
 dtusbl  = dtusbl(indxAry);    %Back-diff of time with the zeros chopped out.
 ttusbl  = tusblr(indxAry);    %Time array with the double points chopped out.

%
% Filter the noisy USBL data:
%
 filterOrder = 4;
 if length(usblr(indxAry,1)) < 6
   disp(' Insufficient Data in dataUsbl.')
   return
 elseif length(usblr(indxAry,1)) < 9
   filterOrder = 2
 elseif length(usblr(indxAry,1)) < 12
   filterOrder = 3
 end

 normCorner  = .2;             %Corner freq is Ws/5
 normCorner  = .1;
 normCorner  = .01;
 Ts          = 1.5;            %Sampling period, seconds.
 [num, den]  = butter(filterOrder, normCorner);
 lowpass     = tf(num, den, Ts);
%[fNorth,tt] = lsim(lowpass, usblr(indxAry,vehN)-usblr(1,vehN) );
%[fEast,tt]  = lsim(lowpass, usblr(indxAry,vehE)-usblr(1,vehE) );
 fNorth      = filtfilt(num, den, usblr(indxAry,vehN)-usblr(1,vehN) );
 fEast       = filtfilt(num, den, usblr(indxAry,vehE)-usblr(1,vehE) );
%
% Back-difference the filtered data to get speed
%
 mag     = sqrt( fNorth.^2 + fEast.^2 );
 dmag    = abs(diff(mag));
 speed = dmag./dtusbl(1:length(dtusbl)-1);

 aveSpeed = mean(speed);
 sprintf('The average speed is %.2f\n', aveSpeed)
 sprintf('The standard deviation is %.2f\n', std(speed))
 ave = dmag'*ones(size(dmag))/(ttusbl(length(ttusbl)) - ttusbl(1));
 sprintf('The overall average speed is %.2f\n', ave)


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

 set( h_speed, 'name', [vehicleName,' Speed'], 'DeleteFcn', 'clear h_speed' );

 plot(ttusbl(1:length(ttusbl)-1)-ttusbl(1), ...
      [speed aveSpeed*ones(size(speed))]);
 ylabel('Meters/Second')
 xlabel('Seconds, UTC')
 title(strvcat(['Vehicle Speed Measured by USBL',...
               ' Speed = ', num2str(aveSpeed), ' meters/sec; '...
               ' Std Dev = ', num2str(std(speed)), ' meters/sec '],...
               ['Initial UTM Coords; East ' num2str(usbl(1,vehE)),...
                ' North ', num2str(usbl(1,vehN)),...
                titleStr]))
 grid on
 zoom on

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

 set( h_location, 'name', [vehicleName,' Location'], 'DeleteFcn', ...
      'clear h_location' );

 plot(usblr(indxAry,vehE)-usblr(1,vehE), usblr(indxAry,vehN)-usblr(1,vehN), '.');
 line(fEast,           fNorth,          'Color', 'r');
 line(usblr(indxAry,shipE)-usblr(1,shipE), ...
      usblr(indxAry,shipN)-usblr(1,shipN), 'Color','g');
 ylabel('North, Meters')
 xlabel('East, Meters')
 title(...
 strvcat([' Vehicle Location in Relative UTM as Measured by USBL/Winfrog ',...
          '; Green is the Ship'],...
         ['Initial UTM Coords; East ' num2str(usbl(1,vehE)),...
          ' North ', num2str(usbl(1,vehN)),...
          titleStr]))
 grid on
 zoom on
 axis equal
 
 if exist( 'h_tloc' ) ~= 1
     h_tloc = figure;
     winpos(position(1,:));
 else
     figure( h_tloc );
 end

 set( h_tloc, 'name', [vehicleName,' Location'], 'DeleteFcn', ...
      'clear h_tloc' );

 subplot(3,1,1)
 plot(ttusbl-ttusbl(1), usblr(indxAry,vehE)-usblr(1,vehE), '.');
 line(ttusbl-ttusbl(1), fEast,  'Color', 'r');
 line(ttusbl-ttusbl(1), usblr(indxAry,shipE)-usblr(1,shipE),  'Color', 'g');
 ylabel('East, Meters')
 title([' Vehicle Location East in Relative UTM as Measured by USBL/Winfrog ',...
        titleStr])
 grid on
 zoom on

 subplot(3,1,2)
 plot(ttusbl-ttusbl(1), usblr(indxAry,vehN)-usblr(1,vehN), '.');
 line(ttusbl-ttusbl(1), fNorth,  'Color', 'r');
 line(ttusbl-ttusbl(1), usblr(indxAry,shipN)-usblr(1,shipN),  'Color', 'g');
 ylabel('North, Meters')
 title([' Vehicle Location North in Relative UTM as Measured by USBL/Winfrog ',...
        titleStr])
 grid on
 zoom on

 subplot(3,1,3)
 plot(ttusbl-ttusbl(1), ...
      unwrap((usblr(indxAry,shipH)-usblr(1,shipH))*pi/180)*180/pi);
 ylabel('Degrees')
 xlabel('Time, Seconds')
 title([' Ship''s Heading from Winfrog ',titleStr])
 grid on
 zoom on

