#include <stdio.h>
#include <math.h>
#include <string.h>

const int Nf = 128*2*2;   //Number of points in the frequency spectrum
const int Mf = Nf;
const int maxSize = 15;
const int nColsOut = 15;

struct constants
{
   int N;
   int N1;
   int Nm;
   int Nt;
   double oml;
   double omh;
   double dpract;
   double beta1;
   double Hs;
   double Te;
   double ti;
   double tf1;
   double tcut;
   double tend;
};

int crest_wave_profile_mbari_multi1_pared( double alpha[], int rows, int cols,
                                           int processedScan[maxSize][maxSize] ); 

int prop_impresp_eta_30s_pred_act_fixedtime_mbari_relt_a_wdispV2( double out[Mf][nColsOut], double alpha[],
                                                                  struct constants *params);

int main()
{
//
   int error = 0;
   double alpha[Mf];
   
//   
// Read in input array:
   const char *csvInName = "CrestDist222253.csv";
   const char *csvOutName = "Alphas.csv";
   FILE *FidIn  = fopen(csvInName, "r");
   FILE *FidOut = fopen(csvOutName,"w");
   
   int buflen = 1024;
   char buf[buflen];
   char *bufp = buf;
   int Mc=0, Mt=0;
   int num;
   int processedScan[maxSize][maxSize];
   int sret;
   char *token;
   
   while( fgets( buf,  buflen, FidIn) != NULL && Mc < maxSize )
   {
      Mt = 0;
      bufp = buf;
      printf("%s\n", buf);
      //while( (sret = sscanf(buf, "%d,", &processedScan[Mc][Mt] )) && Mt < maxSize )
      while( (token = strtok( bufp, ",")) != 0 )
      {
         processedScan[Mc][Mt] = atoi(token);
         printf("%d = ps[%d][%d], returned %d\n",processedScan[Mc][Mt], Mc, Mt, sret );
         Mt++;
         bufp = NULL;
      }
      Mc++;
   }
   printf(" Mc = %d, Mt = %d\n", Mc,Mt);

   if( Mc > maxSize )
   {
      printf("Mc = %d exceeds maximum array limit.\n",Mc);
      return -1;
   }
   if( Mt > maxSize )
   {
      printf("Mc = %d exceeds maximum array limit.\n",Mt);
      return -1;
   }

   error = crest_wave_profile_mbari_multi1_pared( alpha, Mc, Mt, processedScan );
   
   for( int i = 0; i < Mf; i++ )
   {
      fprintf(FidOut, "%g\n", alpha[i]);
   }

//
// Setup constants for the function below:

   constants params, *pptr;
   pptr = &params;

   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;




   
   return error;
}

int prop_impresp_eta_30s_pred_act_fixedtime_mbari_relt_a_wdispV2( double out[Mf][nColsOut], double alpha[],
                                                                  struct constants *params)
{
%function [etap, velnc, tp, tpred, x] = prop_impresp_eta_30s_pred_act_fixedtime_mbari_relt_a_wdispV2( alpha, params )
% This is the correct version. Both phases and magnitudes agree, as shown
% by predicted and computed wave records,and significant wave height
% comparisons. May 29, 2016.
% Impulse repsponse function for linear wave propagation, up-wave-down-wave
% causalization; fixed-time, snapshot measurement
%
%warning('off','all')
g = 9.806; 
rho = 1.025e3;

% Unpack the struct:
ppfields = fieldnames(params);
for( ii=1:length( ppfields ) )
  eval([ppfields{ii} '=' num2str(params.(ppfields{ii})) ';']);
end
%
% End constants
%
Nf = N1/2;
Nx = N1*2;
omeg = zeros(Nf,1);
x = zeros(Nx,1);
xx = zeros(2*Nx,1);
hlx = zeros(2*Nx,1);

k = zeros(Nf,1);
vg = zeros(Nf,1);
%
delom = (omh-oml)/Nf;
%
% Deep water approximation
%
for i = 1:Nf
    omeg(i,1) = oml + (i-1)*delom;
    k(i,1) = omeg(i,1)^2/g;
    vg(i,1) = 0.5*omeg(i,1)/k(i,1);
end %i
%
vgmin = min(vg);
vgmax = max(vg);
df = 0.0;
%
%
%To avoid dealing with small x values in denominators, set xl = 50.0 (can
%be arbitrary)
%

tpred = dpract/vgmax;
%
p = tpred;
%
dR = vgmin*tpred;
xl = dR*1;
xzero = xl;
xR = vgmax*tpred + xl;
xh = xR - dR;
ell = xh - xl;
xpos = ell+xl;
delx = (xpos - xzero)/Nx;

%
hl = zeros(Nx,1);
he = zeros(Nx,1);
ho = zeros(Nx,1);
%

for i = 1:Nx
 x(i,1) = xl + (i-1)*delx;
end
%
for i = 1:Nx
    xx(i,1) = -xl - (i-1)*delx;
    xx(i+Nx,1) = x(i,1);
end

%
for i = 1:Nx
    arg = g*p^2/(4*(xR-x(i,1)));
    cofx1 = sqrt(pi*g/(2*(xR-x(i,1))));

