--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/ltpda_dft/ltpda_dft.c	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,559 @@
+/*
+ * Mex file that implements the core DFT part of the LPSD algorithm.
+ *
+ * M Hewitson  15-01-08
+ *
+ * $Id: ltpda_dft.c,v 1.16 2009/08/18 10:53:27 hewitson Exp $
+ */
+
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <mex.h>
+
+#include "ltpda_dft.h"
+#include "version.h"
+#include "../c_sources/polyreg.c"
+
+#define DEBUG 0
+
+
+/*
+ * Rounding function
+ *
+ */
+int myround(double x) {
+  return ((int) (floor(x + 0.5)));
+}
+
+
+/*
+ * Matlab mex file to make a dft at a single frequency
+ *
+ * Inputs
+ *  - data
+ *  - seg length
+ *  - DFT coefficients
+ *  - overlap (%)
+ *  - order of detrending
+ *
+ * function [P, navs] = ltpda_dft(x, seglen, DFTcoeffs, olap, order);
+ *
+ */
+void  mexFunction(  int nlhs,       mxArray *plhs[],
+        int nrhs, const mxArray *prhs[]) {
+  /* Parse inputs */
+  if( (nrhs == 0) && (nlhs == 0) ) {
+    print_usage(VERSION);
+    return;
+  }
+  else if ( (nrhs == 5) && (nlhs == 3) ) /* let's go */ {
+    double    Pr, Vr;
+    double   *xdata, *Cr, *Ci;
+    double    olap;
+    long int  nData;
+    long int  nSegs;
+    long int  segLen;
+    int       order;
+    double   *ptr;
+    
+    if( !mxIsComplex(prhs[2]) )
+      mexErrMsgTxt("DFT coefficients must be complex.\n");
+    
+    /* Extract inputs */
+    xdata     = mxGetPr(prhs[0]);                 /* Pointer to data */
+    nData     = mxGetNumberOfElements(prhs[0]);   /* Number of data points */
+    segLen    = (int)mxGetScalar(prhs[1]);        /* Segment length */
+    Cr        = mxGetPr(prhs[2]);                 /* Real part of DFT coefficients */
+    Ci        = mxGetPi(prhs[2]);                 /* Imag part of DFT coefficients */
+    olap      = mxGetScalar(prhs[3]);             /* Overlap percentage */
+    order     = (int)mxGetScalar(prhs[4]);        /* Order of detrending */
+    
+    if (order > 10 || order < -1)
+      mexErrMsgTxt("Detrending order must be between -1 and 10");
+    
+    /*mexPrintf("Input data: %d samples\n", nData);*/
+    /*mexPrintf("Segment length: %d\n", segLen);*/
+    /*mexPrintf("Overlap: %f\n", olap);*/
+    
+    /* Compute DFT */
+    dft(&Pr, &Vr, &nSegs, xdata, nData, segLen, Cr, Ci, olap, order);
+    
+    /* Set output matrices */
+    plhs[0] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
+    plhs[1] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
+    plhs[2] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
+    
+    /* Get pointers to output matrices */
+    ptr    = mxGetPr(plhs[0]);
+    ptr[0] = Pr;
+    ptr    = mxGetPr(plhs[1]);
+    ptr[0] = Vr;
+    ptr    = mxGetPr(plhs[2]);
+    ptr[0] = nSegs;
+    
+  }
+  else if ( (nrhs == 6) && (nlhs == 5) ) /* let's go */ {
+    double    Mr, Mi, XX, YY, M2;
+    double   *xdata, *ydata, *Cr, *Ci;
+    double    olap;
+    long int  nData, nxData, nyData;
+    long int  nSegs;
+    long int  segLen;
+    int       order;
+    double   *ptr;
+    
+    if( !mxIsComplex(prhs[3]) )
+      mexErrMsgTxt("DFT coefficients must be complex.\n");
+    
+    /* Extract inputs */
+    xdata     = mxGetPr(prhs[0]);                 /* Pointer to data */
+    ydata     = mxGetPr(prhs[1]);                 /* Pointer to data */
+    nxData    = mxGetNumberOfElements(prhs[0]);   /* Number of data points */
+    nyData    = mxGetNumberOfElements(prhs[0]);   /* Number of data points */
+    segLen    = (int)mxGetScalar(prhs[2]);        /* Segment length */
+    Cr        = mxGetPr(prhs[3]);                 /* Real part of DFT coefficients */
+    Ci        = mxGetPi(prhs[3]);                 /* Imag part of DFT coefficients */
+    olap      = mxGetScalar(prhs[4]);             /* Overlap percentage */
+    order     = (int)mxGetScalar(prhs[5]);        /* Order of detrending */
+    
+    if (order > 10 || order < -1)
+      mexErrMsgTxt("Detrending order must be between -1 and 10");
+    
+    if (nxData != nyData)
+      mexErrMsgTxt("The two input data vector should be the same length.");
+    
+    nData = nxData;
+    
+    /*mexPrintf("Input data: %d samples\n", nData);*/
+    /*mexPrintf("Segment length: %d\n", segLen);*/
+    /*mexPrintf("Overlap: %f\n", olap);*/
+    
+    /* Compute DFT */
+    xdft(&Mr, &Mi, &XX, &YY, &M2, &nSegs, xdata, ydata, nData, segLen, Cr, Ci, olap, order);
+    
+    /* Set output matrices */
+    plhs[0] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
+    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
+    plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
+    plhs[3] = mxCreateDoubleMatrix(1, 1, mxREAL);
+    plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL);
+    
+    /* Get pointers to output matrices */
+    ptr    = mxGetPr(plhs[0]);
+    ptr[0] = Mr;
+    ptr    = mxGetPi(plhs[0]);
+    ptr[0] = Mi;
+    ptr    = mxGetPr(plhs[1]);
+    ptr[0] = XX;
+    ptr    = mxGetPr(plhs[2]);
+    ptr[0] = YY;
+    ptr    = mxGetPr(plhs[3]);
+    ptr[0] = M2;
+    ptr    = mxGetPr(plhs[4]);
+    ptr[0] = nSegs;
+    
+    
+  }
+  else /* we have an error */ {
+    print_usage(VERSION);
+    mexErrMsgTxt("### incorrect usage");
+  }
+}
+
+/*
+ *  Output usage to MATLAB terminal
+ *
+ */
+void print_usage(char *version) {
+  mexPrintf("ltpda_dft version %s\n", version);
+  mexPrintf("  usage:    function [P, navs] = ltpda_dft(x, seglen, DFTcoeffs, olap, order); \n");
+}
+
+/*
+ * Short routine to compute the DFT at a single frequency
+ *
+ */
+void dft(double *Pr, double *Vr, long int *Navs,
+        double *xdata, long int nData, long int segLen, double *Cr, double *Ci, double olap, int order) {
+  long int  istart;
+  double    shift, start;
+  double    *px, *cr, *ci;
+  double    rxsum, ixsum;
+  double    Xr, Mr, M2, Qr;
+  double    p, *x, *a;
+  long int  jj, ii;
+    
+  
+  /* Compute the number of averages we want here */
+  double   ovfact = 1. / (1. - olap / 100.);
+  double   davg = ((double) ((nData - segLen)) * ovfact) / segLen + 1;
+  long int navg = myround(davg);
+    
+  /* Compute steps between segments */
+  if (navg == 1)
+    shift = 1;
+  else
+    shift = (double) (nData - segLen) / (double) (navg - 1);
+  
+  if (shift < 1)
+    shift = 1;
+  
+  /*   mexPrintf("Seglen: %d\t | Shift: %f\t | navs: %d\n", segLen, shift, navg);*/
+  
+  /* allocate vectors */
+  x = (double*)mxCalloc(segLen, sizeof(double));  /* detrending output */
+  a = (double*)mxCalloc(order+1, sizeof(double)); /* detrending coefficients */
+  
+  /* Loop over segments */
+  start = 0.0;
+  Xr = 0.0;
+  Qr = 0.0;
+  Mr = 0.0;
+  M2 = 0.0;
+  
+  for (ii = 0; ii < navg; ii++) {
+    /* compute start index */
+    istart = myround(start);
+    start += shift;
+    
+    /* pointer to start of this segment */
+    px = &(xdata[istart]);
+    
+    /* pointer to DFT coeffs */
+    cr = &(Cr[0]);
+    ci = &(Ci[0]);
+    
+    /* Detrend segment */
+    switch (order) {
+      case -1:
+        /* no detrending */
+        memcpy(x, px, segLen*sizeof(double));
+        break;
+      case 0:
+        /* mean removal */
+        polyreg0(px, segLen, x, a);
+        break;
+      case 1:
+        /* linear detrending */
+        polyreg1(px, segLen, x, a);
+        break;
+      case 2:
+        /* 2nd order detrending */
+        polyreg2(px, segLen, x, a);
+        break;
+      case 3:
+        /* 3rd order detrending */
+        polyreg3(px, segLen, x, a);
+        break;
+      case 4:
+        /* 4th order detrending */
+        polyreg4(px, segLen, x, a);
+        break;
+      case 5:
+        /* 5th order detrending */
+        polyreg5(px, segLen, x, a);
+        break;
+      case 6:
+        /* 6th order detrending */
+        polyreg6(px, segLen, x, a);
+        break;
+      case 7:
+        /* 7th order detrending */
+        polyreg7(px, segLen, x, a);
+        break;
+      case 8:
+        /* 8th order detrending */
+        polyreg8(px, segLen, x, a);
+        break;
+      case 9:
+        /* 9th order detrending */
+        polyreg9(px, segLen, x, a);
+        break;
+      case 10:
+        /* 10th order detrending */
+        polyreg10(px, segLen, x, a);
+        break;
+    }
+    
+    /* Go over all samples in this segment */
+    rxsum = ixsum = 0.0;
+    for (jj=0; jj<segLen; jj++) {
+      p      = x[jj];
+      rxsum += (*cr) * p; /* cos term */
+      ixsum += (*ci) * p; /* sin term */
+      
+      /* increment pointers */
+      cr++;
+      ci++;
+    }
+    /*mexPrintf("   xsum=(%g +i %g), ysum=(%g + i%g)\n", rxsum, ixsum, rysum, iysum);*/
+    
+    /* Average the cross-power
+     * Rsum += rxsum*rxsum + ixsum*ixsum;
+     */
+    
+    /* Welford's algorithm to update mean and variance */
+    if (ii == 0) {
+      Mr = (rxsum*rxsum + ixsum*ixsum);
+    } else {
+      Xr = (rxsum*rxsum + ixsum*ixsum);
+      Qr = Xr - Mr;
+      Mr += Qr/ii;
+      M2 += Qr * (Xr - Mr);
+    }
+    
+  }
+  
+  /* mexPrintf("     start: %f \t istart: %d | %d \n", start, istart, nData-istart);*/
+  /*mexPrintf(" Rsum=%g, MR=%g \n", Rsum,MR); */
+  
+  /* clean up */
+  mxFree(x);
+  mxFree(a);
+  
+  /* Outputs */
+  *Pr = Mr;
+  if(navg == 1){
+    *Vr = Mr*Mr;
+  } else {
+    *Vr = M2/(navg-1);
+  }
+  *Navs = navg;
+}
+
+/*
+ * Short routine to compute the cross-DFT at a single frequency
+ *
+ */
+void xdft(double *Pxyr, double *Pxyi, double *Pxx, double *Pyy, double *Vr, long int *Navs,
+        double *xdata, double *ydata, long int nData, long int segLen, double *Cr, double *Ci, double olap, int order) {
+  long int  istart;
+  double    shift, start;
+  double    *px, *py, *cr, *ci;
+  double    rxsum, ixsum, rysum, iysum;
+  double    XYr, XYi, QXYr, QXYi, QXYrn, QXYin;
+  double    MXYr, MXYi, MXY2;
+  double    XX, YY, QXX, QYY;
+  double    MXX, MYY, MXX2, MYY2;
+  double    p, *x, *y, *a;
+  double    ct, st;
+  long int  jj, ii;
+  
+  /* Compute the number of averages we want here */
+  double   ovfact = 1. / (1. - olap / 100.);
+  double   davg = ((double) ((nData - segLen)) * ovfact) / segLen + 1;
+  long int navg = myround( davg );
+  
+  /* Compute steps between segments */
+  if (navg == 1)
+    shift = 1;
+  else
+    shift = (double) (nData - segLen) / (double) (navg - 1);
+  
+  if (shift < 1)
+    shift = 1;
+  
+  /* mexPrintf("Seglen: %d\t | Shift: %f\t | navs: %d\n", segLen, shift, navg); */
+  
+  /* allocate vectors */
+  y = (double*)mxCalloc(segLen, sizeof(double));  /* detrending output */
+  x = (double*)mxCalloc(segLen, sizeof(double));  /* detrending output */
+  a = (double*)mxCalloc(order+1, sizeof(double)); /* detrending coefficients */
+  
+  /* Loop over segments */
+  start = 0.0;
+  MXYr  = 0.0;
+  MXYi  = 0.0;
+  MXY2   = 0.0;
+  MXX   = 0.0;
+  MYY   = 0.0;
+  MXX2 = 0.0;
+  MYY2 = 0.0;
+  
+  for (ii = 0; ii < navg; ii++) {
+    /* compute start index */
+    istart = myround(start);
+    start += shift;
+    
+    /* pointer to start of this segment */
+    px = &(xdata[istart]);
+    py = &(ydata[istart]);
+    
+    /* pointer to DFT coeffs */
+    cr = &(Cr[0]);
+    ci = &(Ci[0]);
+    
+    /* Detrend segment */
+    switch (order) {
+      case -1:
+        /* no detrending */
+        memcpy(x, px, segLen*sizeof(double));
+        memcpy(y, py, segLen*sizeof(double));
+        break;
+      case 0:
+        /* mean removal */
+        polyreg0(px, segLen, x, a);
+        polyreg0(py, segLen, y, a);
+        break;
+      case 1:
+        /* linear detrending */
+        polyreg1(px, segLen, x, a);
+        polyreg1(py, segLen, y, a);
+        break;
+      case 2:
+        /* 2nd order detrending */
+        polyreg2(px, segLen, x, a);
+        polyreg2(py, segLen, y, a);
+        break;
+      case 3:
+        /* 3rd order detrending */
+        polyreg3(px, segLen, x, a);
+        polyreg3(py, segLen, y, a);
+        break;
+      case 4:
+        /* 4th order detrending */
+        polyreg4(px, segLen, x, a);
+        polyreg4(py, segLen, y, a);
+        break;
+      case 5:
+        /* 5th order detrending */
+        polyreg5(px, segLen, x, a);
+        polyreg5(py, segLen, y, a);
+        break;
+      case 6:
+        /* 6th order detrending */
+        polyreg6(px, segLen, x, a);
+        polyreg6(py, segLen, y, a);
+        break;
+      case 7:
+        /* 7th order detrending */
+        polyreg7(px, segLen, x, a);
+        polyreg7(py, segLen, y, a);
+        break;
+      case 8:
+        /* 8th order detrending */
+        polyreg8(px, segLen, x, a);
+        polyreg8(py, segLen, y, a);
+        break;
+      case 9:
+        /* 9th order detrending */
+        polyreg9(px, segLen, x, a);
+        polyreg9(py, segLen, y, a);
+        break;
+      case 10:
+        /* 10th order detrending */
+        polyreg10(px, segLen, x, a);
+        polyreg10(py, segLen, y, a);
+        break;
+    }
+    
+    /* Go over all samples in this segment */
+    rxsum = ixsum = 0.0;
+    rysum = iysum = 0.0;
+    for (jj=0; jj<segLen; jj++) {
+      ct     = (*cr);
+      st     = (*ci);
+      p      = x[jj];
+      rxsum += ct * p; /* cos term */
+      ixsum += st * p; /* sin term */
+      p      = y[jj];
+      rysum += ct * p; /* cos term */
+      iysum += st * p; /* sin term */
+      
+      /* increment pointers */
+      cr++;
+      ci++;
+    }
+    /*mexPrintf("   xsum=(%g +i %g), ysum=(%g + i%g)\n", rxsum, ixsum, rysum, iysum);*/
+    
+    /* Average XX and YY power
+     * XXsum  += rxsum*rxsum + ixsum*ixsum;
+     * YYsum  += rysum*rysum + iysum*iysum;
+     * XYRsum += rysum*rxsum + iysum*ixsum;
+     * XYIsum += iysum*rxsum - rysum*ixsum; */
+    
+    /* Welford's algorithm to update mean and variance for cross-power */
+    if (ii == 0) {
+      /* for XY  */
+      MXYr = rysum*rxsum + iysum*ixsum;
+      MXYi = iysum*rxsum - rysum*ixsum;
+      /* for XX  */
+      MXX = rxsum*rxsum + ixsum*ixsum;
+      /* for YY  */
+      MYY = rysum*rysum + iysum*iysum;
+    } else {
+      /* for XY cross - power */
+      XYr = rysum*rxsum + iysum*ixsum;
+      XYi = iysum*rxsum - rysum*ixsum;
+      QXYr = XYr - MXYr;
+      QXYi = XYi - MXYi;
+      MXYr += QXYr/ii;
+      MXYi += QXYi/ii;
+      /* new Qs, using new mean */
+      QXYrn = XYr - MXYr;
+      QXYin = XYi - MXYi;
+      /* taking abs to get real variance */
+      MXY2 += sqrt(pow(QXYr * QXYrn - QXYi * QXYin, 2) + pow(QXYr * QXYin + QXYi * QXYrn, 2));
+      /* for XX  */
+      XX = rxsum*rxsum + ixsum*ixsum;
+      QXX = XX - MXX;
+      MXX += QXX/ii;
+      MXX2 += QXX*(XX - MXX);
+      /* for YY  */
+      YY = rysum*rysum + iysum*iysum;
+      QYY = YY - MYY;
+      MYY += QYY/ii;
+      MYY2 += QYY*(YY - MYY);      
+    }
+    
+  }
+  
+  /* mexPrintf("     start: %f \t istart: %d | %d \n", start, istart, nData-istart);*/
+  /*mexPrintf(" Rsum=%g, Isum=%g\n", Rsum, Isum);*/
+  
+  /* clean up */
+  mxFree(y);
+  mxFree(x);
+  mxFree(a);
+  
+  /* Outputs */
+  *Pxyr = MXYr;
+  *Pxyi = MXYi;
+  if(navg == 1){
+    *Vr = MXYr*MXYr; /* set to mean^2 here, but at the end returns Inf in the ao.dy field */
+  } else {
+    *Vr = MXY2/(navg-1);
+  }
+  *Pxx  = MXX;
+  *Pyy  = MYY;      /* we don't return variance for XX and YY  */
+  *Navs = navg;
+}
+
+/*
+ * Fast linear detrending routine
+ *
+ */
+void remove_linear_drift(double *segm, double *data, int nfft) {
+  int i;
+  long double sx, sy, stt, sty, a, b, xm;
+  
+  sx  = (long double) nfft *(long double) (nfft - 1) / 2.0L;
+  xm  = (long double) (nfft - 1) / 2.0L;
+  stt = ((long double) nfft * (long double) nfft * (long double) nfft - (long double)	 nfft) / 12.0L;
+  
+  sy=sty = 0;
+  for (i = 0; i < nfft; i++) {
+    sy += data[i];
+    sty += (i-xm) * data[i];
+  }
+  b = sty / stt;
+  a = (sy - sx * b) / nfft;
+  for (i = 0; i < nfft; i++) {
+    segm[i] = data[i] - (a + b * i);
+  }
+}
+
+