%
% 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  
%
% If there are more than 3 crests detected, take the furthest 3.
for( i=1:Mt)
  processedScan2(:,i) = processedScan{i,end}(1:Mc);
end
%
% Matlab appears to be truncating processedScan2 to integers.  Why?
%
% Save processedSac2 as a csv file:
csvName = 'CrestDist222253.csv';
%fid = fopen(csvName,'w+');
%fprintf(fid, 'Each row is a wave crest location in meters.\n');
%fprintf(fid, 'Each column is a radar scan.\n');
%fclose(fid)
csvwrite(csvName,processedScan2);
% Compute phases from crests.  alpha is a vector of phases.

if( ~exist('useMatlab') ) useMatlab = 1; end;
if( ~exist('plotOnly') )  plotOnly = 0; end;

if( useMatlab && ~plotOnly)

  tic
  alpha = crest_wave_profile_mbari_multi1_pared_mex(processedScan2);

  %alpha = csvread('Alphas.csv');

  %load alphasSave9806.mat;
  %fprintf('alpha - alphasSave = %.2f.\n\n',norm(alpha - alphasSave))
  %return

  % Predict sea-surface elevation tpred seconds ahead.
  params.N1     = 128;
  params.Nm     = 8;
  params.Nt     = 1024;
  params.oml    = 0.18;
  params.omh    = 1.57;
  params.dpract = 475.0;
  params.beta1  = .74;
  params.Hs     = 1.83;
  params.Te     = 10.0;
  params.ti     = 0.0;
  params.tf1    = 5859.7376;
  params.N      = 4096;
  params.tcut   = 500.0;
  params.tend   = 2400.0;

  [out]= prop_impresp_eta_30s_pred_act_fixedtime_mbari_relt_a_wdispV2( alpha, params );

  toc

  if( exist('outSave.mat')==2 )
    system('mv outSave.mat outSaveLast.mat')
    fprintf('Moving outSave.mat to outSaveLast.mat\n');
  else
    fprintf('outSave.mat not found.\n');
  end
  fprintf('Saving output to outSave.mat.\n');
  outSave = out;
  save('outSave.mat', 'outSave');
  clear outSave;

