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

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

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

int main()
{
//
   int error;
   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);

   //double processedScan2[Mc][Mt];

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

   return error;
}


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;
}