%
% PURPOSE:  Load and process radar data from Paragon.
%
% CHANGE LOG: 1) Rob McEwen, initial pass.  Included code from Eric Martin.
%
%clear; close all;

  if( exist('morePlots')==0 ) morePlots=0;end
  if( exist('saveFigs')==0 )  saveFigs=0;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;


  %if( ~exist('scan') )  load radarData200516.mat, end
  %if( ~exist('scan') )  load radarData.mat, end
  %
  % On 28 Sept 2020 it was PDT, so UTC-7=local.
  % 
  %15:22 PDT:  Not on Eric's plot
  %if( ~exist('scan') )  load radarFiles/radarData222222.mat, end   
  %
  dataSet = '210231';  gain = 73;
  dataSet = '202036';  gain = 62;
  dataSet = '221048';  gain = 76;
  dataSet = '200516';  gain = 75;  
  dataSet = '222222';  gain = 70;  
  dataSet = '222253';  gain = 70;  
  %
  % 
  loadStr = sprintf('radarData%s.mat',dataSet)
  pathStr = sprintf('radarFiles/%s',loadStr);
  if( ~exist('scan') )  load(pathStr), end    

  if( ~exist('nav440') ) load('Nav440Processed28Sept2020'), end

  if( exist( 'waveFigPtr' ) ~= 1 )
    waveFigPtr= figure;
    set( waveFigPtr, 'Name', 'Wave 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;
  
  
  plot(nav440(startScanIndx:endScanIndx,1), zz50(startScanIndx:endScanIndx));
  waveFigAxes=gca;
  grid on
  hold on
  xlabel('Unix Time, Seconds')
  ylabel('Surface elevation, meters')
  title(sprintf('Surface Elevation for Data Set %s, Gain %d',dataSet, gain));
  
  
  fprintf('Number of scans is %d.\n',length(scan));

  %
  % Synchronize Ahrs with radar:
  %if( nav440(1,1) > scan(1).pulse(1).time )
  %  for( i=1:
  %else
  
  cpsave = [];
  tArrivalS = 0;
    
  for m = 1:length(scan)
  %for m = 1:10

    if( m == 8  && strcmp(dataSet,'222222')) continue; end
    if( m == 20 && strcmp(dataSet,'222222')) continue; end
    
    pulses = scan(m).pulse;

    if( morePlots )
    
    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:length(pulses) )
      stime(n)    = pulses(n).time;
      sazimuth(n) = pulses(n).azimuth;
      spangle(n)  = pulses(n).pointingAngle;
    end

    sazimuth = unwrap(sazimuth);
    spangle  = unwrap(spangle);
    
    
    for n = 1:length(pulses)
        pulse = pulses(n);
        if( ~mod(n,100) )
          fprintf('Processing pulse %d/%d at azimuth = %.2f degrees.\n',n,length(pulses),sazimuth(n));
        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, length(pulse.radarVideo)) * sazimuth(n) * pi/180;
        heading = ones(1, length(pulse.radarVideo)) * spangle(n)  * pi/180;
        
        % range is self described, convert to array
        range = linspace(pulse.startRange, pulse.endRange, length(pulse.radarVideo));
        
        %Limit the range:
        maxRange=500;
        nStop = round(length(pulse.radarVideo)*maxRange/range(end));
        
        % convert to cartesian
        %[x, y] = pol2cart(azimuth, range);
        %[x, y] = pol2cart(azimuth(1:nStop), range(1:nStop));
        [x, y] = pol2cart(heading(1:nStop), range(1:nStop));

        %Stack all the data in colums:
        X = [X; x'];
        Y = [Y; y'];
        V = [V; pulse.radarVideo(1:nStop)'];

        % plot and map colors to values
        %scatter3(x,y,pulse.radarVideo,3,pulse.radarVideo);
        %handle=scatter(x,y,3,pulse.radarVideo);
        if( morePlots )
          handle=scatter(x,y,3,pulse.radarVideo(1:nStop));
          hold on;
          title(['Scan ' num2str(m)]);
          axis equal;
        end
        %figure(scanFigPtr);
        %pause
    end
    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);

    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( morePlots )
      fprintf('Start plot\n');

      if 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
    end

    %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(sazimuth'-spangle') * pi/180;
    fprintf('Ship heading = %.1f degrees.\n',psi*180/pi);

    % Blank out the bad sector before creating the edge image.
    if( 1 )
      
      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');
    figure;
    plot(sigsum)
    title(['Wave Locs for Scan ' num2str(m)])
    grid on
    hold on
    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.
    plot(maxIndices,sigsum(maxIndices), '^','Color',[217 83 25]/255,...
    'MarkerFaceColor',[217 83 25]/255,'DisplayName','Local maxima');
    %
    % Wave crest locations:
    rho0S = maxIndices - midIndx;
    rho0SIndx = find(rho0S' < 0);
    nwavesS = max(rho0SIndx);
    
    
    
    
    if( morePlots )
      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')
      
      
      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  %morePlots

    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

    
    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( morePlots )
        plot(hEdge,xy(:,1,k),xy(:,2,k),'LineWidth',2,'Color','green');
      end
    end

    %
    % Plot Sharon's lines:
    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');

    %
    % 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:

    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');

    %
    % 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
    
    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

    
    
    
    

    
    
    
    
    
  
