%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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) );
 if( exist('dataNav')==1 )
   fNorth      = filtfilt(num, den, usblr(indxAry,vehN)-posx0 );
   fEast       = filtfilt(num, den, usblr(indxAry,vehE)-posy0 );
 else
   fNorth      = filtfilt(num, den, usblr(indxAry,vehN)-usblr(1,vehN) );
   fEast       = filtfilt(num, den, usblr(indxAry,vehE)-usblr(1,vehE) );
 end
%
% 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 the nav data already on the stack from the previous mission.
%
 if( exist('dataNav')==1 )
   plot( posy, posx, '.','linewidth', 2 );
   line(usblr(indxAry,vehE)-posy0, usblr(indxAry,vehN)-posx0,'linestyle','+');
   line(usblr(indxAry,shipE)-posy0, ...
        usblr(indxAry,shipN)-posx0, 'Color','g');
 else
   plot(usblr(indxAry,vehE)-usblr(1,vehE), ...
        usblr(indxAry,vehN)-usblr(1,vehN), '.');
   line(usblr(indxAry,shipE)-usblr(1,shipE), ...
        usblr(indxAry,shipN)-usblr(1,shipN), 'Color','g');
 end

 line(fEast,           fNorth,          'Color', 'r');

 if exist( 'mGpsValid' ) == 1
   line( dataNav(   find(dataNav(:,mGpsValid)),  mGpsEast ) - posy0, ...
         dataNav(   find(dataNav(:,mGpsValid)),  mGpsNorth) - posx0, ...
         'linewidth', 2, 'linestyle','x','Color','m' );
 end

 if exist( 'dataWP' ) == 1
   line( dataWP(:,3)-posy0, dataWP(:,2)-posx0 ,'color','r','linewidth',2)
   %legendStr = [legendStr ,' Trackline'];
 end

 if exist( 'lblgrid' ) == 1
   line( lblgrid(eastIndx,:)-posy0, lblgrid(northIndx,:)-posx0,...
         'linestyle','x','color','r','linewidth',2)
   for i=1:size(lblgrid,2)
   text( lblgrid(eastIndx,i)-posy0, lblgrid(northIndx,i)-posx0,...
         [' B',num2str(i)] )
   end
   %legendStr = [legendStr ,' Trackline'];
 end
%
% Plot estimated position from lbl
%
 lblscale = 50;
 if ( exist('filter_north') & ...
      norm(dataNav(:,filter_north)) ~= 0),

   line(dataNav(:,filter_east)  - posy0,...
        dataNav(:,filter_north) - posx0,...
        'linewidth', 2, 'Color','c' );

   line(dataNav(find(dataNav(:,efix) ~= 0),efix)  - posy0,...
        dataNav(find(dataNav(:,nfix) ~= 0),nfix)  - posx0,...
        'linewidth', 2, 'linestyle','x','Color','k' );
   %
   % Plot the velocity of the water wrto the bottom:
   %
   fixIndx = find(dataNav(:,efix) ~= 0);
   for i=1:length(fixIndx);
     epos = dataNav(fixIndx(i),filter_east ) - posy0;
     npos = dataNav(fixIndx(i),filter_north) - posx0;
     evel = epos + lblscale*dataNav(fixIndx(i),east_current)
     nvel = npos + lblscale*dataNav(fixIndx(i),north_current)
     line([epos;evel],[npos;nvel],'color','r');
   end
 end

 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)
 if( exist('dataNav')==1 )
   plot(ttusbl-ttusbl(1), usblr(indxAry,vehE)-posy0, '.');
   line(ttusbl-ttusbl(1), fEast,  'Color', 'r');
   line(ttusbl-ttusbl(1), usblr(indxAry,shipE)-posy0,  'Color', 'g');
 else
   plot(ttusbl-ttusbl(1), usblr(indxAry,vehE)-usblr(1,vehE), '.');
   nline(ttusbl-ttusbl(1), fEast,  'Color', 'r');
   line(ttusbl-ttusbl(1), usblr(indxAry,shipE)-usblr(1,shipE),  'Color', 'g');
 end

 ylabel('East, Meters')
 title([' Vehicle Location East in Relative UTM as Measured by USBL/Winfrog ',...
        titleStr])
 grid on
 zoom on

 subplot(3,1,2)
 if( exist('dataNav')==1 )
   plot(ttusbl-ttusbl(1), usblr(indxAry,vehN)-posx0, '.');
   line(ttusbl-ttusbl(1), fNorth,  'Color', 'r');
   line(ttusbl-ttusbl(1), usblr(indxAry,shipN)-posx0,  'Color', 'g');
 else
   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');
 end

 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), ...
      (usblr(indxAry,shipH)-usblr(1,shipH)));
%     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

