%
% PURPOSE: Plot LR power consumption from logged data.
%
% .time is in units of fractional days.
%
% Setup figure positions, etc.
  review;
%  

if exist( 'powerFigPtr' ) ~= 1
  powerFigPtr= figure;
  set( powerFigPtr, 'Name', 'LR Power', 'DeleteFcn', 'clear powerFigPtr' );
  set(gcf,'units','normalized')
  position = [0 0 .979 .935];
  set(gcf,'position',position)
  if(exist('fontsize')) 
    powerFontsize=fontsize;
  else
    powerFontsize=22;
  end
  powerFontweight='normal';
  set(gca,'fontsize',powerFontsize,'fontweigh',powerFontweight)
  set(gcf,'defaultaxesfontsize',powerFontsize,'defaulttextfontsize', ...
      powerFontsize)
else
  figure(powerFigPtr);
end

  indx = intersect(find(data.ThrusterHE.component_avgVoltage.time), find(data.ThrusterHE.component_avgCurrent.time));

  time0=data.ThrusterHE.component_avgCurrent.time(1);
  thrPower = data.ThrusterHE.component_avgCurrent.value(indx).*...
          data.ThrusterHE.component_avgVoltage.value(indx)/1e3;

%
  plot(data.ThrusterHE.component_avgCurrent.time(indx), thrPower,'.');
  hold on;
  grid on;
  
  fs = 1/2.4;
  [bbThr aaThr]= butter(4, .001/(fs/2));
  thrPower = double(thrPower);
  indx2 = find(isfinite(thrPower));
  %index into the original array of non-NaN, non-zero, and existing points.
  ttime = data.ThrusterHE.component_avgCurrent.time(indx2);
  filtThrPower = filtfilt(bbThr, aaThr, thrPower(indx2));
  plot(ttime, filtThrPower,'k','linewidth',3);
  str = sprintf('Mean Thruster Power = %.2f Watts.',mean(thrPower(indx2)));
  text((ttime(end)-ttime(1))*.75+ttime(1), 8, str);
  
  xlabel('Hours');
  ylabel('Watts');
  title([ 'LR Auv Power Consumption for Run  ', DirName, ' / ', filepath], 'interpreter','none');
  dynamicDateTicks
  if( ~isstruct(data.Onboard.MainBatteryVoltage) )
    legend('Thruster Power','Filtered Thruster');
    return
  end
  %
  
  %ppower = double(data.platform_average_power.value);
  
  ppower = abs(double(data.Onboard.MainBatteryVoltage.value .* data.Onboard.BatteryCurrent.value))/1000;

  plot(data.Onboard.MainBatteryVoltage.time, ppower,'r.');
  
  
  halfFs = (1/5)/2;
  [bbVeh aaVeh]= butter(4,.001/halfFs);
  

  pindx = find(isfinite(ppower));

  %ptime = data.platform_average_power.time;
  ptime = data.Onboard.MainBatteryVoltage.time;
  filtppower = filtfilt(bbVeh ,aaVeh ,ppower(pindx));
  plot(ptime(pindx), filtppower,'c--','linewidth',3);
  legend('Thruster Power','Filtered Thruster', 'Total Power','Filtered Total' );
  

return  
  
if exist( 'interpPowerFigPtr' ) ~= 1
  interpPowerFigPtr= figure;
  set( interpPowerFigPtr, 'Name', 'Interpolated LR Power', 'DeleteFcn', 'clear interpPowerFigPtr' );
  set(gcf,'position',position)
  if(exist('fontsize')) 
    powerFontsize=fontsize;
  else
    powerFontsize=22;
  end
  powerFontweight='normal';
  set(gca,'fontsize',powerFontsize,'fontweigh',powerFontweight)
  set(gcf,'defaultaxesfontsize',powerFontsize,'defaulttextfontsize', ...
      powerFontsize)
else
  figure(interpPowerFigPtr);
