%--------------------------------------------------------------------------
%
% PURPOSE: Plot Imagenex 837 multibeam data
% AUTHOR:  Rob McEwen
% DATE:    2013/1/28
%
%--------------------------------------------------------------------------
%

  if( exist('idt') ~= 1 )
    fprintf('Delta T data not present')
    return
  end

  if exist('titleStr') ~= 1
    titleStr = ['; Data Set: ', DirName];
  end
  
  str = [' ',titleStr,' Plotted on ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;
  if( exist('position') ~= 1 ) position = [3 59 1213 1070]; end

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


  if exist( 'deltaTFig2' ) ~= 1
    deltaTFig2 = figure;
    figure(deltaTFig2);
%   winpos( position(1,:) );
  else
    figure(deltaTFig2);
  end
  set( deltaTFig2, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear deltaTFig2' );
  set(gcf,'position',position)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  idt.subPlotPtr = {};
  idt.plotPtr = {};
  lstr = {};

if( exist('idt')==1 )

  idt.nbeams = 120; 
  if( ~isfield(idt,'npbeams') ) idt.npbeams = 1;end
    %
    % Index into idt.data
    idt.BeamRange=idt.BeamRange1+[0:idt.nbeams-1];
    beamIndx   = floor(idt.nbeams/(idt.npbeams+1)*[1:idt.npbeams]);
    %beamAngles = (beamIndx-60)*idt.nbeams/(idt.nbeams-1);
    %beamAngles = (beamIndx*idt.nbeams/(idt.nbeams-1) - 60)*pi/180;  %Degrees
    beamAngles = ([0:idt.nbeams-1]*idt.nbeams/(idt.nbeams-1) - 60)*pi/180;  %Degrees
  if( 0 )
    idtx   = interp1(dataNav(:,1), posx, idt.data(:,1));
    idty   = interp1(dataNav(:,1), posy, idt.data(:,1));
    idtPsi = interp1(dataNav(:,1), dataNav(:,mPsi), idt.data(:,1));
    %
    for( i=1:length(idtx) )
      idtx(i,1:length(beamIndx)) = idtx(i) + sin(idtPsi(i) + beamAngles(beamIndx)).*...
          idt.data(i,idt.BeamRange(beamIndx));
      idty(i,1:length(beamIndx)) = idty(i) - cos(idtPsi(i) + beamAngles(beamIndx)).*...
          idt.data(i,idt.BeamRange(beamIndx));
    end
  end
  hold on
  grid on
  plot(idt.data(:,1), idt.data(:,beamIndx ), 'x');
  for( i=1:length(beamIndx))
    lblStr = sprintf('DeltaT Beam %d at %.1f Deg.',beamIndx(i), ...
	beamAngles(beamIndx(i))*180/pi);
    %lstr(length(lstr)+1,1) = { 'Decimated side-Looking Imagenex Ranges; '};
    lstr(length(lstr)+1,1) = { lblStr };
  end
  xlabel('Seconds');
  ylabel('Meters');
  title('DeltaT Range');

  legend(lstr,'location','best')
  


end
  
  if( exist( 'SideBeamIndex' ) ~= 1) return; end


  if exist( 'deltaTFig' ) ~= 1
    deltaTFig = figure;
    figure(deltaTFig);
%   winpos( position(1,:) );
  else
    figure(deltaTFig);
  end
  set( deltaTFig, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear deltaTFig' );
  set(gcf,'position',position)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  idt.subPlotPtr{end+1} = subplot(2,1,1);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  idt.plotPtr{end+1} = plot(dataDC(:,1), dataDC(:,rangeToWall));
  grid on
  xlabel('Seconds');
  ylabel('Meters');
  title('Median-Filtered Range to Wall');

  idt.subPlotPtr{end+1} = subplot(2,1,2);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  idt.plotPtr{end+1} = plot(dataDC(:,1), dataDC(:,SideBeamIndex));
  grid on
  xlabel('Seconds');
  ylabel('Degrees');
  title('Beam Angle Used for Feedback');
  
  
  linkaxes(get(gcf,'children'),'x')
