%function [] = deltaT(idt)
%--------------------------------------------------------------------------
%
% PURPOSE: Plot Imagenex 837 multibeam data
% AUTHOR:  Rob McEwen
% DATE:    2013/1/28
%
%--------------------------------------------------------------------------
%

%  global startTime dataDC rangeToWall position deltaTFig deltaTFig2

  if( exist('idt_fwd') ~= 1 )
    fprintf('Delta T data not present\n')
    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';

  idt_fwd.figPtr={};
  idt_fwd.subPlotPtr = {};
  idt_fwd.plotPtr = {};
  idt_fwd.legendPtr = {};
  lstr = {};

 if exist( 'deltaTFwdFig2' ) ~= 1
    deltaTFwdFig2 = figure;
    figure(deltaTFwdFig2);
    idt_fwd.figPtr{end+1}=deltaTFwdFig2;
%   winpos( position(1,:) );
  else
    figure(deltaTFwdFig2);
  end
  set( deltaTFwdFig2, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear deltaTFwdFig2' );
  set(gcf,'position',position)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

if( exist('idt_fwd')==1 )

  if( ~isfield(idt_fwd,'nbeams') )  idt_fwd.nbeams = 120; end
  if( ~isfield(idt_fwd,'npbeams') ) idt_fwd.npbeams = 1;end
  if( ~isfield(idt_fwd,'windowLen') ) idt_fwd.windowLen = 1;end
    %
    % Index into idt_fwd.data
    idt_fwd.BeamRange=idt_fwd.BeamRange1+[0:idt_fwd.nbeams-1]';
    beamIndx   = floor(idt_fwd.nbeams/(idt_fwd.npbeams+1)*[1:idt_fwd.npbeams]);
    %beamAngles = (beamIndx-60)*idt_fwd.nbeams/(idt_fwd.nbeams-1);
    %beamAngles = (beamIndx*idt_fwd.nbeams/(idt_fwd.nbeams-1) - 60)*pi/180;  %Degrees
    beamAngles = ([0:idt_fwd.nbeams-1]*idt_fwd.nbeams/(idt_fwd.nbeams-1) - 60)*pi/180;  %Degrees
    beamAngles = ([0:idt_fwd.nbeams-1] - 60)*pi/180;  %Degrees
    if( 0 )
    idtx   = interp1(dataNav(:,1), posx, idt_fwd.data(:,1));
    idty   = interp1(dataNav(:,1), posy, idt_fwd.data(:,1));
    idtPsi = interp1(dataNav(:,1), dataNav(:,mPsi), idt_fwd.data(:,1));
    %
    for( i=1:length(idtx) )
      idtx(i,1:length(beamIndx)) = idtx(i) + sin(idtPsi(i) + beamAngles(beamIndx)).*...
          idt_fwd.data(i,idt_fwd.BeamRange(beamIndx));
      idty(i,1:length(beamIndx)) = idty(i) - cos(idtPsi(i) + beamAngles(beamIndx)).*...
          idt_fwd.data(i,idt_fwd.BeamRange(beamIndx));
    end
  end
  hold on
  grid on
  idt_fwd.plotPtr{end+1} = plot(idt_fwd.data(:,1)-startTime, idt_fwd.data(:,idt_fwd.BeamRange(beamIndx) ), 'x',...
                            'Markersize',8);
  for( i=1:length(beamIndx))
    beamStr = 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) = { beamStr };
  end
  if( ~exist('startTime') ) 
      startTime = idt_fwd.startTime; 
  end
  xstr = sprintf('Seconds; t_0 = %.1f', startTime);
  xlabel(xstr);
  ylabel('Meters');
  title('DeltaT Range');

  if(idt_fwd.npbeams <= 5) 
      idt_fwd.legendPtr=legend(lstr,'location','best');
  end
  