    cofx2 = p/(xR-x(i,1));
    cofx = cofx1*cofx2;
    csx = cos(arg) + sin(arg);
    he(i,1) = cofx*csx/(2*pi); % correct as of May 28
end %i
%
for i = 1:Nx
    arg1 = g*p^2/(4*(xR-x(i,1)));
    arg2 = sqrt(g/(xR-x(i,1)))*p/2;
    a1 = cos(arg1);
    b1 = sin(arg1);
    [a2,b2] = fresnelCS(arg2);
    cofx1 = sqrt(pi*g/(2*(xR-x(i,1))));
    cofx2 = p/(xR-x(i,1));
    cofx = cofx1*cofx2;
    ho1 = cofx*(a1*a2 + b1*b2);
    ho(i,1) = ho1/pi;
end %i

for i = 1:Nx
     hlx(i,1) = 0.0;
end %i

%
for i = 1:Nx
    hlx(i+Nx,1) = he(i,1) + ho(i,1);
end %i
% %
% use up-wave measurements over a time period to evaluate eta at xB at t;
% for a range of t values
% Generate eta at x = xR
%
format short e
% wave amplitude in spectrum
Mf = Nf;

% 2-parameter spectrum based on Newman 1977; Modified Pierson Moskowitz spectrum
enom = 2*pi/Te;
Ue = ((4/5)*beta1)^0.25*g/enom;
multcof = 2*131.5*Hs^2/Te^4;     %needed for Pierson Moskowitz spectral density calculation 
multarg = 1054/(Te^4);           %needed for Pierson Moskowitz spectral density calculation 
omin = min(omeg);
omax = max(omeg);

spec = zeros(Mf,1);              %needed for Pierson Moskowitz spectral density calculation 
amp = zeros(Mf,1);               %needed for Pierson Moskowitz spectral density calculation 
om = zeros(Mf,1);                %needed for Pierson Moskowitz spectral density calculation 
phase = zeros(Mf,1);             %needed for Pierson Moskowitz spectral density calculation 
etaf = zeros(Mf,1);
%
dom = omeg(2,1)-omeg(1,1);       %needed for Pierson Moskowitz spectral density calculation 
phase = 2*pi*rand(Mf,1);
%
for n = 1:Nf
    phase(n,1) = alpha(n,1);     %needed for Pierson Moskowitz spectral density calculation 
end

%
compi = complex(0,1);            %needed for Pierson Moskowitz spectral density calculation 
%
for m = 1:Mf
  om(m,1) = omeg(m,1);           %needed for Pierson Moskowitz spectral density calculation 
  k(m,1) = omeg(m,1)^2/g;        %needed for Pierson Moskowitz spectral density calculation 
  scof = multcof/(om(m,1)^5);    %needed for Pierson Moskowitz spectral density calculation 
  st = exp(-multarg/(om(m,1)^4));%needed for Pierson Moskowitz spectral density calculation 
  spec(m,1) = scof*st;
end %m
%
% spectral moment m_0
mom0 = 0.0;
sum = 0.0;
for m = 1:Mf
    sum = sum + spec(m,1);
end
amom0 = sum*dom/pi;
amom0
%


%needed for Pierson Moskowitz spectral density, amplitude calculation 
for m = 1:Mf;
    amp(m,1) = sqrt(2*spec(m,1)*dom); % use if simply summing varous frequency components to form irregular signal, not inv FT as here.
%   amp(m,1) = sqrt(spec(m,1)/2); % Inverse Fourier transformation with division by delt accounts for dom here.
end %m
%
%needed for Pierson Moskowitz spectral density calculation 

%
etaf1 = zeros(Mf,1);
%
%
for m = 1:Mf
    om(m,1) = omeg(m,1);
    k(m,1) = omeg(m,1)^2/g;
%   amp(m,1) = sqrt(spec(m,1)*dom/2); % use if simply summing varous frequency components to form irregular signal, not inv FT as here.
%   amp(m,1) = 0.5*sqrt(spec(m,1)); % Inverse Fourier transformation with division by delt accounts for dom here.
    etaf1(m,1) = amp(m,1)*exp(compi*phase(m,1));    %needed for Pierson Moskowitz spectral density calculation 
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%needed for wave-elevation map based on phases from crestline locations and along predominant direction 

etam = zeros(Nx,10*Nt,1);
etap = zeros(10*Nt,1);
etaps = zeros(10*Nt,1);


delt = (tf1-ti)/(10*Nt);
t = zeros(10*Nt,1);
tp = zeros(10*Nt,1);
%
for i = 1:10*Nt
    t(i,1) = ti + i*delt;
    %
    etam0 = 0.0;
    for j = 1:Nx
      etam0 = 0.0;
      for jf = 1:Mf
          etamxt = exp(-compi*(k(jf,1)*x(j,1)-om(jf,1)*t(i,1)));
          etam01 = real(etaf1(jf,1)*etamxt);
          etam0 = etam0 + etam01;
      end % jf
      etam(j,i,1) = etam0;
    end %j
end %i
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% wave elevation prediction based on etam and hlx
%
%etai = zeros(Nx,10*Nt,1);
for i = 1:10*Nt
    tp(i,1) = t(i,1) + p;
    etap0 = 0.0;
    for j = 1:Nx
        jj = j + Nx;
        etap0j = hlx(jj,1)*etam(j,i,1);
    etap0 = etap0+etap0j;
    end %j
    etap(i,1) = etap0*delx;
end % i
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
for i = 1:10*Nt
    etaRm0 = 0.0;
    for jf = 1:Mf
        etamxt = exp(-compi*(k(jf,1)*xR - om(jf,1)*tp(i,1)));
        etaRm01 = real(etamxt*etaf1(jf,1));
        etaRm0 = etaRm0 + etaRm01;
    end %jf
    etaRc(i,1) = etaRm0;
end %i
%
%
NN = 10*Nt;
%
%
for n = 1:NN
    tp(n,1) = double(n-1)*delt+p;
    tp(n,1) = t(n,1)+p;
end %n
%
tcf = 30.0;
%
Xcf = tcf/delt;
Ncf = int32(Xcf);
%
%
hrct = zeros(2*N,1);
hactt = zeros(2*N,1);
hactrt = zeros(2*N,1);
hldat = zeros(2*N,1);
hldrt = zeros(2*N,1);
hdct = zeros(2*N,1);
%
vro = zeros(NN,1);
vract = zeros(NN,1);
vractr = zeros(NN,1);
vracta = zeros(NN,1);
velnc = zeros(NN,1);
%
fcla = zeros(NN,1);
fclr = zeros(NN,1);
%
pwrc = zeros(NN,1);
pwra = zeros(NN,1);
%


load impresps_mbari.csv;
fname1 = impresps_mbari;
hrct = fname1(:,1);
hactt = fname1(:,2);
hactrt = fname1(:,3);
hldat = fname1(:,4);
hldrt = fname1(:,5);
hdct = fname1(:,6);

%
etap1 = zeros(NN,1);
%
for n = 1:NN
    
