%
% PURPOSE:  Read in radar and AHRS data, predict sea surface elevation at the buoy.
% DATE:     21 April 2021
% 
% Some possibilities:
% radarData = '210231';  %gain = 73;
% radarData = '202036';  %gain = 62;
% radarData = '221048';  %gain = 76;
% radarData = '200516';  %gain = 75;  
% radarData = '222222';  %gain = 70;  
% radarData = '222253';  %gain = 70;  

radarData = '222253';  gain = 70;
ahrsData  = 'Nav440Processed28Sept2020';
if(~exist('plotLevel')) 
  plotLevel = 0;
end
if(~exist('recompute')) 
  recompute = 0;
end
%
% Find the wave-crest locations:
if( exist('processedScanSave') ~= 1)
  if( exist('processedScanSave.mat') ~= 2 || recompute )
    [processedScan, scan, nav440, zz50, radar_z_dot_NED] = processRadar(ahrsData, radarData, plotLevel);
    save( 'processedScanSave', 'processedScan', 'scan', 'nav440', 'zz50', 'radar_z_dot_NED');
  else
    fprintf('Loading precomputed scan data.\n');
    load('processedScanSave');
  end
end
%
% Reformat processedScan for Umesh.
%
% processedScan: 25 x 3:
%    rows are radar scans.
%    columns: 1 - time of the scan.
%             2 - nx1 array of crest locations computed by Hough, meters.
%             3 - mx1 array of crest locations computed by perpendicular sum, meters.
%
% processedScan2: 3 x 8:
%    each row is a wave crest location in meters.
%    each column is a radar scan.
%
Mc = 3;         %Number of crests
Mt = 8;         %Number of scans  

for( i=1:Mt)
  processedScan2(:,i) = processedScan{i,end}(1:Mc);
end
%
% Compute phases from crests.  alpha is a vector of phases.
alpha = crest_wave_profile_mbari_multi1_pared_mex(processedScan2);
load alphasSave.mat;
fprintf('alpha - alphasSave = %.2f.\n\n',norm(alpha - alphasSave))
return
%
% Predict sea-surface elevation tpred seconds ahead:
[etap velnc tp tpred]= prop_impresp_eta_30s_pred_act_fixedtime_mbari_relt_a_wdispV2( alpha );
%
% Plot the results:
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( '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;

startTime = nav440(startScanIndx,1);
%
% zz50 is the filtered buoy elevation computed from nav440, the raw AHRS
% data. The first column is time.
%
%plot(nav440(startScanIndx:endScanIndx,1)-startTime, zz50(startScanIndx:endScanIndx),'k');
plot(nav440(startScanIndx:endScanIndx,1)-startTime, radar_z_dot_NED(startScanIndx:endScanIndx),'k');
lstr = {};
%lstr(length(lstr)+1,1) = {'Buoy Elevation'};
lstr(length(lstr)+1,1) = {'Paragon Heave Rate'};
waveFigAxes=gca;
grid on
hold on
%
% Overlay scan times:
for(i=1:length(scan))
  scanTime(i,1) = scan(i).pulse(end).time - startTime;
  indx = find(nav440(:,1) - scan(i).pulse(end).time > 0 );
  indxRadar(i,1) = indx(1);
end
%plot(scanTime, zz50(indxRadar),'rx')
plot(scanTime, radar_z_dot_NED(indxRadar),'rx')
lstr(length(lstr)+1,1) = {'Scan Times'};
%set(gca,'xlim',[nav440(startScanIndx,1) 1601331874])
set(gca,'xlim',[0 80]);
%
% Overlay prediction:
%plot(tp+nav440(startScanIndx), etap,'b')
plot(tp, etap,'b')
lstr(length(lstr)+1,1) = {'Predicted \eta'};
plot(tp, velnc,'r')
lstr(length(lstr)+1,1) = {'Predicted Buoy Heave Rate'};

%xlabel('Unix Time, Seconds')
xlabel('Seconds')
ylabel('Surface elevation, meters')
title(sprintf('Heave Rate for Data Set %s, Gain %d',radarData, gain));
legend(lstr);