end



  if( exist( 'SideBeamIndex' ) == 1) 
      fprintf('Data from DynamicControlLog\n');
      if(isfield(idt_fwd,'dataSource'))
          fprintf('Warning: DynamicControlLog exists and idt contains %s.\n',...
                  idt_fwd.dataSource);
      else
          idt_fwd.dataSource = 'DynamicControlLog';
      end
  elseif(isfield(idt_fwd,'dataSource'))
      fprintf('Data from %s\n',idt_fwd.dataSource);
  else
      return; 
  end

  if(~isfield(idt_fwd,'computeValues')) idt_fwd.computeValues=0;end
  
  if( ~strcmp(idt_fwd.dataSource,'DynamicControlLog') | idt_fwd.computeValues )
      fprintf('Finding SideBeamIndex in Matlab.\n');
      fprintf('Finding the minimum distance for each sounding.\n');
      idt_fwd.minAllowableRange = 1;
      idt_fwd.halfWindowLen = floor( (idt_fwd.windowLen-1)/2 );
      
      % [idt_fwd.minRange, idt_fwd.minRangeIndx]=min(idt_fwd.data(:,idt_fwd.BeamRange1: ...
      %                                               121)');
      % [idt_fwd.tIndx idt_fwd.minRangeIndx]=...
      %     find(idt_fwd.data(:,idt_fwd.BeamRange1:idt_fwd.BeamRange1+120)>idt_fwd.minAllowableRange & ...
      %          idt_fwd.data(:,idt_fwd.BeamRange1:idt_fwd.BeamRange1+120)<idt_fwd.maxAllowableRange);
      % idt_fwd.minRange=idt_fwd.minRange';
      % idt_fwd.minRangeIndx=idt_fwd.minRangeIndx'+idt_fwd.BeamRange1-1;

      idt_fwd.minRange = 500*ones(length(idt_fwd.data),1);
      idt_fwd.minAngle = NaN(length(idt_fwd.data),1);
      idt_fwd.minIndx  = NaN(length(idt_fwd.data),1);
      idt_fwd.median   = NaN(length(idt_fwd.data),1);
      tic
      for( i=1:length(idt_fwd.data) )
          for( j=idt_fwd.BeamRange1:idt_fwd.BeamRange1+119 )
              if( idt_fwd.data(i,j) > idt_fwd.minAllowableRange & idt_fwd.data(i,j) < idt_fwd.minRange(i) ) 
                  idt_fwd.minRange(i) = idt_fwd.data(i,j);
                  idt_fwd.minIndx(i) = j;
              end
          end
          minArrayIndx=idt_fwd.minIndx(i)-idt_fwd.BeamRange1+1;
          if( isnan(minArrayIndx) )
              idt_fwd.median(i) = NaN;
              continue;
          end
          idt_fwd.minAngle(i) = beamAngles(minArrayIndx);
          %
          % Now find the median
          if( idt_fwd.windowLen > 1 )
              %
              % Median filter.  Select halfWindowLen neighboring beams to either side
              % of the minimum range beam.  Handle the case where _beamIndx is within
              % halfWindowLen of either end of the array.

              if( minArrayIndx < idt_fwd.halfWindowLen ) 
                  startIndx = 0;
              elseif( minArrayIndx > idt_fwd.nbeams - idt_fwd.halfWindowLen - 1 ) 
                  startIndx = idt_fwd.nbeams - idt_fwd.windowLen;
              else 
                  startIndx = minArrayIndx - idt_fwd.halfWindowLen;
              end
              %
              % Find the median in the window
              idt_fwd.median(i) = median(...
                  idt_fwd.data(i,startIndx+idt_fwd.BeamRange1:startIndx+idt_fwd.BeamRange1+idt_fwd.windowLen-1));
          else
              idt_fwd.median(i) = idt_fwd.data(i,idt_fwd.minIndx(i));
          end
      end
      toc
  end

  lstr={};
  if exist( 'deltaTFwdFig' ) ~= 1
    deltaTFwdFig= figure;
    figure(deltaTFwdFig);
%   winpos( position(1,:) );
    idt_fwd.figPtr{end+1}=deltaTFwdFig;
  else
    figure(deltaTFwdFig);
  end
  set( deltaTFwdFig, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear deltaTFwdFig' );
  set(gcf,'position',position)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  idt_fwd.subPlotPtr{end+1} = subplot(2,1,1);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  if( strcmp(idt_fwd.dataSource,'DynamicControlLog') & ~idt_fwd.computeValues ) 
      idt_fwd.plotPtr{end+1} = plot(dataDC(:,1), dataDC(:,rangeToWall),'x');
  else
      idt_fwd.minTimeIndx = find(~isnan(idt_fwd.minIndx));
      %idt_fwd.plotPtr{end+1} = plot(idt_fwd.data(idt_fwd.minTimeIndx,1), ...
      %                          idt_fwd.data(idt_fwd.minTimeIndx, ...
      %                                   idt_fwd.minIndx(idt_fwd.minTimeIndx)),'o');
      idt_fwd.plotPtr{end+1} = plot(idt_fwd.data(idt_fwd.minTimeIndx,1), ...
                                idt_fwd.minRange(idt_fwd.minTimeIndx),'o');
      lstr(length(lstr)+1,1) = {'Unfiltered'};
      hold on
      idt_fwd.plotPtr{end+1} = plot(idt_fwd.data(idt_fwd.minTimeIndx,1), ...
                                idt_fwd.median(idt_fwd.minTimeIndx),'rx');
      lstr(length(lstr)+1,1) = {'Median Filtered'};
      
      if(idt_fwd.npbeams <= 5) 
        idt_fwd.legendPtr(end+1,1)=legend(lstr,'location','best');
      end
      
  end
  grid on
  xlabel('Seconds');
  ylabel('Meters');
  title('Range to Wall');


  idt_fwd.subPlotPtr{end+1} = subplot(2,1,2);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  if( strcmp(idt_fwd.dataSource,'DynamicControlLog') & ~idt_fwd.computeValues )
      idt_fwd.plotPtr{end+1} = plot(dataDC(:,1), dataDC(:,SideBeamIndex));
  else
      idt_fwd.plotPtr{end+1} = plot(idt_fwd.data(idt_fwd.minTimeIndx,1), ...
                                idt_fwd.minAngle(idt_fwd.minTimeIndx)*180/pi,'x');
  end
  grid on
  xlabel('Seconds');
  ylabel('Degrees');
  title('Beam Angle Used for Feedback');

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