#include "mex.h"
/*#include "matrix.h"*/
#include <math.h>

/*Compute correlation coefficients */
void calcCorrcoeffs(double *corrcoeffs, const double *signal, size_t signal_length, int min_lag, int win)
{
    int i, j;
    double a_mean;
/*     double a_std; */
    double b_mean;
/*     double b_std; */
    double cov;
    double a_sample;
    double b_sample;
    double w;
    int win_len;
    int mid;
    int b_start;
    
    mid = signal_length/2;
    
    /*Iterate through all possible lags */
    for(i = min_lag; i <= signal_length/2; ++i)
    {
        win_len = i < win ? win : i;
        b_start = mid - i;
        
        /*Compute mean for segments a and b */
        /*
        a_mean = 0.0;
        b_mean = 0.0;
        for(j = 0; j < win_len; ++j )
        {
            a_mean += signal[mid+j];
            b_mean += signal[b_start+j];
        }
        a_mean /= (double)win_len;
        b_mean /= (double)win_len;
        */
                 
        /*Calc. cov between a and b as well as */
        /*standard deviation for them */
/*         a_std = 0.0; */
/*         b_std = 0.0; */
        cov = 0.0;
        for(j = 0; j < win_len; ++j)
        {
            w = 1.0;/*(1.0 - cos(6.283185307179586 * (double)j / (double)win_len));*/
            a_sample = w*(signal[mid+j]);/* - a_mean);*/
            b_sample = w*(signal[b_start+j]);/* - b_mean);*/
/*             a_std += a_sample*a_sample; */
/*             b_std += b_sample*b_sample; */
            cov += a_sample*b_sample;
        }
/*         a_std = sqrt(a_std / (double)(win_len - 1)); */
/*         b_std = sqrt(b_std / (double)(win_len - 1)); */
        
        /*Calculate the correlation coefficient for this window position */
        corrcoeffs[i-min_lag] = cov / (double)win_len;/*((double)(win_len - 1) * a_std * b_std);*/
    }
}

/*Compute correlation coefficients */
void calcMaxSpectrum(double *spectrum, const double *signal, size_t signal_length, int min_lag)
{
    int i, j;
    double max;
    double value;
    int mid = signal_length/2;        
    
    /*Iterate through all possible lags */
    for(i = min_lag; i <= signal_length/2; ++i)
    {
        /*mean max row value */
        max = (signal[mid-i] + signal[mid])/2.0;
        for(j = 1; j < i; ++j)
        {
            value = (signal[mid-i+j] + signal[mid+j])/2.0;
            if( value > max )
            {
                max = value;
            }
        }

        /*Calculate the correlation coefficient for this window position */
        spectrum[i-min_lag] = max;
    }
}

/*Compute correlation coefficients */
void calcADMF(double *d, const double *signal, size_t signal_length, int min_lag, int win)
{
    int i, j;
    double a_mean;
    double b_mean;
    double a_sample;
    double b_sample;
    double diff;
    double w;
    int win_len;
    int mid;
    int b_start;
    
    mid = signal_length/2;
    
    /*Iterate through all possible lags */
    for(i = min_lag; i <= signal_length/2; ++i)
    {
        win_len = i < win ? win : i;
        b_start = mid - i;
        
        /*Compute mean for segments a and b */
        /*
        a_mean = 0.0;
        b_mean = 0.0;
        for(j = 0; j < win_len; ++j )
        {
            a_mean += signal[mid+j];
            b_mean += signal[b_start+j];
        }
        a_mean /= (double)win_len;
        b_mean /= (double)win_len;
        */

        /*Calc. cov between a and b as well as */
        /*standard deviation for them */
        diff = 0.0;
        for(j = 0; j < win_len; ++j)
        {
            w = 1.0;/*(1.0 - cos(6.283185307179586 * (double)j / (double)win_len));*/
            a_sample = w*(signal[mid+j]);/* - a_mean);*/
            b_sample = w*(signal[b_start+j]);/* - b_mean);*/
            diff += fabs(a_sample-b_sample);/*(a_sample-b_sample);*/
        }
        
        /*Calculate the correlation coefficient for this window position */
        d[i-min_lag] = (diff / ((double)win_len));
    }
}


void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
/*Matlab call: corrcoeffs = template_corrcoeff(template, signal, detrending_order) */
{
    /*Signal variables */
    double *signal;
    double *corrcoeffs;
    double *spectrum;
    double *d;
    size_t signal_length;
    size_t signal_m;
    size_t signal_n;
    int min_lag;
    int win;
    
    /*Check if we got the correct # of input arguments */
    if(nrhs < 2)
    {
        mexErrMsgTxt("2 input arguments required: x_spectrum(signal, min_lag)"); /*TODO: Change Text*/
    }
    
    /*Check if we got the correct # of input arguments */
    if(nlhs < 3)
    {
        mexErrMsgTxt("3 output arguments required: x_spectrum(signal, min_lag)"); /*TODO: Change Text*/
    }
    
    /*Signal dimensions */
    signal_m = mxGetM(prhs[0]);
    signal_n = mxGetN(prhs[0]);
    
    /*Make sure the signal is a 1xN or Nx1 Matrix and determine its length */
    if( signal_n == 1 && signal_m > 1 )
    {
        signal_length = signal_m;
    }
    else if(signal_m == 1)
    {
        signal_length = signal_n;
    }
    else
    {
        mexErrMsgTxt("Signal argument must be a 1xN or Nx1 matrix");
    }
    
    /*Make sure that the signal length is even */
    if( signal_length % 2 )
    {
        signal_length--;
    }
    
    /*Minimal lag */
    min_lag = (int)mxGetScalar(prhs[1]);
    
    /*Make sure minlag is valid */
    if( min_lag > signal_length/2 )
    {
        mexErrMsgTxt("min_lag argument must be smaller than length(signal)/2");
    }
    
    /*Adapt window size parameter */
    if( nrhs > 2 )
    {
        if( mxIsInf(mxGetScalar(prhs[2])) ) 
        {
            win = signal_length/2;
        }
        else
        {
            win = mxGetScalar(prhs[2]);
            win = win > signal_length/2 ? signal_length/2 : win;
        }
    }
    else
    {
        win = 0;
    }
    
    /*Get vectors */
    signal = mxGetPr(prhs[0]);

    /*Setup output matrix */
    if( signal_m > 1 )
    {
        plhs[0] = mxCreateDoubleMatrix(signal_length/2-min_lag+1, 1, mxREAL);
        plhs[1] = mxCreateDoubleMatrix(signal_length/2-min_lag+1, 1, mxREAL);
        plhs[2] = mxCreateDoubleMatrix(signal_length/2-min_lag+1, 1, mxREAL);
    }
    else
    {
    	plhs[0] = mxCreateDoubleMatrix(1, signal_length/2-min_lag+1, mxREAL);
        plhs[1] = mxCreateDoubleMatrix(1, signal_length/2-min_lag+1, mxREAL);
        plhs[2] = mxCreateDoubleMatrix(1, signal_length/2-min_lag+1, mxREAL);
    }
    corrcoeffs = mxGetPr(plhs[0]);
    spectrum   = mxGetPr(plhs[1]);
    d          = mxGetPr(plhs[2]);
    
    /*Calculate the correlation coefficients TODO: change text*/
    calcCorrcoeffs(corrcoeffs, signal, signal_length, min_lag, win);
    calcMaxSpectrum(spectrum, signal, signal_length, min_lag);
    calcADMF(d, signal, signal_length, min_lag, win);
}