end

  %indx = find(isfinite(thrPower));

  
  thrPowerPtime = interp1( ...
      data.ThrusterHE.component_avgCurrent.time(indx), thrPower(indx), data.Onboard.MainBatteryVoltage.time);
  %   data.ThrusterHE.component_avgCurrent.time(indx), thrPower(indx), data.platform_average_power.time);
  %   data.ThrusterHE.component_avgCurrent.time(indx:end-3), thrPower(indx), data.platform_average_power.time);
  
  depthPtime = interp1( data.depth.time, data.depth.value, data.Onboard.MainBatteryVoltage.time);
  
  time = data.Onboard.MainBatteryVoltage.time;
  
  thrPowerPtime = double(thrPowerPtime);

  offSurfaceDepth = 4;
  
  %indxPtime = find(isfinite(thrPowerPtime) & isfinite(data.platform_average_power.value) & depthPtime > offSurfaceDepth);
  indxPtime = find(isfinite(thrPowerPtime) & isfinite(data.platform_average_power.value) & depthPtime > offSurfaceDepth);
  indxNotPtime = setxor(indxPtime,[1:length(time)]);
  
  
  %hotel = data.platform_average_power.value(indxPtime) - thrPowerPtime(indxPtime);
  hotel = data.platform_average_power.value - thrPowerPtime;

  plot(time(indxPtime), thrPowerPtime(indxPtime),'b.');
  hold on
  plot(time(indxPtime), data.platform_average_power.value(indxPtime),'r.');
  plot(time(indxPtime), hotel(indxPtime),'g.');

  pAvePower                   = data.platform_average_power.value;

  thrPowerPtime(indxNotPtime) = NaN(size(indxNotPtime));
  hotel(indxNotPtime)         = NaN(size(indxNotPtime));
  pAvePower(indxNotPtime)     = NaN(size(indxNotPtime));
  
  % plot(time, thrPowerPtime,'b.');
  % hold on
  % plot(time, data.platform_average_power.value,'r.');
  % plot(time, hotel,'g.');
  
  grid on;
  dynamicDateTicks;
  ylabel('Watts');
  xlabel('Hours');
  title([ 'Interpolated LR Auv Power Consumption; vehicle below' ...
        ,num2str(offSurfaceDepth),' m ; ', DirName, ' / ' ,filepath], 'interpreter','none');

  
  halfFsPtime = (1/60)/2;
  
  [bbPtime aaPtime]= butter(4,.001/halfFsPtime);
  
  hotel = double(hotel);
  data.platform_average_power.value = double(data.platform_average_power.value);

  filtThrPowerPtime = NaN(size(time));
  filtHotelPtime    = NaN(size(time));
  filtPPower        = NaN(size(time));
  
  
  filtThrPowerPtime(indxPtime) = filtfilt(bbPtime ,aaPtime ,thrPowerPtime(indxPtime));
  filtHotelPtime(indxPtime)    = filtfilt(bbPtime ,aaPtime ,hotel(indxPtime));
  filtPPower(indxPtime)        = filtfilt(bbPtime ,aaPtime ,data.platform_average_power.value(indxPtime));

  
  %plot(time(indxPtime), filtThrPowerPtime,'k')
  %plot(time(indxPtime), filtPPower,'r')
  %hotelHandle = plot(time(indxPtime), filtHotelPtime, 'g','linewidth',3);
  plot(time(indxPtime), filtThrPowerPtime(indxPtime),'k');
  plot(time(indxPtime), filtPPower(indxPtime),'r');
  hotelHandle = plot(time(indxPtime), filtHotelPtime(indxPtime), 'g','linewidth',3);
  str = sprintf('Mean Hotel = %.2f Watts.',mean(hotel(indxPtime)));
  %text(time(indxPtime(end)), 20, str);
  set(gca,'xlim',time(indxPtime([1 end])))
  grid on;
  dynamicDateTicks;
  ylabel('Watts');
  xlabel('Hours');

%  plot(SpeedControl.speedCmd.time, SpeedControl.speedCmd.value*5,'b') 
%  legend('Thruster Power','Total Power','Hotel','Filtered Thruster',...
%      'Filtered Total','Filtered Hotel','5xCommanded Speed' );

% plot(SpeedControl.speedCmd.time, SpeedControl.speedCmd.value*5,'b') 
  thrusterStr = sprintf('Thruster Power. Mean = %.2f Watts.', mean(thrPowerPtime(indxPtime)));
  totalStr = sprintf('Total Power. Mean = %.2f Watts.', mean(pAvePower(indxPtime)));
  hotelStr = sprintf('Hotel Power. Mean = %.2f Watts.', mean(hotel(indxPtime)));
  
  legend( thrusterStr,totalStr,hotelStr,'Filtered Thruster',...
      'Filtered Total','Filtered Hotel');
  
%  legend('Thruster Power','Total Power','Hotel','Filtered Thruster',...
%      'Filtered Total','Filtered Hotel');


if exist( 'TsFigPtr' ) ~= 1
  TsFigPtr= figure;
  set( TsFigPtr, 'Name', 'LR Ts', 'DeleteFcn', 'clear TsFigPtr' );
  set(gcf,'position',position)
  if(exist('fontsize')) 
    TsFontsize=fontsize;
  else
    TsFontsize=22;
  end
  TsFontweight='normal';
  set(gca,'fontsize',TsFontsize,'fontweigh',TsFontweight)
  set(gcf,'defaultaxesfontsize',TsFontsize,'defaulttextfontsize', ...
      TsFontsize)
else
  figure(TsFigPtr);
end

  subplot(2,1,1)
  plot(data.ThrusterHE.component_avgCurrent.time(2:end), ...
      diff(data.ThrusterHE.component_avgCurrent.time)*86400,'.');
  grid on;
  dynamicDateTicks;
  hold on;
  plot(data.depth.time(2:end),  diff(data.depth.time)*86400,'r.')
  ylabel('Thruster, Depth, Seconds');
  xlabel('Hours');
  title([ 'Sampling Rates  ', filepath], 'interpreter','none');
  set(gca,'ylim',[.1 3]);
  legend('Thruster','Depth')
%  
  subplot(2,1,2)
  plot(data.platform_average_power.time(2:end), diff(data.platform_average_power.time)*86400,'.');
%  
  grid on;
  dynamicDateTicks;
  hold on;
  ylabel('Pltfm Average Power, Seconds');
  xlabel('Hours');
  
  figure(interpPowerFigPtr);
