%
% PURPOSE: Plot horizontal plane data.
%
% LOG:     22 Feb 2021 rsm.
%
lstr={};
lrDatPtr=[];
lrUsblPtr=[];
%global titleStr;
titleStr = [' Data Set: ', replace(DirName2,'_','-'), ' / ', replace(filepath,'_','-')];
% if exist( 'position' ) ~= 1
%   MonitorPositions = get(0,'MonitorPositions');
%   position = MonitorPositions(end,:);
%   npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
%   npposition(3:4) = round([position(3)*.95 position(4)*.90]);
%   position = npposition;
% end

position = [0 0 .979 .935];


if exist( 'lrVplaneFigPtr' ) ~= 1
  lrVplaneFigPtr= figure;
  set( lrVplaneFigPtr, 'Name', 'LR Vertical Plane', 'DeleteFcn', 'clear lrVplaneFigPtr' );
  set(gcf,'units','normalized')
  set(gcf,'position',position)
  if(exist('fontsize1')) 
    lrNavFontsize=fontsize1;
  else
    lrNavFontsize=20;
  end
  lrNavFontweight='normal';
  set(gca,'fontsize',lrNavFontsize,'fontweigh',lrNavFontweight)
  set(gcf,'defaultaxesfontsize',lrNavFontsize,'defaulttextfontsize', ...
      lrNavFontsize)
  lrNavAxesPtr = gca;
else
 figure(lrVplaneFigPtr);
end

vehicleDataAvail = 0;
%
% Check to see if we've got vehicle data.  If so, plot it.
if( exist( 'data' ) && isfield( data, 'depth') )
  vehicleDataAvail = 1;
  fprintf('Plotting vehicle data.\n');
  lstr={};
  ptr_vpz=subplot(2,1,1);
  plot(data.depth.time, data.depth.value,'b');
  lstr(length(lstr)+1,1) = {'Measured'};
  set(gca,'ydir','reverse')
  grid on
  hold on
  plot(data.VerticalControl.depthCmd.time, data.VerticalControl.depthCmd.value);
  lstr(length(lstr)+1,1) = {'Commanded'};
  plot(data.VerticalControl.smoothDepthInternal.time, data.VerticalControl.smoothDepthInternal.value);
  lstr(length(lstr)+1,1) = {'Smoothed Command'};
  ylabel('Depth, m')
  dynamicDateTicks
  title(titleStr)
  legend(lstr)
  
  lstr={};
  ptr_vptheta=subplot(4,1,3);
  plot(data.platform_pitch_angle.time, data.platform_pitch_angle.value*180/pi,'b');
  lstr(length(lstr)+1,1) = {'Measured'};
  grid on
  hold on
  ylabel('Pitch, Deg.')
  Ts = data.LoopControl.nominalDt.value;
  if( isempty(Ts) ) Ts = .4;end
  if( Ts <=0 ) Ts = .4;end
  if(length(Ts) > 1) Ts = Ts(1);end
  plot(data.VerticalControl.depthIntegralInternal.time, data.VerticalControl.depthIntegralInternal.value/Ts)
  lstr(length(lstr)+1,1) = {'Integral Term'};
  plot(data.VerticalControl.pitchInternal.time, data.VerticalControl.pitchInternal.value*180/pi)
  lstr(length(lstr)+1,1) = {'Commanded'};
  dynamicDateTicks
  legend(lstr)
  
  ptr_vpdeltaE=subplot(4,1,4);
  plot(data.platform_elevator_angle.time, ...
      data.platform_elevator_angle.value*180/pi,'-ob');
  hold on;
  plot(data.platform_mass_position.time,...
      data.platform_mass_position.value*10000,'--rs');
%      data.platform_mass_position.value*1000,'--rx');
  plot(data.platform_buoyancy_position.time,...
      data.platform_buoyancy_position.value,'-dg');
%      data.platform_buoyancy_position.value*10000,'-dg');
  grid on
  ylabel('Actuator Displacements')
  legend('Elevator, degrees','Mass, tenths of mm''s ahead','Buoyancy; cc''s pumped out')
  dynamicDateTicks
  set(gca,'ydir','reverse')
  
  linkaxes(get(gcf,'children'),'x')


else
  fprintf('No data found.  Assuming orignal vehicle data (not replay) is in struct ''data''.\n');
  return
end