else

  if( ~plotOnly )
    fprintf('Running predict.cc\n')
    tic
    system('./predict')
    toc
  end

  BuoyOut = csvread('BuoyOut.csv');
  SeaSurfaceOut = csvread('SeaSurfaceOut.csv');
  out.x     = BuoyOut(:,1);
  out.he    = BuoyOut(:,2);
  out.ho    = BuoyOut(:,3);
  out.etap  = SeaSurfaceOut(:,1);
  out.etaRc = SeaSurfaceOut(:,2);
  out.fclr  = SeaSurfaceOut(:,3);
  out.fcla  = SeaSurfaceOut(:,4);
  out.pwra  = SeaSurfaceOut(:,5);
  out.pwrc  = SeaSurfaceOut(:,6);
  out.velnc = SeaSurfaceOut(:,7);
  out.tp    = SeaSurfaceOut(:,8);

  %
  % Check against Matlab version:
  if( exist('outSave.mat') == 2 )
    load('outSave')

    fprintf('The numbers below are percent average error, and maximum percent error.\n')
    
    
    indx = find( out.x ~= 0 );
    ave    = mean( (out.x(indx) - outSave.x(indx))./out.x(indx) )*100;
    maxerr = max( (out.x(indx) - outSave.x(indx))./out.x(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in x = [%g %g].\n', ave, maxerr);

    indx = find( out.he ~= 0 );
    ave    = mean( (out.he(indx) - outSave.he(indx))./out.he(indx) )*100;
    maxerr = max( (out.he(indx) - outSave.he(indx))./out.he(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in he = [%g %g].\n', ave, maxerr);

    ave    = mean( (out.ho - outSave.ho)./out.ho )*100;
    maxerr = max( (out.ho - outSave.ho)./out.ho )*100;
    fprintf('[%% Average, %% Max ] Error in ho = [%g %g].\n', ave, maxerr);

    ave    = mean( (out.etaRc - outSave.etaRc)./out.etaRc )*100;
    maxerr = max( (out.etaRc - outSave.etaRc)./out.etaRc )*100;
    fprintf('[%% Average, %% Max ] Error in etaRc = [%g %g].\n', ave, maxerr);

    ave    = mean( (out.etap - outSave.etap)./out.etap )*100;
    maxerr = max( (out.etap - outSave.etap)./out.etap )*100;
    fprintf('[%% Average, %% Max ] Error in etap = [%g %g].\n', ave, maxerr);

    indx = find( out.fclr ~= 0 );
    ave    = mean( (out.fclr(indx) - outSave.fclr(indx))./out.fclr(indx) )*100;
    maxerr = max( (out.fclr(indx) - outSave.fclr(indx))./out.fclr(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in fclr = [%g %g].\n', ave, maxerr);

    indx = find( out.fcla ~= 0 );
    ave    = mean( (out.fcla(indx) - outSave.fcla(indx))./out.fcla(indx) )*100;
    maxerr = max( (out.fcla(indx) - outSave.fcla(indx))./out.fcla(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in fcla = [%g %g].\n', ave, maxerr);

    indx = find( out.pwra ~= 0 );
    ave    = mean( (out.pwra(indx) - outSave.pwra(indx))./out.pwra(indx) )*100;
    maxerr = max( (out.pwra(indx) - outSave.pwra(indx))./out.pwra(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in pwra = [%g %g].\n', ave, maxerr);

    indx = find( out.pwrc ~= 0 );
    ave    = mean( (out.pwrc(indx) - outSave.pwrc(indx))./out.pwrc(indx) )*100;
    maxerr = max( (out.pwrc(indx) - outSave.pwrc(indx))./out.pwrc(indx) )*100;
    fprintf('[%% Average, %% Max ] Error in pwrc = [%g %g].\n', ave, maxerr);
    
    figure(1);
    %if( exist('testfig') )
      hold on
      plot(gca, outSave.x, outSave.he, '--k')
    %end

    figure(5);
    %if( exist('testfig') )
      hold on
      plot(gca, outSave.tp, outSave.etap, '--k')
      axis([500,600,-1.0,1.0]);
    %end
    
  else
    fprintf('Can''t check against Matlab:  outSave.mat not found.\n')
  end  

end



% Unpack the struct:
% ppfields = fieldnames(outputs);
% for( ii=1:length( ppfields ) )
%   eval([ppfields{ii} '=' num2str(outputs.(ppfields{ii})) ';']);
% end


set(0,'DefaultAxesFontName','Arial')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultAxesColorOrder',[1 0 0;0 0 1; 0 0 0],'DefaultAxesLineStyleOrder','-|--|-.|:|.')

figure(1)
plot(out.x,out.he,'LineWidth',2)
xlabel('x (m)','FontSize',16)
ylabel('h_e(x)','FontSize',16)
grid on
%
figure(2)
plot(out.x,out.ho,'LineWidth',2)
xlabel('x (m)','FontSize',16)
ylabel('h_o(x)','FontSize',16)
grid on
%
% fname1 = 'hlx_plot';
% figure(3)
% %plot(t,ho,'LineWidth',2)
% plot(xx,hlx,'o','LineWidth',2)
% xlabel('x (m)','FontSize',16)
% ylabel('h_l(x)','FontSize',16)
% grid on
% saveas(gcf,fname1,'jpeg');
% saveas(gcf,fname1,'epsc')
%
%
fname1 = 'hlx_plot';
figure(4)
plot(out.x,out.he+out.ho,'LineWidth',4)
xlabel('x (m)','FontSize',16)
ylabel('h_l(x)','FontSize',16)
grid on
%saveas(gcf,fname1,'jpeg');
%saveas(gcf,fname1,'epsc')
fname1 = 'pred_compx1';
figure(5)
plot(out.tp,out.etaRc,'-',out.tp,out.etap,'o','LineWidth',4)
axis([500,600,-1.0,1.0]);
xlabel('t','FontSize',16)
ylabel('\eta_{computed}, \eta_{pred}','FontSize',16)
legend('Computed','Predicted with \eta snapshot','location','best')
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
fname1 = 'pred_resis';
figure(7)
plot(out.tp,out.fclr,'-r','LineWidth',4)
axis([500 800,-1.5e4,1.5e4]);
xlabel('Time','FontSize',16)
ylabel('Resistive Force (N)','FontSize',16)
%legend('Computed','Predicted with \eta snapshot','location','best')
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
% 
%
fname1 = 'pred_reac';
figure(8)
%
plot(out.tp,out.fcla,'-b','LineWidth',4)
axis([500 800,-500,500]);
xlabel('Time','FontSize',16)
ylabel('Reactive Force (N)','FontSize',16)
%legend('Computed','Predicted with \eta snapshot','location','best')
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
%
fname1 = 'power_reac_pred';
figure(9)
plot(out.tp,out.pwra,'-k','LineWidth',3)
axis([500 800 0 4e5])
xlabel('Time','FontSize',16)
ylabel('Reactive Power (W)','FontSize',16)
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
%
%
%
fname1 = 'power_conv_pred';
figure(9)
plot(out.tp,out.pwrc,'-k','LineWidth',3)
axis([500 800 0 4e5])
xlabel('Time','FontSize',16)
ylabel('Converted Power (W)','FontSize',16)
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
%
%
fname1 = 'buoy_heave_vel1';
figure(10)
plot(out.tp,out.velnc,'-r','LineWidth',3)
axis([800 1200 -1.0 1.0])
xlabel('Time','FontSize',16)
ylabel('Heave velocity (no control) (m)','FontSize',16)
grid on
%saveas(gcf,fname1,'jpeg')
%saveas(gcf,fname1,'epsc')
