%clear; close all;

  if( exist('morePlots')==0 ) morePlots=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 = '222222';  gain = 70;  
  dataSet = '200516';  gain = 75;  
  %
  % 
  loadStr = sprintf('radarData%s.mat',dataSet)
  pathStr = sprintf('radarFiles/%s',loadStr);
  if( ~exist('scan') )  load(pathStr), end    

  if( ~exist('nav440') ) load('Nav440Processed28Sept2020'), end
  
  
  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
  
  
  
  for m = 1:length(scan)

    if( m == 8 && 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;
	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);
    
    title(['Scan ' num2str(m)]);
    axis equal;

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

    VBorig = VB;

    % Compute average ship heading during this scan (2.5 seconds)
    psi = mean(sazimuth'-spangle') * pi/180;

    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 = round( r*cos(az) );
	  yb = round( r*sin(az) );
	  VB( yb + origin, xb + origin ) = 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.
    waveHeading = -66;  %Degrees
    exclusion = 5;      %Degrees
    
    [H, T, R] = hough(VB,'theta', waveHeading-exclusion:waveHeading+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

    
    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
      
    %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, loadStr, 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');
    toc

    %
    % Convert rho0 from pixels to meters:
    rho0 = rho0*meshRes;
    
    
    [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];

    %
    % Find closest incoming wave:
    for( i=1:length(wdistP))
      if( wdistP(i) > 0 )
	j=i-1;
	break;
      end
      j=i;
    end

    dNext = wdistP(j);
    
    %
    % Deep water dispersion:
    cp =  sqrt( 9.806 * median(wlengthsP) /2/pi);
    
    tArrival = -dNext / cp;
    
    %
    % 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.',waveHeading,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 = %.1f\n'],... 
	textnumstr2,...
	median(wlengthsP),...
	median(theta(indx(indx2))'*180/pi));
    fprintf('%s\n',textstr2)
    text(hEdge2(m), textXoffset, textYoffset2, textstr2,'color','m');

    textstr3 = sprintf([...
	  'Next wave is %.1f meters out.\n',... 
	  'Arrival predicted in %.1f seconds.\n',...
	'From deep water dispersion relation.\n'],dNext, tArrival);
    text(hEdge2(m), textXoffset, textYoffset3, textstr3);

    if( 0 )
      %
      % Trying to fix problem of image resizing when raising an earlier figure/image.
      xlim = [1 size(VB,1)];
      ylim = [1 size(VB,2)];
      set(hEdge2(m),'xlim',xlim);
      set(hEdge2(m),'ylim',ylim);
    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));
    
    
    pause
    %break;
  end
