%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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 (  (exist('usblTime') ~= 1) | (exist('usbl') ~= 1)  ) , return, end
  
  if( exist('tusbl') ~=1 )
    sprintf(' Begin time conversion.  Please wait...\n')
%   tusbl = (datenum(usblTime) - datenum(usblTime(1,:)))*24*3600;
    tusbl = (datenum(usblTime) )*24*3600;
    sprintf(' End time conversion\n')
  end

%
% Hand code the indirect addressing of the columns in usbl.
%
 shipN = 1;
 shipE = 2;
 shipH = 3;
 vehN  = 4;
 vehE  = 5;

 tusblr = tusbl;
 usblr  = usbl;

 if exist('tmin') == 1
   indx   = find( tusbl-tusbl(1) > tmin );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 end

 if exist('tmax') == 1
   indx   = find( tusbl-tusbl(1) < tmax );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 end

 if (  (exist('tmin') == 1)   &   (exist('tmax') == 1 )  )
   indx   = find(  (tusbl-tusbl(1) > tmin)  &  (tusbl-tusbl(1) < tmax)  );
   tusblr = tusbl(indx);
   usblr  = usbl(indx,:);
 end
 clear tmin tmax indx

%
% 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

 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