    etap0 = 0.0;
    for j = 1:Nx
        jj = j+Nx;
        etap0j = hlx(jj,1)*etam(j,n,1);
        etap0 = etap0+etap0j;
    end %j
    etap1(n,1)= etap0*delx;
    %
    if n > 3*Ncf
        sumvo = 0.0;
        sumvrt = 0.0;
        sumvat = 0.0;
        sumfla = 0.0;
        sumflr = 0.0;
        sumdo = 0.0;
        %
        for m = 1:3*Ncf
            sumvo = sumvo + hrct(N-Ncf+m,1)*etap1(n-m+1,1);
            sumvrt = sumvrt + hactrt(N-Ncf+m,1)*etap1(n-m+1,1);
            sumvat = sumvat + hactt(N-Ncf+m,1)*etap1(n-m+1,1);
            sumfla = sumfla - hldat(N-Ncf+m,1)*etap1(n-m+1,1);
            sumflr = sumflr + hldrt(N-Ncf+m,1)*etap1(n-m+1,1);
            sumdo = sumdo + hdct(N-Ncf+m,1)*etap1(n-m+1,1);
            %
        end
        %
        vro(n,1) = sumvo*delt;
        vractr(n,1) = sumvrt*delt;
        vracta(n,1) = sumvat*delt;
        fcla(n,1) = sumfla*delt;
        fclr(n,1) = sumflr*delt;
        velnc(n,1) = sumdo*delt;
    else
    end
end %n
%
%
vract = vractr + vracta;
%
for n = 1:NN;
    pwrc(n,1) = fclr(n,1)*vract(n,1);
    pwra(n,1) = fcla(n,1)*vract(n,1);
end
%
sumeta = 0.0;
NN1 = 0;
for n = 1:NN
    if abs(etap(n,1)) > 0
    sumeta = sumeta + etap(n,1)^2;
    NN1 = NN1 + 1;
    end
end
fNN = double(NN1);
Hs1 = 4.0*sqrt(sumeta/fNN);
%
sumeta1 = 0.0;
NN1 = 0;
for n = 1:NN
    if abs(etam(100,n,1)) > 0
    sumeta1 = sumeta1 + etam(100,n,1)^2;
    NN1 = NN1 + 1;
    end
end
fNN = double(NN1);
Hs3 = 4.0*sqrt(sumeta1/fNN);
%
etaRc = zeros(10*Nt,1);
etaRcs = zeros(10*Nt,1);
%
for i = 1:10*Nt
    etaRm0 = 0.0;
    for jf = 1:Mf
        etamxt = exp(-compi*(k(jf,1)*xR - om(jf,1)*tp(i,1)));
        etaRm01 = real(etamxt*etaf1(jf,1));
        etaRm0 = etaRm0 + etaRm01;
    end %jf
    etaRc(i,1) = etaRm0;
end %i
%
sumeta = 0.0;
NN1 = 0;
for n = 1:NN
    if abs(etaRc(n,1)) > 0
    sumeta = sumeta + etaRc(n,1)^2;
    NN1 = NN1 + 1;
    end
end
fNN = double(NN1);
Hs2 = 4.0*sqrt(sumeta/fNN);
%
etaps = etap1;
etaRcs = etaRc;
%
%
%

Ncut = int32(tcut/delt);
Nend = int32(tend/delt);
sumpc = 0.0;
sumpam = 0.0;
for n = 1:NN
    if (n > Ncut) && (n < Nend)
        sumpc = sumpc + pwrc(n,1);
        sumpam = sumpam + pwra(n,1);
    end
end
%
Nsum = Nend-Ncut;
%
pwrcav = sumpc/double(Nsum);
%

outputs.x          = x;
outputs.he         = he;
outputs.ho         = ho;
outputs.etap       = etap;
outputs.etaRc      = etaRc;
outputs.fclr       = fclr;
outputs.fcla       = fcla;
outputs.pwra       = pwra;
outputs.pwrc       = pwrc;
outputs.velnc      = velnc;
outputs.tp         = tp;
   
