%--------------------------------------------------------------------------
%
% PURPOSE: Plot Imagenex 881AL echo sounder data
% AUTHOR:  Rob McEwen
% DATE:    2007/10/1
%
%--------------------------------------------------------------------------
%

es.maxRange = 200;

global titleStr, noReturnRange;

if( exist('noReturnRange')==0 | isempty(noReturnRange) ) 
  noReturnRange=65;
end

if( exist('echoSounder.cfg')==2 )
  fprintf('Found echoSounder.cfg\n')
  es.fid = fopen('echoSounder.cfg');
  while(~feof(es.fid) ) 
    %es.str = fgets(es.fid);
    es.str = fgetl(es.fid);
    [es.s1, es.s2] = strtok(es.str);
    if(strcmp(es.s1,'range')) break; end;
  end
  if(~feof(es.fid)) 
    [es.s3, es.s4] = strtok(es.s2);
    [es.s5, es.s6] = strtok(es.s4,' .;');
    es.maxRange = str2num(es.s5);
  end
  fclose(es.fid);
end

es.profile   = findInFile('echosounder.cfg','profile');
es.startGain = findInFile('echosounder.cfg','startGain');
es.noReturnRange = findInFile('echosounder.cfg','noReturnRange');
es.minGrazing = findInFile('echosounder.cfg','minGrazingSim');
fprintf('On the vehicle, noReturnRange = %d m; but in this plot it = %d m.\n',...
        es.noReturnRange, noReturnRange);

fprintf('es.maxRange = %d m.\n',es.maxRange);

if( exist('dataES') == 1 )
  es.data     = dataES;
  es.status   = ESstatus;
  es.range    = ESrange;
  for( i=0:499 )
    eval([ 'es.raw' num2str(i) ' = ESraw'  num2str(i) ';']);
    eval([ 'clear ESraw'  num2str(i) ';']);
  end
  clear dataES ESstatus ESrange;
end

if( exist('fontsize') == 0), fontsize = 20; end
if( fontsize ==12 ), 
  es.linewidth = .5;
else
  es.linewidth = 1;
end
fontweight = 'normal';

if( exist('position') ~= 1 ) position = [3 59 1213 1070]; end

