%function [ ] = processRadarEric( dataSet, plotLevel)
%
% PURPOSE:  Load and process radar data from Dec 2021 PowerBuoy Deployment.  The
%           data is in Eric's format.
%
% CHANGE LOG: 1) Rob McEwen, initial pass.  Included code from Eric Martin.
%
%clear; close all;

if( exist('plotLevel')==0 ) plotLevel=1;end
if( exist('saveFigs')==0 )  saveFigs=0;end    
if( exist('existAhrs')==0 )  existAhrs=0;end    


if(~exist('dataSet') || isempty(dataSet) )
  if( ...
        exist('garmin/data/test_1646869869.h5'))
    dataSet = '1646869869';
  else
    fprintf('Please supply data.\n');
    return
  end
end

co = [0 0 1;
      0 0.5 0;
      1 0 0;
      0 0.75 0.75;
      0.75 0 0.75;
      0.75 0.75 0;
      0.25 0.25 0.25];

set(groot,'defaultAxesColorOrder',co)

MonitorPositions = get(0,'MonitorPositions');
nMon = size(MonitorPositions);
nMon = nMon(1);
%Select widest monitor
position = MonitorPositions(1,:);
if( nMon > 1 && MonitorPositions(2,3) > MonitorPositions(1,3) )  
  position =  MonitorPositions(2,:); 
end

%position = get(0,'screensize');
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;

% 
%loadStr = sprintf('test_%s.h5',dataSet)
loadStr = sprintf('atSea_%s.h5',dataSet)
%pathStr = sprintf('garmin/data/%s',loadStr);
pathStr = sprintf('%s',loadStr);

gain = floor(double(h5read(pathStr, '/reports_a/gain')));
sea  = floor(double(h5read(pathStr, '/reports_a/sea')));
rain = floor(double(h5read(pathStr, '/reports_a/rain')));
range= floor(double(h5read(pathStr, '/reports_a/range')));

% Deal with changed gain value in the title:
if(  norm(diff((gain))) )
  indx = find(diff(gain));
  gainVal = [gain(1); gain(indx+1)]/100;
  fprintf('This segment has multiple gain values.\n');
else
  gainVal = gain(1)/100;
end
if(  norm(diff((sea))) )
  indx = find(diff(sea));
  seaVal = [ sea(1) sea(indx+1)]/100;
  fprintf('This segment has multiple "sea" values.\n');
else
  seaVal = sea(1)/100;
end
if(  norm(diff((rain))) )
  indx = find(diff(rain));
  rainVal = rain(indx+1)/100;
  fprintf('This segment has multiple "rain" values.\n');
else
  rainVal = rain(1)/100;
end

if( existAhrs )

%%% Begin Ahrs Section %%%

% 1. 'altitude' 
% 2. 'dVelocity' 
% 3. 'eVelocity' 
% 4. 'latitude' 
% 5. 'longitude' 
% 6. 'nVelocity' 
% 7. 'packet_time' 
% 8. 'pitchAngle' 
% 9. 'rollAngle' 
% 10.'timestamp' 
% 11.'xAccel' 
% 12.'xRateCorrected' 
% 13.'xRateTemp' 
% 14.'yAccel' 
% 15.'yRateCorrected' 
% 16.'yawAngleTrue' 
% 17.'zAccel' 
% 18.'zRateCorrected' 

roll  = h5read(pathStr, '/ahrs/rollAngle')*pi/180;
pitch = h5read(pathStr, '/ahrs/pitchAngle')*pi/180;
yaw   = h5read(pathStr, '/ahrs/yawAngleTrue')*pi/180;
altitude = h5read(pathStr, '/ahrs/altitude');
time_unix   = h5read(pathStr, '/ahrs/timestamp');

omega(:,1) = double(h5read(pathStr,'/ahrs/xRateCorrected'))*pi/180;
omega(:,2) = double(h5read(pathStr,'/ahrs/yRateCorrected'))*pi/180;
omega(:,3) = double(h5read(pathStr,'/ahrs/zRateCorrected'))*pi/180;

clear v_P
v_P(1,:)   = double(h5read(pathStr,'/ahrs/nVelocity'));
v_P(2,:)   = double(h5read(pathStr,'/ahrs/eVelocity'));
v_P(3,:)   = double(h5read(pathStr,'/ahrs/dVelocity'));


% Location of the radar wrto the nav440, in nav440 coordinates.  
  rr = [-2 -22.5 -50]'*.0254;
  