   return 0;
}







int crest_wave_profile_mbari_multi1_pared( double alpha[], int Mc, int Mt, int processedScan[maxSize][maxSize] ) 
{

   double g = 9.806;
   double rho = 1.025e3;
//
// alpha has dimension Mf.
//   
// Mc is the number of crests considered in each scan.
// Mt is the number of scans.
// processedScan is Mc x Mt
//
   double oml = 0.18;     //Low and high cut-off frequencies, radian/sec. 
   double omh = 1.57;
   double delom;
   //int Mc = 3;         //Number of crests
   //int Mt = 8;         //Number of scans  
   double delt = 1.26;    //time between scans + 1/2 the time needed for 1 scan.
   int mf, mc, mt;

/*

   size_t Mc = mxGetM(prhs[0]);
   size_t Mt = mxGetN(prhs[0]);
   mexPrintf("Mc = %d, Mt = %d\n", Mc, Mt);
   mxDouble *processedScan = mxGetDoubles(prhs[0]);
//
// Create intermediate variables:   
   mxDouble *omeg = mxGetDoubles(mxCreateDoubleMatrix(Mf,1,mxREAL));
   mxDouble *k    = mxGetDoubles(mxCreateDoubleMatrix(Mf,1,mxREAL));
   mxDouble *tm   = mxGetDoubles(mxCreateDoubleMatrix(Mt,1,mxREAL));
   mxDouble *xc   = mxGetDoubles(mxCreateDoubleMatrix(Mc,Mt,mxREAL));
   mxDouble *yc   = mxGetDoubles(mxCreateDoubleMatrix(Mc,Mt,mxREAL));
   mxDouble *Ac   = mxGetDoubles(mxCreateDoubleMatrix(Mf,1,mxREAL));
   mxDouble *Bc   = mxGetDoubles(mxCreateDoubleMatrix(Mf,1,mxREAL));
   mxDouble *C    = mxGetDoubles(mxCreateDoubleMatrix(Mf,1,mxREAL));
//   
// Create output array:
   plhs[0] = mxCreateDoubleMatrix(Nf, 1,  mxREAL);
   mxDouble *alpha = mxGetPr(plhs[0]);
   int i;
*/

   double omeg[Mf];
   double k[Mf];
   double tm[Mt];
   double xc[Mc][Mt];
   double yc[Mc][Mt];
   double Ac[Mf];
   double Bc[Mf];
   double C[Mf];
// double alpha[Nf];


   for(int i=0; i<Nf; i++) omeg[i] = 0.;

   delom = (omh-oml)/( (double) Nf );

   for( mf = 0; mf < Mf; mf++ )
   {
      omeg[mf] = oml + mf*delom;
      if( mf < 10 ) printf("omeg[%d] = %.4f\n",mf,omeg[mf]);
   }

   for( mf = 0; mf < Mf; mf++ )
   {
      k[mf] = omeg[mf]*omeg[mf]/g;
      if( mf >= Mf-10 ) printf("k[%d] = %.4f\n",mf,k[mf]);
   }

   for( mt = 0; mt < Mt; mt++ ) tm[mt] = 0.;

   for( mt = 0; mt < Mt; mt++ )  tm[mt] = mt*delt;
   
//
// fname1 = processedScan;

//xc = zeros(Mc,Mt,1);
//yc = zeros(Mc,Mt,1);

   int i;

   for( mc = 0; mc < Mc; mc++ )
      for( mt = 0; mt < Mt; mt++ )
         xc[mc][mt] = 0.;

   //for( i = 0; i<Mc*Mt; i++ )  xc[i] = 0.;

   for( mc = 0; mc < Mc; mc++ )
      for( mt = 0; mt < Mt; mt++ )
         yc[mc][mt] = 0.;

   //for( i = 0; i<Mc*Mt; i++ )  yc[i] = 0.;

   
 for( mc = 0; mc < Mc; mc++ )
 {
    for( mt = 0; mt < Mt; mt++ )
    {
       xc[mc][mt] = -processedScan[mc][mt];
       yc[mc][mt] = .5;
    }
 }

   // for( i = 0; i < Mc*Mt; i++ )
   // {
   //    xc[i] = -processedScan[i];
   //    yc[i] = .5;
   // }

   
   for( mf = 0; mf < Mf; mf++ )
   {
      Ac[mf] = 0.;
      Bc[mf] = 0.;      // set to unity
      alpha[mf] = 0.;   // phases
      C[mf] = 0.;       // amplitudes
   }

//

   size_t index;
   
   for( int n = 0; n < Mf; n++ )
   {
      double sumnum = 0.0;
      double sumden = 0.0;
      double sumnt, sumdt, cterm, sterm;
      int m;
      for( m = 0; m<Mc; m++ )
      {
         sumnt = 0.0;
         for( mt = 0; mt<Mt;  mt++ )
         {
            //index = SUB2IND_2D(Mc,Mt,m,mt);
            cterm = cos(k[n]*xc[m][mt] - omeg[n]*tm[mt]);
            //cterm = cos(k[n]*xc[index] - omeg[n]*tm[mt]);
            sumnt = sumnt + cterm;
         } //mt
         sumnum = sumnum + sumnt/Mt;
      } //m
      //
      for( m = 0; m<Mc; m++ )
      {
         sumdt = 0.0;
         for( mt = 0; mt<Mt;  mt++ )
         {
            //index = SUB2IND_2D(Mc,Mt,m,mt);
            sterm = sin(k[n]*xc[m][mt] - omeg[n]*tm[mt]);
            //sterm = sin(k[n]*xc[index] - omeg[n]*tm[mt]);
            sumdt = sumdt + sterm;
         } //mt
         sumden = sumden + sumdt/Mt;
      }
      //
      Ac[n] = sumnum/sumden;
      alpha[n] = atan2(1.0,Ac[n]);

   }

   return 1;
}