if( es.profile ~= 1 )
  if exist( 'echoFig' ) ~= 1
    echoFig = figure;
    figure( echoFig );
  else
    figure( echoFig );
  end

  set( gcf, 'name', 'Echo Sounder' );
  set(gcf,'position',position)

  if( 0 )
    es.h1=subplot(2,1,1);
    plot(es.data(:,1), es.data(:,es.range)/100 )
    ylabel('Range, m')
    grid on
    set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  end

  %es.h2=subplot(2,1,2);
  es.h2=imagesc(es.data(:,1)-es.data(1,1), [1:500]'*es.maxRange/500, ...
		es.data(:,[es.raw0:es.raw499])');
  set(gca,'ydir','normal');
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  %axis equal tight;
  grid on
  %es.h3=colorbar('ylabel','Intensity');
  es.h3=colorbar;
  hold on;
  es.h4=plot(es.data(:,1)-es.data(1,1), es.data(:,es.range)/100,...
	     'color','k','linestyle','.','linewidth',es.linewidth);
  ylabel('Range, m')
  xlabel('Seconds')
  grid on
end
%
% Turn this on for tank testing.
%
if( 0 )

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

set( gcf, 'name', 'Polar' );
set(gcf,'position',position)

es.theta = (es.data(:,es.train)-600)*.3*pi/180;

xx = es.data(:,es.range)/100.*cos(es.theta);
yy = es.data(:,es.range)/100.*sin(es.theta);

es.h5 = plot( xx,yy,'linestyle','.','linewidth',es.linewidth,'color','k' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
grid on
hold on
axis equal
es.h6 = plot( 0,0,'linestyle','d','linewidth',es.linewidth,'color','r' );
xlabel('meters')
ylabel('meters')

end

if exist( 'echoFigDiff' ) ~= 1
  echoFigDiff = figure;
  figure( echoFigDiff );
else
  figure( echoFigDiff );
end
set(gcf,'position',position)
set(gcf, 'Name', 'Imagenex Plot', 'DeleteFcn', 'clear gcf' );
set(gcf,'position',npposition)
if( exist('fontsize') == 0), fontsize = 14; end
%if( fontsize ==12 ), linewidth = .5;end
fontweight = 'normal';
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

sphand1=subplot(3,1,1);
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
plot(es.data(2:end,1)-es.data(2,1),diff(es.data(:,1)),'linestyle','.', ...
     'linewidth',es.linewidth);
ylabel('Sampling Period, Seconds')
if( exist('titleStr') == 1 )
  title( ['Imagenex T_s, Range, Train: ' titleStr] );
else
  title( ['Imagenex T_s, Range, Train '] );
end
grid on


sphand2=subplot(3,1,2);
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
plot(es.data(:,1)-es.data(1,1),es.data(:,es.range)/100,'linestyle','.',...
     'linewidth',es.linewidth);
grid on
hold on
es.titlestr2=[ '{\bf Blue=range $r$; Green=altitude$=-r\sin(\theta)$;}'];


% Example:
% textstr= [ '$y = ', num2str(fscale,'%.2f'),'\left( ',...
%                     num2str(a0,'%.4f'),'\sqrt{x} - ',...
%                     num2str(a1,'%.4f'),'x - ',...
%                     num2str(a2,'%.4f'),'x^2 + ',...
%                     num2str(a3,'%.4f'),'x^3 - ',...
%                     num2str(a4,'%.4f'),'x^4\right)$'];


indxR = find( es.data(:,es.range)/100 <noReturnRange & ...
              es.data(:,es.range)/100 > 0);
es.altitude = -es.data(indxR,es.range)/100.*sin((es.data(indxR,es.train)-600)*.3*pi/180);
plot(es.data(indxR,1)-es.data(1,1),es.altitude,'linestyle','.',...
     'linewidth',es.linewidth, 'color','g');

if(sum(strcmp(fields(es),'simGrazing')))
  plot(es.data(:,1),es.data(:,es.simGrazing)*180/pi,'color','r');
  tempStr = sprintf(' {\\bf Red=grazing; minGrazing = %.0f Deg}', es.minGrazing);
  es.titlestr2=strcat(es.titlestr2,tempStr);
end

hold off
ylabel('Range, m')
title(es.titlestr2,'interpreter','latex','fontsize',fontsize,'fontweigh',fontweight);

sphand3=subplot(3,1,3);
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
plot(es.data(:,1)-es.data(1,1),(es.data(:,es.train) -600)*.3,'linestyle','.', ...
    'linewidth',es.linewidth);
grid on
hold on
%indxT = find( es.data(:,es.range)/100 ~= 0 & es.data(:,es.range)/100 <= 99);

indxT = find( es.data(:,es.range)/100 >noReturnRange & ...
              es.data(:,es.range)/100 < 100);
plot(es.data(indxT,1)-es.data(1,1),(es.data(indxT,es.train) -600)*.3,...
     'linestyle','.','color','r','linewidth',es.linewidth);

indxNr = find( es.data(:,es.range)/100 == 100. );
plot(es.data(indxNr,1)-es.data(1,1),(es.data(indxNr,es.train) -600)*.3,...
     'linestyle','o','color','g','linewidth',es.linewidth);

%indxZ = find( es.data(:,es.range)/100 == 0. );
%plot(es.data(indxZ,1)-es.data(1,1),(es.data(indxZ,es.train) -600)*.3,...
%     'linestyle','o','color','m','linewidth',es.linewidth);

hold off
es.titlestr3=sprintf([ 'Blue: Range < %d m; Red: Rng > %d and < %d; ',...
                    'Green: Rng = %d; startGain = %d'],round(noReturnRange),...
                    round(noReturnRange), round(es.maxRange), ...
                    round(es.maxRange),es.startGain  );
title(es.titlestr3);
ylabel('{\bf Head Angle, $\theta$, Degrees}','interpreter','latex',...
       'fontsize',fontsize,'fontweigh',fontweight);
xlabel('Seconds')

linkaxes([sphand1 sphand2 sphand3],'x')
