#include "mex.h"
#include "mat.h"
#include "matrix.h"
#include <math.h>

#define SUB2IND_2D(s1, s2, i1, i2) ((s1)*(i2) + (i1))

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
   double g = 9.806;
// double g = 9.8;
   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
//
   int Nf = 128*2*2;   //Number of points in the frequency spectrum
   int Mf = Nf;
   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;
   mxDouble *mxGetDoubles(const mxArray *pa);
//   
// Read in input array:
   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;


   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 ) mexPrintf("omeg[%d] = %.4f\n",mf,omeg[mf]);
   }

   for( mf = 0; mf < Mf; mf++ )
   {
      k[mf] = omeg[mf]*omeg[mf]/g;
//    if( mf >= Mf-10 ) mexPrintf("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);

// 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]);
   }

}