%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot range vector, magnitude, and other stuff measured by the Usbl
% AUTHOR:  Rob McEwen
% DATE:    05/12/7
% MODS:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
global titleStr;
global vehicleName;
titleStr = [' Data Set: ', DirName];

if( exist('usbllog') == 0), return, end

if( exist('fontsize') == 0), fontsize = 14; end
if( fontsize ==12 ), linewidth = .5;end
fontweight = 'normal';


 if( exist('position') ~=1 ) 
   npposition = [4 32 815 658];
 else
   npposition = position;
 end

if exist( 'usblFig' ) ~= 1
   usblFig = figure;
   figure( usblFig );
 else
   figure( usblFig );
 end

 set( gcf, 'name', [vehicleName,' Usbl'] );

title([ 'Usbl Range', titleStr ])
set(gcf,'position',npposition)

 
lstr = {};

indx = ...
    find( usbllog(:,sonardyneStatus) == 0 | usbllog(:,sonardyneStatus) == 3);

subplot( 2, 1, 1 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%plot( usbllog(:,1), usbllog(:,[sonardyneRange sonardyneX:sonardyneZ]),'.' )
stairs( usbllog(:,1), usbllog(:,[sonardyneRange sonardyneX:sonardyneZ]) )
hold on
plot( usbllog(:,1), usbllog(:,[sonardyneRange ]),'xb' )
plot( usbllog(:,1), usbllog(:,[sonardyneX]), 'xg' )
plot( usbllog(:,1), usbllog(:,[sonardyneY]), 'xr' )
plot( usbllog(:,1), usbllog(:,[sonardyneZ]), 'xc' )
lstr(length(lstr)+1:length(lstr)+4,1) = { 'Range','X','Y','Z'};
grid on
title([ 'Usbl R=Blue;X=Green;Y=Red;Z=Cyan', titleStr ]);
ylabel('Range, Meters')
%legend(lstr);

subplot( 2, 1, 2 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
deltaT = [diff(usbllog(:,1));0];
stairs( usbllog(:,1), deltaT);
hold on
indxx = find(usbllog(:,sonardyneStatus)==0);
indx = [];
if( length(indx)>1 & indx(1)==1 ) 
  indx(1) = []; 
  indxx = indx-1;
else
  indxx = indx;
end
plot( usbllog(indxx,1), deltaT(indxx),'xc' )

indxx = find(usbllog(:,sonardyneStatus)==1);
plot( usbllog(indxx,1), deltaT(indxx),'xg' )

indxx = find(usbllog(:,sonardyneStatus)==2);
plot( usbllog(indxx,1), deltaT(indxx),'xb' )

indxx = find(usbllog(:,sonardyneStatus)==3);
plot( usbllog(indxx,1), deltaT(indxx),'xr' )
%set(gca, 'ylim', [0 5] )
%title('0=Undefined, 1=AllGood, 2=RangeOnly, 3=Stale')
title( ['Ping Interval; Cyan=Undefined;Green=All;Blue=Range Only;Red=Beacon ' ...
       'Not Detected'] )
%legend('Time Interval','Undefined','All','Range Only','Timed Out')
ylabel('Seconds');
xlabel('Seconds');
zoom on
grid on

% subplot( 2, 1, 2 )
% set(gca,'fontsize',fontsize,'fontweigh',fontweight)
% stairs( usbllog(:,1), usbllog(:,sonardyneStatus) )
% hold on
% plot( usbllog(:,1), usbllog(:,sonardyneStatus),'xr' )
% %plot( usbllog(:,1), usbllog(:,sonardyneStatus) ,'.')
% set(gca, 'ylim', [0 5] )
% title('0=Undefined, 1=AllGood, 2=RangeOnly, 3=Stale')
% ylabel('Status');
% xlabel('Seconds');
% zoom on
% grid on


linkaxes(get(gcf,'children'),'x')


if exist( 'usblFig2' ) ~= 1
   usblFig2 = figure;
   figure( usblFig2 );
 else
   figure( usblFig2 );
 end


set( gcf, 'name', [vehicleName,' Usbl'] );


set(gcf,'position',npposition)
lstr = {};

subplot( 4, 1, 1 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%plot( usbllog(:,1), usbllog(:,[sonardyneSigLevel sonardyneDetectLevel]), '.' )
stairs( usbllog(:,1), usbllog(:,[sonardyneSigLevel sonardyneDetectLevel]))
hold on
plot( usbllog(:,1), usbllog(:,[sonardyneSigLevel]),'xb')
clippedIndx = find(  usbllog(:,[sonardyneClipped]) == 1 );
plot( usbllog(clippedIndx,1), usbllog(clippedIndx,[sonardyneSigLevel]),'xr')
plot( usbllog(:,1), usbllog(:,[sonardyneDetectLevel]),'xg')
lstr(length(lstr)+1:length(lstr)+2,1) = { 'Signal','S/N'};
title([ 'Signal Level and Signal-to-Noise Ratio', titleStr ])
ylabel('Decibels')
grid on

subplot( 4, 1, 3 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%plot( usbllog(:,1), usbllog(:,sonardyneDoppler), '.' )
stairs( usbllog(:,1), usbllog(:,sonardyneDoppler))
title('Doppler Offset of Signal from Carrier Frequency')
ylabel('Hz')
grid on

subplot( 4, 1, 4 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%plot( usbllog(:,1), usbllog(:,sonardyneAcceptance), '.' )
stairs( usbllog(:,1), usbllog(:,sonardyneAcceptance))
title('Acceptance: Number of Good Slices Through Pulse Signal')
ylabel('N/A')
xlabel('Seconds')
set(gca,'ylim',[0 10]);
grid on

subplot( 4, 1, 2 )
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%plot( usbllog(:,1), usbllog(:,sonardyneQuality), '.' )
stairs( usbllog(:,1), usbllog(:,sonardyneQuality))
title('Quality (Least Squares Residual Error)')
ylabel('N/A')
grid on


linkaxes(get(gcf,'children'),'x')