% Compute the velocity of the radar in NED coordinates.  First, the component
% due to the ship's angular motion, in ship coordinates:
  vv_B = cross(omega', rr*ones(1,length(omega)));

% Preallocate memory:
  vv_N      = NaN(3,length(omega));
  v_P_B     = NaN(3,length(omega));
  v_radar_B = NaN(3,length(omega));
  v_radar   = NaN(3,length(omega));
  
% Recoordinatize in NED and add the translational term v_P.

  for( i=1:length(omega) )
    cphi   = cos( roll(i) );
    sphi   = sin( roll(i) );
    ctheta = cos( pitch(i) );
    stheta = sin( pitch(i) );
    cpsi   = cos( yaw(i) );
    spsi   = sin( yaw(i) );
    %
    % Form the direction cosine matrix that recoordinatizes a vector from the 
    % body frame (B) into the local-level-local-vertical (LV) navigation 
    % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
    % Here we will assume the LV frame is intertially fixed and thus
    % also a Newtonian (N) frame.
    %
    % The transformation T_B_N, is 3-2-1 Euler.  The matrix below, T_N_B, is
    % the transpose.
    %
    %
    T_N_B(1,1) =  cpsi.*ctheta; 
    T_N_B(2,1) =  spsi.*ctheta; 
    T_N_B(3,1) = -stheta; 

    T_N_B(1,2) = -spsi.*cphi + cpsi.*stheta.*sphi; 
    T_N_B(2,2) =  cpsi.*cphi + sphi.*stheta.*spsi; 
    T_N_B(3,2) =  ctheta.*sphi; 

    T_N_B(1,3) =  spsi.*sphi + cpsi.*cphi.*stheta; 
    T_N_B(2,3) = -cpsi.*sphi + stheta.*spsi.*cphi;
    T_N_B(3,3) =  ctheta.*cphi; 

    
    vv_N(:,i) = T_N_B * vv_B(:,i);
    
    %
    % Add translational velocity.
    v_radar(:,i) = v_P(:,i) + vv_N(:,i);

    %
    % Now let's go the other way compute v_radar in ship coordinates.  So, we
    % just need convert and store v_radar in ship (B) coordinates:
    
    v_radar_B(:,i) = T_N_B'*v_radar(:,i);
    
    %
    % Let's also save velocity at the Ahrs:
    v_P_B(:,i) = T_N_B'*v_P(:,i);
    %
    % Check:
    %v_P_B_2(:,i)= vv_B(:,i) + v_P_B(:,i);
    
  end

  % Now flip these back to tall and thin:
  v_P       = v_P';
  v_P_B     = v_P_B';
  v_radar   = v_radar';
  v_radar_B = v_radar_B';
  %
  % Sampling rate is 10 Hz.
  zz = cumtrapz(double(v_radar(:,3)))*.1;
  %
  % Cutoff periods of 50, 100 and 500 seconds. If fc is cutoff then use fc/(fs/2).
  [b,a]=butter(4,1/50/5,'high');
  zz50  = -filtfilt(b,a,zz)  + double(altitude(1));

  [b,a]=butter(4,1/100/5,'high');
  zz100 = -filtfilt(b,a,zz) + double(altitude(1));

  [b,a]=butter(4,1/200/5,'high');
  zz200 = -filtfilt(b,a,zz) + double(altitude(1));



% Let's convert the timestamp into something more plottable.
time = datetime(time_unix, 'ConvertFrom','epochtime');


if( plotLevel == 2)

  if( exist( 'fig2' ) ~= 1 )
    fig2 = figure;
    figure(fig2);
  else
    figure(fig2);
    clf
  end
  set( fig2, 'Name', 'Buoy Data', 'DeleteFcn', 'clear fig2' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  if( ~exist('fontsize') )
    fontsize = 20;
  end
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  subplot(3,1,1)
  plot(time,[v_P(:,1) v_radar(:,1)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('North, m/s')
  title('Velocity of Nav440 Ahrs and Radar in NED Coordinates');
  
  subplot(3,1,2)
  plot(time,[v_P(:,2) v_radar(:,2)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('East, m/s')
  
  subplot(3,1,3)
  plot(time,detrend([v_P(:,3) v_radar(:,3)]))
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('Down, m/s')
  xlabel('Minutes Since Midnight')
  
  linkaxes(get(gcf,'children'),'x')
  

  if( exist( 'fig3' ) ~= 1 )
    fig3 = figure;
    figure(fig3);
  else
    figure(fig3);
    clf
  end
  set( fig3, 'Name', 'Buoy Data', 'DeleteFcn', 'clear fig3' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  
  subplot(3,1,1)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,1)],'b');
  plot(time,[v_radar_B(:,1)],'g');
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Ahrs x, m/s and Deg.')
  title('Velocity of Nav440 Ahrs and Radar in Ahrs (FSK) Coordinates');
  
  subplot(3,1,2)
  plot(time, roll*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,2)],'b');
  plot(time,[v_radar_B(:,2)],'g');
  grid on 
  legend('Roll','Ahrs','Radar','location','northeastoutside');
  ylabel('Ahrs y, m/s and Deg.')
  
  subplot(3,1,3)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[detrend(v_P_B(:,3))],'b')
  plot(time,[detrend(v_radar_B(:,3))],'g')
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Ahrs z (down), m/s and Deg.')
  xlabel('Minutes Since Midnight')
  
  linkaxes(get(gcf,'children'),'x')
  
end
if( plotLevel == 1)
  
  
  
  if( exist( 'waveFigPtr' ) ~= 1 )
    waveFigPtr= figure;
    set( waveFigPtr, 'Name', 'Height', 'DeleteFcn', 'clear waveFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      scanFontsize=fontsize;
    else
      scanFontsize=22;
    end
    scanFontweight='normal';
    set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
    set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
        scanFontsize)
  else
    figure(waveFigPtr);
  end

  
% indx = find(nav440(:,1) - scan(1).pulse(1).time > 0 );
% startScanIndx = indx(1);
  
% indx = find(nav440(:,1) - scan(end).pulse(end).time > 0 );
% endScanIndx = indx(1) + 200;

  nExtrema = 5;
  maxIndices = islocalmax(zz50,'MaxNumExtrema',nExtrema);
  maxIndices = find(maxIndices);                       %Filter out the zeros.


% plot(nav440(startScanIndx:endScanIndx,1), [zz50(startScanIndx:endScanIndx) v_radar(startScanIndx:endScanIndx,3)]);
% plot(time, [altitude zz50  zz100 zz500]);
% plot(time, [altitude zz50 zz100 zz200]);
  lstr = {};
  plot(time, altitude);
  lstr(length(lstr)+1,1) = { 'Buoy Elevation' };
  waveFigAxes=gca;
  grid on
  hold on
    plot(time(maxIndices),zz50(maxIndices), '^','Color',[217 83 25]/255,...
    'MarkerFaceColor',[217 83 25]/255,'DisplayName','Local maxima');
    lstr(length(lstr)+1,1) = { 'Wave Crests' };
  xlabel('Time Since Midnight')
  ylabel('See Legend. Meters')
  plot(time, zz50);
  lstr(length(lstr)+1,1) = { 'Radar Elevation 50 sec Filter' };
  %plot(time, zz100);
  %lstr(length(lstr)+1,1) = { 'Radar Elevation 100 sec Filter' };
  %plot(time, zz200);
  %lstr(length(lstr)+1,1) = { 'Radar Elevation 200 sec Filter' };

  lhand = legend(lstr);
  
% legend('Radar Elevation','Radar Heave Rate');
% legend('Ahrs GPS Elevation','Ahrs Accel 50 sec', 'Ahrs Accel 100 sec', 'Ahrs Accel 500 sec');
  %lhand=legend({'Ahrs GPS Elevation','Radar Elev; 50 sec Filter'});
%  lhand=legend({'Ahrs GPS Elevation','Radar Motion; 50 sec Filter', 'Radar Motion; 100 sec Filter', 'Radar Motion; 200 sec Filter'});
  title(sprintf('GPS/Ahrs Sea Surface and Elevation for Data Set %s',replace(dataSet,'_','-')));
end


  radar_z_dot_NED = v_radar(:,3);

end  


%%% End Ahrs Section %%%
  
  
  angle = double(h5read(pathStr, '/radar_a/angle'))/8 * 360/720/2 * pi/180;   %radians
  max_range = double(h5read(pathStr, '/radar_a/range'));                      %meters
  pulseArray  = double(h5read(pathStr, '/radar_a/scan'));
  rTime = datetime(double(h5read(pathStr, '/radar_a/time'))/10e9, 'convertfrom','epochtime');

  domeSpeed = double(h5read(pathStr, '/reports_a/dome_speed'));
  if(std(domeSpeed) ~= 0 )
    fprintf('There are at least two scan speeds in this data set.\n')
  end
  
  %pulsesPerScan = 2048;    %Simrad

  %Garmin:
  if(domeSpeed(1) == 0)
    pulsesPerScan = 2880;    %Garmin  Dome speed 0
  elseif( domeSpeed(1) == 2)
    pulsesPerScan = 1440;    %Garmin  Dome speed 2
  else
    fprintf('Unrecognized dome speed.\n')
    return
  end

  nscans = floor(width(pulseArray)/pulsesPerScan);
  fprintf('Number of scans is %d.\n',nscans);
  fprintf('Number of pulses (spokes) per scan is %d.\n',pulsesPerScan);
  fprintf('\n\n');

  
first = 1;
cpsave = [];
tArrivalS = 0;

if( exist('auto') )
  
  for( i=1:nscans )
    indx = 1 + (i-1)*pulsesPerScan;
    scanIndx = [indx : indx+pulsesPerScan-1];
    rtime   = rTime(scanIndx);
    scanFinishTime(i,1) = rtime(end);
  end
  
  for( i=1:nExtrema )
    scanRange(1,i) = indxfn2(time(maxIndices(i)), scanFinishTime);
  end

  fprintf('Wavecrest identified at scan %d\n', scanRange');
  fprintf('Corresponding times are:\n');
  scanFinishTime(scanRange)

end

if( ~exist('scanRange') )
  scanRange =  input('Enter scan range, e.g. 10:15:\nscanRange = ');
  %scanRange = 1:nscans;
end

if( isempty('scanRange') )
  return;
end


for( m = scanRange)
  
% pulses = scan(m).pulse;
%
% Each row of the pulses array is a pulse (a spoke in the wheel).  Columms are
% distances, values are the reflection at that distance.

  indx = 1 + (m-1)*pulsesPerScan;
  scanIndx = [indx : indx+pulsesPerScan-1];
  pulses = pulseArray(:, scanIndx )';
  azimuth = angle(scanIndx);
  rtime   = rTime(scanIndx);
  

  if( plotLevel == 2)
    
    if exist( 'scanFigPtr' ) ~= 1
      scanFigPtr= figure;
      set( scanFigPtr, 'Name', 'Data in Polar Coordinates', 'DeleteFcn', 'clear scanFigPtr' );
      set(gcf,'position',position)
      if(exist('fontsize')) 
        scanFontsize=fontsize;
      else
        scanFontsize=22;
      end
      scanFontweight='normal';
      set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
      set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
          scanFontsize)
    else
      figure(scanFigPtr);
    end

  end
  
  
  fprintf('Completed reading the data.  Start processing %d pulses from scan %d.\n', length(pulses),m);

  X = [];
  Y = [];
  V = [];

  tstart = tic;

  % for(n = 1:height(pulses) )
  %   stime(n)    = pulses(n).time;
  %   sazimuth(n) = pulses(n).azimuth;
  %   spangle(n)  = pulses(n).pointingAngle;
  % end

  stime = datenum(rtime - rtime(1))*86400;
  azimuthu = unwrap(azimuth);
  %yawu  = unwrap(yaw);
  
  
  
  for n = 1:pulsesPerScan
    pulse = pulses(n,:);
    if( ~mod(n,100) )
      fprintf('Processing pulse %d/%d at azimuth = %.2f degrees.\n',n,length(pulses),azimuth(n)*180/pi);
    end
    
    %if( azimuth > pi-pi/12  &&  azimuth < pi+pi/12 ) continue; end
    %if( 170 < pulse.azimuth &&  pulse.azimuth < 185 ) continue; end

    % angle comes either from azimuth or pointing angle, in radians
    %azimuth = ones(1, length(pulse.radarVideo))* pulse.azimuth * pi/180;
    %heading = ones(1, length(pulse.radarVideo))* (pulse.azimuth+pulse.pointingAngle) * pi/180;

    %azimuth = ones(1, width(pulse)) * sazimuth;
    azimuthArray = ones(length(pulse), 1) * azimuthu(n);

    % range is self described, convert to array
    
    %range = linspace(pulse.startRange, pulse.endRange, length(pulse.radarVideo));
    range = [1:length(pulse)]';

    %Limit the range:
    maxRange=500;   %Simrad  meters?
    maxRange=705;   %Garmin  but these are bytes.
    %
    % 705 is the array length for Garmin.  This is then scaled by max_range above.
    %nStop = round(length(pulse)*maxRange/range(end));
    nStop = length(pulse);

    % convert to cartesian
    %[x, y] = pol2cart(azimuth, range);
    %[x, y] = pol2cart(azimuth(1:nStop), range(1:nStop));
    [x, y] = pol2cart(azimuthArray(1:nStop), max_range(1)/nStop*range(1:nStop));

    %Stack all the data in columns.  
    %Hacked in x <-> y swap below for the Garmin.
    X = [X; y];
    Y = [Y; x];
    V = [V; pulse(1:nStop)'];

    coord = 'cartesian';
    coord = 'polar';
    % plot and map colors to values
    if( plotLevel == 2 && first )
      if(strncmpi(coord,'cart',4))
        hscatter=scatter(x,y,3,pulse(1:nStop));
        axis equal;
        set(gca,'xlim',[-maxRange maxRange]);
        set(gca,'ylim',[-maxRange maxRange]);
        hold on;
      else
        hscatter=polarscatter(azimuthArray(1:nStop), range(1:nStop),3,pulse(1:nStop));
        hold on;
      end
      title(['Scan ' num2str(m)]);
      first = 0;
      grid on
    else
      if(strncmpi(coord,'cart',4))
        hold on
        hscatter.XData = x;
        hscatter.YData = y;
        hscatter.CData = pulse(1:nStop);
        hold on
      else
        hscatter.XData = azimuthArray(1:nStop);
        hscatter.YData = range(1:nStop);
        hscatter.CData = pulse(1:nStop);
        hold on
      end
    end
    drawnow update
    
  end

  V = double(V);
  
  fprintf('Converted scan %d to Cartesian coordinates!\n',m);
  tstop1 = toc(tstart);
  fprintf('Cartesian conversion and memory allocation took %.2f sec.\n',tstop1);
    
  
    fprintf('Start meshgrid\n');
    tstart2 = tic;

    meshRes = 1;    %meters per pixel
    %[Xq, Yq] = meshgrid(-maxRange:meshRes:maxRange,-maxRange:meshRes:maxRange);
    [Xq, Yq] = meshgrid(-max_range(1):meshRes:max_range(1),-max_range(1):meshRes:max_range(1));

    fprintf('End meshgrid, start griddata\n');
    
    Vq = griddata(X,Y,V, Xq, Yq);

    fprintf('Finished griddata.\n');
    tstop2 = toc(tstart2);
    fprintf('Interpolation to Cartesian grid took %.2f sec.\n',tstop2);


    %if( plotLevel == 1 )
    if( plotLevel <= 2 )
      fprintf('Start plot\n');

      if( 1 || exist( 'gridFigPtr' ) ~= 1 )
        gridFigPtr= figure;
        set( gridFigPtr, 'Name', 'Cartesian Contours', 'DeleteFcn', 'clear gridFigPtr' );
        set(gcf,'position',position)
        if(exist('fontsize')) 
          scanFontsize=fontsize;
        else
          scanFontsize=22;
        end
        scanFontweight='normal';
        set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
        set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
            scanFontsize)
      else
        figure(gridFigPtr);
      end

      contourf(Xq, Yq, Vq);
      axis equal
      xlabel('Meters')
      ylabel('Meters')
      title(sprintf('Garmin Data Set %s, Scan %d, Gain %d, Sea %d, Rain %d',...
          replace(dataSet,'_','-'), m, gainVal(1), seaVal(1), rainVal(1)));
      %title(sprintf('Garmin Data Set %s, Scan %d',replace(dataSet,'_','-'), m));
    end

    continue
    
    %VB = imbinarize(Vq);
    VB = edge(Vq,'canny');

    %Vqsave{m}  = Vq;
    VBsave{m}  = VB;
    VBbsave{m} = imbinarize(Vq);

    % Compute average ship heading during this scan (2.5 seconds)
    % psi = mean(azimuthu'-yawu') * pi/180;
    % fprintf('Ship heading = %.1f degrees.\n',psi*180/pi);

    % Blank out the bad sector before creating the edge image.
    if( 0 )
      
      maxR   = maxRange/meshRes;
      origin = maxRange/meshRes;
      
      thetaBlank0 = 160 * pi/180  - psi;
      thetaBlank1 = thetaBlank0 + 35*pi/180;
      
      for( az = thetaBlank0:2*pi/2048:thetaBlank1 )
        for( r = 1:maxR )
          xb = ceil( r*cos(az) );
          yb = ceil( r*sin(az) );
          VBbsave{m}( yb + origin, xb + origin ) = 0;
          VBbsave{m}( yb + origin+1, xb + origin ) = 0;
          VBbsave{m}( yb + origin, xb + origin+1 ) = 0;
          VBbsave{m}( yb + origin+1, xb + origin+1 ) = 0;
        end
      end

    end

    
    %
    % Tried replacing each blob with its centroid.  It didn't seem to help.
    if( 0 )
      VB2=imfill(VB,'holes');    
      s = regionprops(VB2, 'centroid');
      centroids = cat(1,s.Centroid);
      centroids = round(centroids);
      VB3 = zeros(size(VB));
      for(i=1:length(centroids))
        VB3(centroids(i,2), centroids(i,1)) = 1;
      end
    end
    

    tstart3 = tic;
    %
    % Lines with a normal exceeding waveHeading +/- exclusion will be excluded.
    waveHeadingDeg = -72;  %Degrees
    exclusion = 5;      %Degrees
    
    [H, T, R] = hough(VB,'theta', waveHeadingDeg-exclusion:waveHeadingDeg+exclusion);
    
    thetaExcl= 2*exclusion-1;
    
    nPeaks = 7;
    P  = houghpeaks(H,nPeaks,'theta',T,'nhoodsize',[91 thetaExcl]);
    fprintf('Completed Hough Transform and Peak Detection Calculations.\n');
    tstop3 = toc(tstart3);
    fprintf('Hough Transform and peak detection took %.2f sec.\n',tstop3);

    %
    % Translate origin from upper left to center:
    xPiv  = (size(Xq,2)-1)/2;
    yPiv  = (size(Xq,1)-1)/2;
    rho   = R(P(:,1));
    theta = T(P(:,2))*pi/180;
    rho0  = [];

    %
    % See Rob's notes for the derivation.  rho0 is normal from the line to the
    % center of the image.
    for( i = 1:length(rho) )
      if( abs(theta(i)) < pi/4 )
        rho0(i) = rho(i) - xPiv/cos(theta(i)) - ...
            (yPiv - xPiv*tan(theta(i)))*sin(theta(i));
      else
        rho0(i) = rho(i) - yPiv/sin(theta(i)) - ...
            (xPiv*cos(theta(i)) - yPiv*cos(theta(i))^2/sin(theta(i)));
      end
    end
    %
    % Now try Sharon's way:
    %
    img = imrotate(VBbsave{m},waveHeadingDeg);
    sigsum = sum(img,1);
    sigsum = smoothdata(sigsum,'gaussian','SmoothingFactor',0.25);
    %sigsum3 = detrend(sigsum2,2);
    %sigsum4 = normalize(sigsum3,'range');
    midIndx = round(xPiv*sqrt(2)*cos((waveHeadingDeg+45)*pi/180));
    
    
    %stem(midIndx, sigsum(midIndx));
    
    
    maxIndices = islocalmax(sigsum,'MaxNumExtrema',8);
    maxIndices = find(maxIndices);                       %Filter out the zeros.
    
    if( plotLevel )
      figure;
      plot(sigsum)
      title(['Wave Locs for Scan ' num2str(m)])
      grid on
      hold on
      plot(maxIndices,sigsum(maxIndices), '^','Color',[217 83 25]/255,...
          'MarkerFaceColor',[217 83 25]/255,'DisplayName','Local maxima');
    end
    %
    % Wave crest locations:
    rho0S = maxIndices - midIndx;
    rho0SIndx = find(rho0S' < 0);
    nwavesS = max(rho0SIndx);

    
    if( plotLevel == 1 )
      if exist( 'eFigPtr' ) ~= 1
        eFigPtr= figure;
        set( eFigPtr, 'Name', 'Edge Detection', 'DeleteFcn', 'clear eFigPtr' );
        set(gcf,'position',position)
        if(exist('fontsize')) 
          scanFontsize=fontsize;
        else
          scanFontsize=22;
        end
        scanFontweight='normal';
        set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
        set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
            scanFontsize)
      else
        figure(eFigPtr);
      end
      
      
      imshow(VB)
      hold on
      set(gca,'visible','on');
      set(gca,'ydir','normal')
      set(gcf,'position',position)
      xlabel('Pixels Ahead; Two Meters/Pixel');
      ylabel('Pixels Abeam; Two Meters/Pixel');
      title('Edges')
      

      return
      
      
      
      if exist( 'bFigPtr' ) ~= 1
        bFigPtr= figure;
        set( bFigPtr, 'Name', 'Hough', 'DeleteFcn', 'clear bFigPtr' );
        set(gcf,'position',position)
        if(exist('fontsize')) 
          scanFontsize=fontsize;
        else
          scanFontsize=22;
        end
        scanFontweight='normal';
        set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
        set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
            scanFontsize)
      else
        figure(bFigPtr);
      end

      
      hEdge=subplot(2,1,1);
      
      imshow(VB)
      hold on
      set(hEdge,'visible','on');
      xlabel('Pixels Ahead; Two Meters/Pixel');
      ylabel('Pixels Abeam; Two Meters/Pixel');
      title('Edges')
      set(gca,'ydir','normal')
      
      hHough=subplot(2,1,2);
      
      imshow(imadjust(rescale(H)),'XData',T,'YData',R,'InitialMagnification','fit');
      title('Hough transform');
      xlabel('\theta'), ylabel('\rho');
      axis on, axis normal, hold on;
      colormap(gca,hot);

      plot(theta*180/pi,rho,'s','color','green');
      
      hold on
    end  %plotLevel

    if(0)
      lines = houghlines(VB,T,R,P,'FillGap',5,'MinLength',7);
      for k = 1:length(lines)
        xy = [lines(k).point1; lines(k).point2];
        plot(hEdge,xy(:,1),xy(:,2),'LineWidth',2,'Color','green');
      end
    end


    
    return
    
    
    
    
    xMax = length(VB);
    yMax = xMax;

    xy = NaN(2,2,2);
    
    for( k = 1:length(theta) )
      if( abs(theta(k)) > pi/4 )
        xy(:,1,k) = [1 xMax]';
        xy(1,2,k) = -cos(theta(k))/sin(theta(k))         +  rho(k)/sin(theta(k));
        xy(2,2,k) = -cos(theta(k))/sin(theta(k))*xMax    +  rho(k)/sin(theta(k));
      else
        xy(1,1,k) = -tan(theta(k))        +  rho(k)/cos(theta(k));
        xy(2,1,k) = -tan(theta(k))*yMax   +  rho(k)/cos(theta(k));
        xy(:,2,k) = [1 yMax]';
      end
      if( plotLevel == 2 )
        plot(hEdge,xy(:,1,k),xy(:,2,k),'LineWidth',2,'Color','green');
      end
    end

    % Switch waveHeading into radians.
    waveHeading = waveHeadingDeg*pi/180;

    %
    % Plot Sharon's lines:
    if( plotLevel )
      if( 1 )
        blobFigPtr(m,1)= figure;
        set( blobFigPtr(m), 'Name', 'Blob Plot', 'DeleteFcn', 'clear blobFigPtr(m)' );
        set(gcf,'position',position)
        if(exist('fontsize')) 
          scanFontsize=fontsize;
        else
          scanFontsize=22;
        end
        scanFontweight='normal';
        set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
        set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
            scanFontsize)
      else
        figure(blobFigPtr(m));
      end
      clf
      hImage = imshow(VBbsave{m});
      hBlob2(m) = imgca(blobFigPtr(m));
      set(hBlob2(m),'visible','on');
      set(blobFigPtr(m),'position',position)
      if( meshRes == 1 )
        xlabel(hBlob2(m),'Meters East');
        ylabel(hBlob2(m),'Meters North');
      else
        xlabel(hBlob2(m),'Pixels East; Two Meters/Pixel');
        ylabel(hBlob2(m),'Pixels North; Two Meters/Pixel');
      end
      title(hBlob2(m),sprintf('Scan %d, Data Set %s, Gain = %d', m, dataSet, gain));
      hold(hBlob2(m))
      
      xyS = NaN(2,2,2);

      % Switch waveHeading into radians.
      waveHeading = waveHeadingDeg*pi/180;

      %Shift rho0S back to the upper left corner:
      %
      % See Rob's notes for the derivation.  rho0 is normal from the line to the
      % center of the image.
      for( i = 1:length(rho0S) )
        if( abs(waveHeading) < pi/4 )
          rhoS(i) = rho0S(i) + xPiv/cos(waveHeading) + ...
              (yPiv - xPiv*tan(waveHeading))*sin(waveHeading);
        else
          rhoS(i) = rho0S(i) + yPiv/sin(waveHeading) + ...
              (xPiv*cos(waveHeading) - yPiv*cos(waveHeading)^2/sin(waveHeading));
        end
      end
      
      for( k = 1:length(rho0S) )
        if( abs(waveHeading) > pi/4 )
          xyS(:,1,k) = [1 xMax]';
          xyS(1,2,k) = -cos(waveHeading)/sin(waveHeading)         +  rhoS(k)/sin(waveHeading);
          xyS(2,2,k) = -cos(waveHeading)/sin(waveHeading)*xMax    +  rhoS(k)/sin(waveHeading);
        else
          xyS(1,1,k) = -tan(waveHeading)        +  rhoS(k)/cos(waveHeading);
          xyS(2,1,k) = -tan(waveHeading)*yMax   +  rhoS(k)/cos(waveHeading);
          xyS(:,2,k) = [1 yMax]';
        end
        plot(hBlob2(m),xyS(:,1,k),xyS(:,2,k),'LineWidth',2,'Color','green');
      end
      set(hBlob2(m),'ydir','normal')
      
      
      %if( exist( 'edgeFigPtr' ) ~= 1 )
      if( 1 )
        %edgeFigPtr= figure;
        edgeFigPtr(m,1)= figure;
        set( edgeFigPtr(m), 'Name', 'Edge Plot', 'DeleteFcn', 'clear edgeFigPtr(m)' );
        set(gcf,'position',position)
        if(exist('fontsize')) 
          scanFontsize=fontsize;
        else
          scanFontsize=22;
        end
        scanFontweight='normal';
        set(gca,'fontsize',scanFontsize,'fontweigh',scanFontweight)
        set(gcf,'defaultaxesfontsize',scanFontsize,'defaulttextfontsize', ...
            scanFontsize)
      else
        figure(edgeFigPtr);
      end
      clf
      %hImage = imshow(VB,[],'xdata',[-500 500],'ydata',[-500 500]);
      hImage = imshow(VB);
      hEdge2(m) = imgca(edgeFigPtr(m));
      set(hEdge2(m),'visible','on');
      set(edgeFigPtr(m),'position',position)
      if( meshRes == 1 )
        xlabel(hEdge2(m),'Meters East');
        ylabel(hEdge2(m),'Meters North');
      else
        xlabel(hEdge2(m),'Pixels East; Two Meters/Pixel');
        ylabel(hEdge2(m),'Pixels North; Two Meters/Pixel');
      end
      
      %title('Edges')
      title(hEdge2(m),sprintf('Scan %d, Data Set %s, Gain = %d', m, dataSet, gain));
      set(hEdge2(m),'ydir','normal')
      hold(hEdge2(m))

      for( k=1:length(theta) )
        plot(hEdge2(m),xy(:,1,k),xy(:,2,k),'LineWidth',2,'Color','green');
      end
      fprintf('End plot\n');
    end %plotLevel

    %
    % Convert rho0 from pixels to meters:
    rho0 = rho0*meshRes;
    %
    % Compute crest locations and wavelengths
    [wdist  indx] = sort(rho0');
    wlengths = diff(wdist);
    %
    % Discard waves that are too close to the preceding wave:
    indxw  = find(wlengths > 50);
    
    wdistP     = wdist([1; indxw+1]);
    wlengthsP  = diff(wdistP);

    indx2 = [1; indxw+1];

    processedScan{m,1} = stime(end);
    processedScan{m,2} = wdistP;
    
    %
    % Find closest incoming wave:
    for( i=1:length(wdistP))
      if( wdistP(i) > 0 )
        j=i-1;
        break;
      end
      j=i;
    end

    dNext = wdistP(j);

    
    if(meshRes == 1)
      textXoffset = 1050;
      textYoffset1 = 700;
      textYoffset2 = 300;
      textYoffset3 = 50;
    else
      textXoffset = 520;
      textYoffset1 = 300;
      textYoffset2 = 150;
      textYoffset3 = 150;
    end

    
    %
    % Now repeat this with Sharon's algo:
    wdistS     = rho0S(rho0SIndx);
    wlengthsS  = diff(wdistS);
    
    dNextS = wdistS(end);
    
    processedScan{m,3} = wdistS';
    %
    % Annotate Sharon plot:

    if( plotLevel )
      textnumstr = sprintf('%11.1f %11.1f\n',[wdistS' [NaN; wlengthsS']]'); ...

    textstr = sprintf(...
        [ 'Distance,m  Wavelength,m \n',...
          '%s\n',... 
          'Median wave length = %.1f meters\n',... 
          'Wave heading = %.1f Degrees\n'],... 
        textnumstr,...
        median(wlengthsS),...
        waveHeadingDeg);
    
        text(hBlob2(m),textXoffset,textYoffset1, textstr,'color','g');
    end
  %
  % Now compute wave speed using xcorr:
  if( m > 1 )
    [xcf,lags] = xcorr(lastsigsum,sigsum);
    [M,ind] = max(xcf); 
    %disp("Cross Correlation peak lag for Scan " + m + " is : " + lags(ind) + " m.");
    fprintf('Cross Correlation peak lag for Scan %d is %.1f m.\n', m ,lags(ind));

    cpmS = lags(ind)/(processedScan{m-1,1} - stime(end));
    
    fprintf('Correlation speed = %.1f m/sec.\n\n', cpmS);

    if( cpmS > 0 )
      tArrivalS = -dNextS/cpmS;
    end
    
  end
  lastsigsum = sigsum;
  
  
  %
  % Deep water dispersion:
  %cp =  sqrt( 9.806 * median(wlengthsP) /2/pi);
  %
  % Try the image translation method of computing speed:
  increment = 2;
  nFrames = 3;
  if( m < nFrames )
    % Deep water dispersion:
    cp =  sqrt( 9.806 * median(wlengthsP) /2/pi);
    cpsave = [cp;cpsave];
    fprintf('Dispersion speed = %.2f m/s, scan %d.\n',cp,m)
    cpm = median(cpsave);
  end

  firstFrame = 1;
  lastFrame  = 25;
  mLast = 1;
  if( ~mod(m,nFrames) )
    for( i=firstFrame:lastFrame )
      x =  i*increment*cos(-waveHeadingDeg);
      y = -i*increment*sin(-waveHeadingDeg);
      VBtran = imtranslate(VBbsave{mLast},[x y]);
      VBand = VBbsave{m}.*VBtran;
      %figure;
      %imshow( VBand );
      interCount(i,1) = sum(VBand,'all');
      fprintf('Initial scan displaced %d, intersection = %d\n',i*increment, ...
          interCount(i));
    end
    [val indxMax]=max(interCount);
    if( indxMax > firstFrame && indxMax < lastFrame )
      dist = indxMax*increment;
      %deltat =  processedScan{nFrames,1} - processedScan{1,1}
      deltat =  processedScan{m,1} - processedScan{mLast,1}
      if( deltat > 0 )
        cp = dist/deltat;
        cpsave = [cp;cpsave];
        fprintf('Image Sliding speed = %.2f m/s, scan %d.\n',cp,m)
      end
      if(length(cpsave) < 5)
        cpm = median(cpsave);
      else
        cpm = median(cpsave(1:5));
      end
      % Skip median:
      cpm = cp;
    else
      cpm = NaN;
    end
    mLast = m;
  end
  
  tArrival = -dNext / cpm;
  
  %
  % Compute headings in NED coordinates:
  headingM =  pi/2 - median(theta(indx));
  headingP =  pi/2 - median(theta(indx(indx2)));
  waveHeadingNED = pi/2 - waveHeading;
  %
  % Convert from "wave going" direction to "wave from".
  if( headingM < pi ) 
    headingM = headingM + pi;
  else
    headingM = headingM - pi;
  end
  if( headingP < pi ) 
    headingP = headingP + pi;
  else
    headingP = headingP - pi;
  end
  if( waveHeadingNED < pi ) 
    waveHeadingNED = waveHeadingNED + pi;
  else
    waveHeadingNED = waveHeadingNED - pi;
  end
  
  if( plotLevel )
    for( k=1:length(indx2) )
      plot(hEdge2(m),xy(:,1,indx(indx2(k))),xy(:,2,indx(indx2(k))),'LineWidth',2,'Color','magenta');
    end
    
    fprintf('\n\n');
    fprintf('Scan %d\n', m);
    fprintf('Lines with normals that exceed %d +/- %d degrees with be excluded.',...
        waveHeadingDeg, exclusion)
    fprintf( 'Distance, m  Wavelength, m Heading,deg \n' );
    fprintf('%11.1f %11.1f %11.1f\n',[wdist [NaN; wlengths] theta(indx)'*180/pi ]');
    fprintf('Median wavelength = %.1f\n', median(wlengths));
    fprintf('Median wave heading = %.1f\n', median(theta(indx)'*180/pi));
    %fprintf('Median wave heading = %.1f\n', headingM*180/pi);

    textnumstr = sprintf('%11.1f %11.1f %11.1f\n',[wdist [NaN; wlengths] ...
          theta(indx)'*180/pi ]');
    %         wrapTo360(pi/2-theta(indx)'*180/pi) ]');

    exclstr = sprintf([ '\nExcluding all Waves with headings \n',...
          'greater or less than %d +/- %d degrees.\n',...
          'Headings in ENU coordinates.\n\n'],...
        waveHeading, exclusion);

    %    exclstr = sprintf([ '\nExcluding all Waves with headings \n',...
    %       'greater or less than %.1f +/- %d degrees.\n\n'],...
    %       waveHeadingNED, exclusion);
        
    textstr = sprintf(...
        [ exclstr,...
          'Distance, m  Wavelength, m Heading,deg \n',...
          '%s\n',... 
              'Median wave length = %.1f\n',... 
              'Median wave heading = %.1f\n'],... 
            textnumstr,...
            median(wlengths),...
            median(theta')*180/pi);
        
    % if(meshRes == 1)
    %   textXoffset = 1050;
    %   textYoffset1 = 700;
    %   textYoffset2 = 300;
    %   textYoffset3 = 50;
    % else
    %   textXoffset = 520;
    %   textYoffset1 = 300;
    %   textYoffset2 = 150;
    %   textYoffset3 = 150;
    % end
    
    text(hEdge2(m),textXoffset,textYoffset1, textstr,'color','g');

    textnumstr2 = sprintf('%11.1f %11.1f %11.1f\n',[wdistP [NaN; wlengthsP] theta(indx(indx2))'*180/pi ]');
      
      textstr2 = sprintf(...
          [ '\nAdditionally excluding all waves with\n',...
            'wavelength < 50 meters.\n\n',...
            'Distance, m  Wavelength, m Heading,deg \n',...
            '%s\n',... 
            'Median wave length = %.1f\n',... 
            'Median wave heading (Hough Frame) = %.1f\n'],... 
          textnumstr2,...
          median(wlengthsP),...
          median(theta(indx(indx2))'*180/pi));
      fprintf('%s\n',textstr2)
      text(hEdge2(m), textXoffset, textYoffset2, textstr2,'color','m');

      if( m < nFrames )

        textstr3 = sprintf([...
              'Next wave is %.1f meters out.\n',... 
              'Wave speed is %.1f meters/sec.\n',... 
              'Arrival predicted in %.1f seconds.\n',...
              'From deep water dispersion relation.\n'],dNext, cp, tArrival);
      else
        textstr3 = sprintf([...
              'Next wave is %.1f meters out.\n',... 
              'Wave speed is %.1f meters/sec.\n',... 
              'Arrival predicted in %.1f seconds.\n',...
              'From image sliding method.\n'],dNext, cpm, tArrival);
      end
      text(hEdge2(m), textXoffset, textYoffset3, textstr3);
      %
      % Also annotate Sharon plot:
      if( m > 1 )
        textstr2S = sprintf([...
              'Next wave is %.1f meters out.\n',... 
              'Corr Wave speed  %.1f m/s, arrival %.1f s.\n',... 
              'Im Sld Wave speed %.1f m/s, arrival in %.1f s.\n'],...
            dNextS, cpmS, tArrivalS, cpm, tArrival);
        text(hBlob2(m), textXoffset, textYoffset2, textstr2S);
      end    
      
      
      
      if(saveFigs)
        fileName = sprintf('Scan%d-%s',m,dataSet);
        %savefig(edgeFigPtr(m), fileName, 'compact');
        savefig(blobFigPtr(m), fileName, 'compact');
      end

      fprintf('Cartesian converstion took %.2f sec.\n',tstop1);
      fprintf('Interpolation to Cartesian grid took %.2f sec.\n',tstop2);
      fprintf('Hough Transform and peak detection took %.2f sec.\n',tstop3);
      fprintf('Total time including plotting took %.2f sec.\n',toc(tstart));
      
      indx = find(nav440(:,1)-scan(m).pulse(1).time>0);
      startIndx = indx(1);
      
      indx = find(nav440(:,1)-scan(m).pulse(end).time>0);
      stopIndx = indx(1);

      indx = find(nav440(:,1)-(tArrivalS+scan(m).pulse(end).time)>0);
      arrivIndx = indx(1);
      
      %plot(waveFigAxes,scan(m).pulse(end).time+tArrival,zz50(arrivIndx),'rd')
      plot(waveFigAxes,nav440(arrivIndx,1),zz50(arrivIndx),'rd','markersize',10)
      text(waveFigAxes,nav440(arrivIndx,1),zz50(arrivIndx)+.05,...
          sprintf('S%d',m), 'color','r');

      plot(waveFigAxes,nav440(stopIndx,1),zz50(stopIndx),'x')
      text(waveFigAxes,nav440(stopIndx,1),zz50(stopIndx)+.05,...
          sprintf('S%d %.2f',m,tArrivalS));

      axis(waveFigAxes,'normal');

      
      figure(blobFigPtr(m));
      %pause
      %break;
    end


    if(saveFigs)
      fileName = sprintf('SurfaceElevation%s',dataSet);
      savefig(waveFigPtr, fileName, 'compact');
    end

  end  %plotLevel
  
  
  
  

  
  
  
  
  
  
