//This takes univariate X, windows X with W, applies the 1D FFT to each frame,
//takes power, transforms with T mat to B cfs, and then compresses power with log.
//Then the cepstral coeffs (CCs) are obtained by the 1D DCT-II of each frame,
//followed by a lifter (weighting in the cepstral domain).

//This is equivalent to :                                                                                spectrogram -> get_ccs.
//which is equivalent to:                                                                                spectrogram -> dct -> lifter.
//which is equivalent to:                                             stft -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to:                     window_univar -> fft_squared -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to:             window_univar -> fft_hc -> hc_square -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to: frame_univar -> apply_win -> fft_hc -> hc_square -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.

#include <stdio.h>
#include <float.h>
#include <math.h>
#include <cblas.h>
#include <fftw3.h>

#ifndef M_PIf
   #define M_PIf 3.141592653589793238462643383279502884f
#endif

#ifndef M_PI
   #define M_PI 3.141592653589793238462643383279502884
#endif

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int mfccs_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int B, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float preg, const int ndct, const float Q, const int K);
int mfccs_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int B, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double preg, const int ndct, const double Q, const int K);

//int mfccs_s (float *Y, sigi Yi, const float *X, const sigi Xi, const float *W, const sigi Wi, const float *H, const sigi H, const int dim, const int c0, const float stp, const float preg, const int ndct, const float Q)
int mfccs_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int B, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float preg, const int ndct, const float Q, const int K)
{
    const float z = 0.0f, o = 1.0f;
    const float Q2 = 0.5f*Q, PQ = M_PIf/Q;
    //const int normalize_H = 0;
    const int F = nfft/2 + 1;   //num non-negative FFT freqs
    const int F2 = F-1+nfft%2;  //for square loop below
    const int Lpre = L/2;       //nsamps before center samp
    int ss = c0 - Lpre;         //start samp of current frame
    int r, c, f, b, k;
    float *X1, *Y1, *X1d, *Y1d; //, *freqs, *cfs, *H;
    fftwf_plan fft_plan, dct_plan;
    //struct timespec tic, toc;

    //Checks
    if (N<1) { fprintf(stderr,"error in mfccs_s: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in mfccs_s: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in mfccs_s: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in mfccs_s: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in mfccs_s: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in mfccs_s: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in mfccs_s: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0f) { fprintf(stderr,"error in mfccs_s: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in mfccs_s: nfft must be >= L (winlength)\n"); return 1; }
    if (preg!=preg || preg<0.0f) { fprintf(stderr,"error in mfccs_s: preg must be nonnegative\n"); return 1; }
    if (Q<0.0f) { fprintf(stderr,"error in mfccs_s: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in mfccs_s: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in mfccs_s: K must be <= ndct>\n"); return 1; }

    //Initialize fftwf for FFT
    X1 = fftwf_alloc_real((size_t)nfft);
    Y1 = fftwf_alloc_real((size_t)nfft);
    fft_plan = fftwf_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!fft_plan) { fprintf(stderr,"error in mfccs_s: problem creating fftw plan for fft\n"); return 1; }
    cblas_scopy(nfft,&z,0,&X1[0],1); //zero-pad

    //Initialize fftwf for DCT
    X1d = fftwf_alloc_real((size_t)ndct);
    Y1d = fftwf_alloc_real((size_t)ndct);
    dct_plan = fftwf_plan_r2r_1d(ndct,X1d,Y1d,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!dct_plan) { fprintf(stderr,"error in mfccs_s: problem creating fftw plan for dct\n"); return 1; }
    cblas_scopy(ndct,&z,0,&X1d[0],1); //zero-pad

    //Get frequencies and H
    //if (get_stft_freqs_s(&freqs[0],F,fs,nfft)) { fprintf(stderr,"error in mfccs_s: problem getting STFT freqs\n"); return 1; }
    //if (get_cfs_T_s(&cfs[0],B,fs,'mel')) { fprintf(stderr,"error in mfccs_s: problem getting Mel cfs\n"); return 1; }
    //if (get_spectrogram_T_mat_s(&H[0],iscolmajor,&freqs[0],F,&cfs[0],B,normalize_H))
    //{ fprintf(stderr,"error in mfccs_s: problem getting STFT freq -> Mel cfs transform matrix\n"); return 1; }

    //clock_gettime(CLOCK_REALTIME,&tic);

    //Initialize lifter
    //float *lift; int n;
    //if (!(lift=(float *)malloc((size_t)K*sizeof(float)))) { fprintf(stderr,"error in get_ccs_s: problem with malloc for lifter. "); perror("malloc"); return 1; }
    //for (k=0; k<K; k++) { lift[k] = fmaf(Q2,sinf(k*PQ),1.0f); }

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,B,F,1.0f,&H[0],B,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[c*K],1);
                //cblas_ssbmv(CblasColMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1d[0],1,0.0f,&Y[c*K],1);
                //for (k=0; k<K; k++) { Y[k+c*K] = lift[k]*Y1d[k]; }
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); } }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,B,F,1.0f,&H[0],F,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[c],C);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*C],1); } }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,B,F,1.0f,&H[0],B,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[r],R);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*R],1); } }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,B,F,1.0f,&H[0],F,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[r*K],1);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); } }
        }
    }
    else
    {
        fprintf(stderr,"error in mfccs_s: dim must be 0 or 1.\n"); return 1;
    }
    //clock_gettime(CLOCK_REALTIME,&toc); fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);

    //Exit
    fftwf_destroy_plan(fft_plan); fftwf_free(X1); fftwf_free(Y1);
    fftwf_destroy_plan(dct_plan); fftwf_free(X1d); fftwf_free(Y1d);
    return 0;
}

int mfccs_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int B, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double preg, const int ndct, const double Q, const int K)
{
    const double z = 0.0, o = 1.0;
    const double Q2 = 0.5*Q, PQ = M_PI/Q;
    const int F = nfft/2 + 1;   //num non-negative FFT freqs
    const int F2 = F-1+nfft%2;  //for square loop below
    const int Lpre = L/2;       //nsamps before center samp
    int ss = c0 - Lpre;         //start samp of current frame
    int r, c, f, b, k;
    double *X1, *Y1, *X1d, *Y1d;
    fftw_plan fft_plan, dct_plan;

    //Checks
    if (N<1) { fprintf(stderr,"error in mfccs_d: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in mfccs_d: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in mfccs_d: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in mfccs_d: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in mfccs_d: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in mfccs_d: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in mfccs_d: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0) { fprintf(stderr,"error in mfccs_d: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in mfccs_d: nfft must be >= L (winlength)\n"); return 1; }
    if (preg!=preg || preg<0.0) { fprintf(stderr,"error in mfccs_d: preg must be nonnegative\n"); return 1; }
    if (Q<0.0) { fprintf(stderr,"error in mfccs_d: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in mfccs_d: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in mfccs_d: K must be <= ndct>\n"); return 1; }

    //Initialize fftw for FFT
    X1 = fftw_alloc_real((size_t)nfft);
    Y1 = fftw_alloc_real((size_t)nfft);
    fft_plan = fftw_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!fft_plan) { fprintf(stderr,"error in mfccs_d: problem creating fftw plan for fft\n"); return 1; }
    cblas_dcopy(nfft,&z,0,&X1[0],1); //zero-pad

    //Initialize fftw for DCT
    X1d = fftw_alloc_real((size_t)ndct);
    Y1d = fftw_alloc_real((size_t)ndct);
    dct_plan = fftw_plan_r2r_1d(ndct,X1d,Y1d,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!dct_plan) { fprintf(stderr,"error in mfccs_d: problem creating fftw plan for dct\n"); return 1; }
    cblas_dcopy(ndct,&z,0,&X1d[0],1); //zero-pad

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,B,F,1.0,&H[0],B,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[c*K],1);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k],K); } }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,B,F,1.0,&H[0],F,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[c],C);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k*C],1); } }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,B,F,1.0,&H[0],B,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[r],R);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k*R],1); } }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,B,F,1.0,&H[0],F,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[r*K],1);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k],K); } }
        }
    }
    else
    {
        fprintf(stderr,"error in mfccs_d: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    fftw_destroy_plan(fft_plan); fftw_free(X1); fftw_free(Y1);
    fftw_destroy_plan(dct_plan); fftw_free(X1d); fftw_free(Y1d);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/bessel_Inu.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "bessel.h"
#include "bessel_temme.h"

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_bessel_Inu_scaled_e(double nu, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < 0.0 || nu < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < 10.0*(nu+1.0)) {
    gsl_sf_result b;
    double ex = exp(-x);
    int stat = gsl_sf_bessel_IJ_taylor_e(nu, x, 1, 100, GSL_DBL_EPSILON, &b);
    result->val  = b.val * ex;
    result->err  = b.err * ex;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat;
  }
  else if(0.5/(nu*nu + x*x) < GSL_ROOT3_DBL_EPSILON) {
    return gsl_sf_bessel_Inu_scaled_asymp_unif_e(nu, x, result);
  }
  else {
    int N = (int)(nu + 0.5);
    double mu = nu - N;      /* -1/2 <= mu <= 1/2 */
    double K_mu, K_mup1, Kp_mu;
    double K_nu, K_nup1, K_num1;
    double I_nu_ratio;
    int stat_Irat;
    int stat_Kmu;
    int n;

    /* obtain K_mu, K_mup1 */
    if(x < 2.0) {
      stat_Kmu = gsl_sf_bessel_K_scaled_temme(mu, x, &K_mu, &K_mup1, &Kp_mu);
    }
    else {
      stat_Kmu = gsl_sf_bessel_K_scaled_steed_temme_CF2(mu, x, &K_mu, &K_mup1, &Kp_mu);
    }

    /* recurse forward to obtain K_num1, K_nu */
    K_nu   = K_mu;
    K_nup1 = K_mup1;

    for(n=0; n<N; n++) {
      K_num1 = K_nu;
      K_nu   = K_nup1;
      K_nup1 = 2.0*(mu+n+1)/x * K_nu + K_num1;
    }

    /* calculate I_{nu+1}/I_nu */
    stat_Irat = gsl_sf_bessel_I_CF1_ser(nu, x, &I_nu_ratio);

    /* solve for I_nu */
    result->val = 1.0/(x * (K_nup1 + I_nu_ratio * K_nu));
    result->err = GSL_DBL_EPSILON * (0.5*N + 2.0) * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_Kmu, stat_Irat);
  }
}

int
gsl_sf_bessel_Inu_e(double nu, double x, gsl_sf_result * result)
{
  gsl_sf_result b;
  int stat_I = gsl_sf_bessel_Inu_scaled_e(nu, x, &b);
  int stat_e = gsl_sf_exp_mult_err_e(x, fabs(x*GSL_DBL_EPSILON),
                                        b.val, b.err,
                                        result);
  return GSL_ERROR_SELECT_2(stat_e, stat_I);
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Inu_scaled(double nu, double x)
{
  EVAL_RESULT(gsl_sf_bessel_Inu_scaled_e(nu, x, &result));
}

double gsl_sf_bessel_Inu(double nu, double x)
{
  EVAL_RESULT(gsl_sf_bessel_Inu_e(nu, x, &result));
}


/*
 * Copyright 2014 Google Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * ---------------------------------------------------------------------------
 * Copyright 2014 Nervana Systems Inc.  All rights reserved.
 *
 * * Added sumabs, maxabs, save function headers
 * * Fixed numpy deprecation warnings
 * ---------------------------------------------------------------------------
 */

#ifndef MATRIX_H_
#define MATRIX_H_

#include "matrix_funcs.h"
#include <sstream>
#ifdef NUMPY_INTERFACE
#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <arrayobject.h>
#endif
#include <limits>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <cuda_runtime.h>

extern "C" {
#include <cblas.h>
}

#ifdef DOUBLE_PRECISION
#define CBLAS_GEMM cblas_dgemm
#define CBLAS_SCAL cblas_dscal
#define CBLAS_AXPY cblas_daxpy
#else
#define CBLAS_GEMM cblas_sgemm
#define CBLAS_SCAL cblas_sscal
#define CBLAS_AXPY cblas_saxpy
#endif /* DOUBLE_PRECISION */

#define MTYPE_MAX numeric_limits<MTYPE>::max()

typedef long long int int64;

class Matrix {
private:
    MTYPE* _data;
    bool _ownsData;
    int64 _numRows, _numCols;
    int64 _numElements;
    CBLAS_TRANSPOSE _trans;

    void _init(MTYPE* data, int64 numRows, int64 numCols, bool transpose, bool ownsData);
    void _tileTo2(Matrix& target) const;
    void _copyAllTo(Matrix& target) const;
    MTYPE _sum_column(int64 col) const;
    MTYPE _sum_row(int64 row) const;
    MTYPE _aggregate(MTYPE(*agg_func)(MTYPE, MTYPE), MTYPE initialValue) const;
    void _aggregate(int64 axis, Matrix& target, MTYPE(*agg_func)(MTYPE, MTYPE), MTYPE initialValue) const;
    MTYPE _aggregateRow(int64 row, MTYPE(*agg_func)(MTYPE, MTYPE), MTYPE initialValue) const;
    MTYPE _aggregateCol(int64 row, MTYPE(*agg_func)(MTYPE, MTYPE), MTYPE initialValue) const;
    void _updateDims(int64 numRows, int64 numCols);
    void _applyLoop(MTYPE(*func)(MTYPE));
    void _applyLoop(MTYPE (*func)(MTYPE), Matrix& target);
    void _applyLoop2(const Matrix& a, MTYPE(*func)(MTYPE, MTYPE), Matrix& target) const;
    void _applyLoop2(const Matrix& a, MTYPE (*func)(MTYPE,MTYPE, MTYPE), MTYPE scalar, Matrix& target) const;
    void _applyLoop2(const Matrix& a, MTYPE (*func)(MTYPE,MTYPE, MTYPE, MTYPE), MTYPE scalar1, MTYPE scalar2, Matrix& target) const;
    void _applyLoopScalar(const MTYPE scalar, MTYPE(*func)(MTYPE, MTYPE), Matrix& target) const;
    void _checkBounds(int64 startRow, int64 endRow, int64 startCol, int64 endCol) const;
    void _divideByVector(const Matrix& vec, Matrix& target);
    inline int64 _getNumColsBackEnd() const {
        return _trans == CblasNoTrans ? _numCols : _numRows;
    }
public:
    enum FUNCTION {
        TANH, RECIPROCAL, SQUARE, ABS, EXP, LOG, ZERO, ONE, LOGISTIC1, LOGISTIC2, SIGN
    };
    Matrix();
    Matrix(int64 numRows, int64 numCols);
    Matrix(int64 numRows, int64 numCols, bool transpose);
#ifdef NUMPY_INTERFACE
    Matrix(PyArrayObject *src);
    void _initpy(PyArrayObject *src);
#endif
    Matrix(const Matrix &like);
    Matrix(MTYPE* data, int64 numRows, int64 numCols);
    Matrix(float *buffer, int64 numRows, int64 numCols, int nil);
    Matrix(MTYPE* data, int64 numRows, int64 numCols, bool transpose);
    ~Matrix();

    inline MTYPE& getCell(int64 i, int64 j) const {
        assert(i >= 0 && i < _numRows);
        assert(j >= 0 && j < _numCols);
        if (_trans == CblasTrans) {
            return _data[j * _numRows + i];
        }
        return _data[i * _numCols + j];
    }

    MTYPE& operator()(int64 i, int64 j) const {
        return getCell(i, j);
    }

    inline MTYPE* getData() const {
        return _data;
    }

    inline bool isView() const {
        return !_ownsData;
    }

    inline int64 getNumRows() const {
        return _numRows;
    }

    inline int64 getNumCols() const {
        return _numCols;
    }

    inline int64 getNumDataBytes() const {
        return _numElements * sizeof(MTYPE);
    }

    inline int64 getNumElements() const {
        return _numElements;
    }

    inline int64 getLeadingDim() const {
        return _trans == CblasTrans ? _numRows : _numCols;
    }

    inline int64 getFollowingDim() const {
        return _trans == CblasTrans ? _numCols : _numRows;
    }

    inline CBLAS_TRANSPOSE getBLASTrans() const {
        return _trans;
    }

    inline bool isSameDims(const Matrix& a) const {
        return a.getNumRows() == getNumRows() && a.getNumCols() == getNumCols();
    }

    inline bool isTrans() const {
        return _trans == CblasTrans;
    }

    /*
     * Only use if you know what you're doing!
     * Does not update any dimensions. Just flips the _trans flag.
     *
     * Use transpose() if you want to get the transpose of this matrix.
     */
    inline void setTrans(bool trans) {
        assert(isTrans() == trans || !isView());
        _trans = trans ? CblasTrans : CblasNoTrans;
    }

    void apply(FUNCTION f);
    void apply(Matrix::FUNCTION f, Matrix& target);
    void subtractFromScalar(MTYPE scalar);
    void subtractFromScalar(MTYPE scalar, Matrix &target) const;
    void biggerThanScalar(MTYPE scalar);
    void smallerThanScalar(MTYPE scalar);
    void equalsScalar(MTYPE scalar);
    void biggerThanScalar(MTYPE scalar, Matrix& target) const;
    void smallerThanScalar(MTYPE scalar, Matrix& target) const;
    void equalsScalar(MTYPE scalar, Matrix& target) const;
    void biggerThan(Matrix& a);
    void biggerThan(Matrix& a, Matrix& target) const;
    void smallerThan(Matrix& a);
    void smallerThan(Matrix& a, Matrix& target) const;
    void minWith(Matrix &a);
    void minWith(Matrix &a, Matrix &target) const;
    void maxWith(Matrix &a);
    void maxWith(Matrix &a, Matrix &target) const;
    void equals(Matrix& a);
    void equals(Matrix& a, Matrix& target) const;
    void notEquals(Matrix& a) ;
    void notEquals(Matrix& a, Matrix& target) const;
    void add(const Matrix &m);
    void add(const Matrix &m, MTYPE scale);
    void add(const Matrix &m, MTYPE scaleThis, MTYPE scaleM);
    void add(const Matrix &m, Matrix& target);
    void add(const Matrix &m, MTYPE scaleM, Matrix &target);
    void add(const Matrix &m, MTYPE scaleThis, MTYPE scaleM, Matrix &target);
    void subtract(const Matrix &m);
    void subtract(const Matrix &m, Matrix& target);
    void subtract(const Matrix &m, MTYPE scale);
    void subtract(const Matrix &m, MTYPE scale, Matrix& target);
    void addVector(const Matrix& vec, MTYPE scale);
    void addVector(const Matrix& vec, MTYPE scale, Matrix& target);
    void addVector(const Matrix& vec);
    void addVector(const Matrix& vec, Matrix& target);
    void addScalar(MTYPE scalar);
    void addScalar(MTYPE scalar, Matrix& target) const;
    void maxWithScalar(MTYPE scalar);
    void maxWithScalar(MTYPE scalar, Matrix &target) const;
    void minWithScalar(MTYPE scalar);
    void minWithScalar(MTYPE scalar, Matrix &target) const;
    void eltWiseMultByVector(const Matrix& vec);
    void eltWiseMultByVector(const Matrix& vec, Matrix& target);
    void eltWiseDivideByVector(const Matrix& vec);
    void eltWiseDivideByVector(const Matrix& vec, Matrix& target);
    void resize(int64 newNumRows, int64 newNumCols);
    void resize(const Matrix& like);
    Matrix& slice(int64 startRow, int64 endRow, int64 startCol, int64 endCol) const;
    void slice(int64 startRow, int64 endRow, int64 startCol, int64 endCol, Matrix &target) const;
    Matrix& sliceRows(int64 startRow, int64 endRow) const;
    void sliceRows(int64 startRow, int64 endRow, Matrix& target) const;
    Matrix& sliceCols(int64 startCol, int64 endCol) const;
    void sliceCols(int64 startCol, int64 endCol, Matrix& target) const;
    void rightMult(const Matrix &b, MTYPE scale);
    void rightMult(const Matrix &b, Matrix &target) const;
    void rightMult(const Matrix &b);
    void rightMult(const Matrix &b, MTYPE scaleAB, Matrix &target) const;
    void addProduct(const Matrix &a, const Matrix &b, MTYPE scaleAB, MTYPE scaleThis);
    void addProduct(const Matrix& a, const Matrix& b);
    void eltWiseMult(const Matrix& a);
    void eltWiseMult(const Matrix& a, Matrix& target) const;
    void eltWiseDivide(const Matrix& a);
    void eltWiseDivide(const Matrix& a, Matrix &target) const;
    Matrix& transpose() const;
    Matrix& transpose(bool hard) const;
    Matrix& tile(int64 timesY, int64 timesX) const;
    void tile(int64 timesY, int64 timesX, Matrix& target) const;
    void copy(Matrix &dest, int64 srcStartRow, int64 srcEndRow, int64 srcStartCol, int64 srcEndCol, int64 destStartRow, int64 destStartCol) const;
    Matrix& copy() const;
    void copy(Matrix& target) const;
    Matrix& sum(int64 axis) const;
    void sum(int64 axis, Matrix &target) const;
    MTYPE sum() const;
    MTYPE sumabs() const;
    MTYPE max() const;
    MTYPE maxabs() const;
    Matrix& max(int64 axis) const;
    void max(int64 axis, Matrix& target) const;
    MTYPE min() const;
    Matrix& min(int64 axis) const;
    void min(int64 axis, Matrix& target) const;
    MTYPE norm() const;
    MTYPE norm2() const;
    void scale(MTYPE scale);
    void scale(MTYPE alpha, Matrix& target);
    void reshape(int64 numRows, int64 numCols);
    Matrix& reshaped(int64 numRows, int64 numCols);
    void printShape(const char* name) const;
    bool hasNan() const;
    bool hasInf() const;

    void randomizeNormal(MTYPE mean, MTYPE stdev);
    void randomizeUniform();
    void randomizeNormal();
    void print() const;
    void print(int64 startRow,int64 rows, int64 startCol,int64 cols) const;
    void print(int64 rows, int64 cols) const;
};

typedef std::vector<Matrix*> MatrixV;

#endif /* MATRIX_H_ */


#ifndef EX_UTIL
#define EX_UTIL

#include <gsl/gsl_vector.h>

typedef double (*db_fxy)(double x, double y);
typedef double (*db_fx) (double);

/* print_table_fxy
 * prints f(x,y) tables given range for x and y
 */
void print_table_fxy(db_fxy fun,
        double x_start, double x_end, double x_step, char xch,
        double y_start, double y_end, double y_step, char ych);

/*
 * vec_linspace
 * returns a pointer to a gsl_vector from xlow (inclusive) to xhigh (exclusive)
 * with steps xstep
 */
gsl_vector *vec_linspace(double xlow, double xhigh, double xstep);

/*
 * vec_fx
 * given a function db_fx and vector of xpoints xdata, returns a vector f(x)
 */
gsl_vector *vec_fx(db_fx f, gsl_vector *xdata);

/*
 * vec_from_arr
 * generates a vector from double array
 */
gsl_vector *vec_from_arr(double *arr, int arr_len);

/*
 * sign
 */
int sign(double x);
#endif


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_lapacke.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <lapacke.h>
#endif

/*!
 * @brief Convert from moment tnesor to scalar seismic moment.
 *
 * @param[in] nm     Number of moment tensors.
 * @param[in] im0    =1 for Silver and Jordan (1982) formula. \n
 *                   =2 for GCMT formula. \n
 *                   =3 for old `Caltech Way.'
 * @param[in] M      [nm x 6] array of moment tensors packed
 *                   {M11, M22, M33, M12, M13, M23} with leading
 *                   dimension 6.  The Mij element should be of units
 *                   of Newton-meters (although it should not affect
 *                   the function here).
 *
 * @param[out] M0    Scalar moments with units of Newton-meters.  This
 *                   is an array of dimension [nm].
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @copyright MIT
 *
 */
int compearth_CMT2m0(const int nm, const int im0,
                     const double *__restrict__ M,
                     double *__restrict__ M0)
{
    double Mt9[9], lams[3], work[100], M11, M22, M33, M12, M13, M23;
    const double sqrt2i = 1.0/sqrt(2.0);
    const int lwork = 100;
    int i, ierr, info;
    ierr = 0;
    if (im0 == 2 || im0 == 3)
    {
        for (i=0; i<nm; i++)
        {
            // Fill upper-triangle; column 1
            Mt9[0] = M[6*i];
            Mt9[1] = 0.0;
            Mt9[2] = 0.0;
            // column 2
            Mt9[3] = M[6*i+3];
            Mt9[4] = M[6*i+1];
            Mt9[5] = 0.0;
            // column 3
            Mt9[6] = M[6*i+4];
            Mt9[7] = M[6*i+5];
            Mt9[8] = M[6*i+2];
            // get eigenvalues in ascending order
            info = LAPACKE_dsyev_work(LAPACK_COL_MAJOR, 'N', 'U', 3,
                                      Mt9, 3, lams, work, lwork);
            if (info != 0)
            {
                fprintf(stderr, "%s: Error computing eigenvalues\n", __func__);
                return -1;
            }
            // GCMT double couple formulation
            if (im0 == 2)
            {
                M0[i] = 0.5*(fabs(lams[0]) + fabs(lams[2]));
            }
            else
            {
                M0[i] = fabs(lams[2]);
                if (fabs(lams[0]) > fabs(lams[2])){M0[i] = fabs(lams[0]);}
            }
        }
    }
    // Silver and Jordan
    else if (im0 == 1)
    {
        for (i=0; i<nm; i++)
        {
            M11 = M[6*i];
            M22 = M[6*i+1];
            M33 = M[6*i+2];
            M12 = M[6*i+3];
            M13 = M[6*i+4];
            M23 = M[6*i+5];
            M0[i] = sqrt2i*sqrt((      M11*M11 + M22*M22 + M33*M33
                                + 2.0*(M12*M12 + M13*M13 + M23*M23)));
        }
    }
    else
    {
        fprintf(stderr, "%s: Invalid magnitude type %d\n", __func__, im0);
        for (i=0; i<nm; i++){M0[i] = 0.0;}
        return -1;
    }
    return ierr;
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  entropy.c
 *
 *    Description:  Calculate the entropy production for a given state, generator
 *    			and equilibrium state
 *
 *        Version:  1.0
 *        Created:  05/05/2014 20:53:01
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  entropy_production
 *  Description:
 *
 */
double entropy_production ( const gsl_vector* rho, const gsl_vector* rhoeq,
		const gsl_matrix* L  )
{
	/* l1, l2, l3 */
	double l[3] ; unsigned int i, j ;
	for ( i = 1 ; i < 3 ; i++ )
	{
		l[i] = 0 ;
		for ( j = 0 ; j < 3 ; j++ )
			l[i] += gsl_matrix_get(L,i,j)*VECTOR(rho,j) ;
	}

	/* L[rho] */
	double Lr = 0 ;
	for ( i = 1 ; i < 3 ; i++ )
		Lr += l[i]*VECTOR(rho,i) ;

	/* L[rhoeq] */
	double Leq = 0 ;
	for ( i = 1 ; i < 3 ; i++ )
		Leq += l[i]*VECTOR(rhoeq,i) ;

	/* r , req */
	double r, req ;
	r = req = 0 ;

	r = gsl_hypot3(VECTOR(rho,1),VECTOR(rho,2),VECTOR(rho,3)) ;
	req = gsl_hypot3(VECTOR(rhoeq,1),VECTOR(rhoeq,2),
		VECTOR(rhoeq,3)) ;

	/* internal entropy s */
	double s ;
	if ( r < 1 && req < 1 )
		s = -(gsl_sf_log((1+r)/(1-r))*Lr/r -gsl_sf_log((1+req)/(1-req))*Leq/req);
	else if ( r < 1 && req >= 0 )
		s = -gsl_sf_log((1+r)/(1-r))*Lr/r ;
	else
		s = 0 ;

	return s;
}		/* -----  end of function entropy_production  ----- */

/*
 *      FUNCTION
 *         Name:  entropy_of_state
 *  Description:  Calculate the Von Neumann entropy of state 'rho'
 *
 */
double entropy_of_state ( const gsl_vector* rho )
{
	double entr = 0 ;

	/* Finding the eigenvalues */
	gsl_eigen_herm_workspace* rho_ei = gsl_eigen_herm_alloc(2);
	gsl_matrix_complex* dens = gsl_matrix_complex_calloc (2,2);
	gsl_matrix_complex_set (dens, 0, 0, gsl_complex_rect(1+VECTOR(rho, 3),0));
	gsl_matrix_complex_set (dens,0,1,gsl_complex_rect(VECTOR(rho,1),-VECTOR(rho,2)));
	gsl_matrix_complex_set (dens,1,0,gsl_complex_rect(VECTOR(rho,1),VECTOR(rho,2)));
	gsl_matrix_complex_set (dens,1,1,gsl_complex_rect(1-VECTOR(rho,3),0));
	gsl_matrix_complex_scale (dens, gsl_complex_rect(0.5,0));
	gsl_vector* eigenvalues = gsl_vector_calloc(2) ;
	gsl_eigen_herm (dens, eigenvalues, rho_ei) ;

	/* Calculating entropy */
	double norm = gsl_hypot3( VECTOR(rho,1), VECTOR(rho,2),	VECTOR(rho,3) ) ;
	if ( gsl_fcmp(norm, 1, 1e-9) > 0 )
		entr = 0 ;
	else
		entr = - (VECTOR(eigenvalues,0)*gsl_sf_log(VECTOR(eigenvalues,0)) +
			VECTOR(eigenvalues,1)*gsl_sf_log(VECTOR(eigenvalues,1))) ;

	return (entr);
}		/* -----  end of function entropy_of_state  ----- */


/* movstat/test_minmax.c
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_movstat.h>

/* compute filtered data by explicitely constructing window and finding min/max */
int
slow_minmax(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y_min, gsl_vector * y_max,
            const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      gsl_vector_view v = gsl_vector_view_array(window, wsize);
      double min, max;

      gsl_vector_minmax(&v.vector, &min, &max);
      gsl_vector_set(y_min, i, min);
      gsl_vector_set(y_max, i, max);
    }

  free(window);

  return GSL_SUCCESS;
}

static double
func_min(const size_t n, double x[], void * params)
{
  gsl_vector_view v = gsl_vector_view_array(x, n);
  (void) params;
  return gsl_vector_min(&v.vector);
}

static double
func_max(const size_t n, double x[], void * params)
{
  gsl_vector_view v = gsl_vector_view_array(x, n);
  (void) params;
  return gsl_vector_max(&v.vector);
}

static void
test_minmax_x(const double tol, const gsl_vector * x, const int H, const int J,
              const gsl_movstat_end_t endtype, const char * desc)
{
  const size_t n = x->size;
  gsl_vector * u_min = gsl_vector_alloc(n);
  gsl_vector * y_min = gsl_vector_alloc(n);
  gsl_vector * z_min = gsl_vector_alloc(n);
  gsl_vector * u_max = gsl_vector_alloc(n);
  gsl_vector * y_max = gsl_vector_alloc(n);
  gsl_vector * z_max = gsl_vector_alloc(n);
  gsl_movstat_workspace * w = gsl_movstat_alloc2(H, J);
  gsl_movstat_function F1, F2;
  char buf[2048];

  F1.function = func_min;
  F1.params = NULL;

  F2.function = func_max;
  F2.params = NULL;

  /* compute moving min/max */
  gsl_movstat_min(endtype, x, u_min, w);
  gsl_movstat_max(endtype, x, u_max, w);
  gsl_movstat_minmax(endtype, x, y_min, y_max, w);

  /* compute moving min/max with slow brute force method */
  slow_minmax(endtype, x, z_min, z_max, H, J);

  sprintf(buf, "test_minmax: %s min endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, u_min, z_min, buf);

  sprintf(buf, "test_minmax: %s max endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, u_max, z_max, buf);

  sprintf(buf, "test_minmax: %s minmax(minimum) endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, y_min, z_min, buf);

  sprintf(buf, "test_minmax: %s minmax(maximum) endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, y_max, z_max, buf);

  /* in-place tests */

  gsl_vector_memcpy(u_min, x);
  gsl_vector_memcpy(u_max, x);

  gsl_movstat_min(endtype, u_min, u_min, w);
  gsl_movstat_max(endtype, u_max, u_max, w);

  sprintf(buf, "test_minmax: %s in-place min endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, u_min, z_min, buf);

  sprintf(buf, "test_minmax: %s in-place max endtype=%d n=%zu H=%d J=%d", desc, endtype, n, H, J);
  compare_vectors(tol, u_max, z_max, buf);

  /* user-defined function tests */

  gsl_movstat_apply(endtype, &F1, x, z_min, w);
  sprintf(buf, "n=%zu H=%d J=%d endtype=%u min user", n, H, J, endtype);
  compare_vectors(tol, z_min, y_min, buf);

  gsl_movstat_apply(endtype, &F2, x, z_max, w);
  sprintf(buf, "n=%zu H=%d J=%d endtype=%u max user", n, H, J, endtype);
  compare_vectors(tol, z_max, y_max, buf);

  gsl_vector_free(u_min);
  gsl_vector_free(y_min);
  gsl_vector_free(z_min);
  gsl_vector_free(u_max);
  gsl_vector_free(y_max);
  gsl_vector_free(z_max);
  gsl_movstat_free(w);
}

/* test alternating sequence [a,b,a,b,...] input */
static void
test_minmax_alt(const double tol, const size_t n, const int H, const int J,
                const gsl_movstat_end_t endtype)
{
  const double a = 5.0;
  const double b = -5.0;
  gsl_vector * x = gsl_vector_alloc(n);
  size_t i;

  for (i = 0; i < n; ++i)
    {
      if (i % 2 == 0)
        gsl_vector_set(x, i, a);
      else
        gsl_vector_set(x, i, b);
    }

  test_minmax_x(tol, x, H, J, endtype, "alternating");

  gsl_vector_free(x);
}

/* test noisy sine wave input */
static void
test_minmax_sine(const double tol, const size_t n, const int H, const int J,
                 const gsl_movstat_end_t endtype, gsl_rng * rng_p)
{
  gsl_vector * x = gsl_vector_alloc(n);

  /* construct noisy sine signal */
  test_noisy_sine(0.5, x, rng_p);

  test_minmax_x(tol, x, H, J, endtype, "noisy_sine");

  gsl_vector_free(x);
}

/* test random input */
static void
test_minmax_random(const double tol, const size_t n, const int H, const int J,
                   const gsl_movstat_end_t endtype, gsl_rng * rng_p)
{
  gsl_vector * x = gsl_vector_alloc(n);

  /* construct random input signal */
  random_vector(x, rng_p);

  test_minmax_x(tol, x, H, J, endtype, "random");

  gsl_vector_free(x);
}

static void
test_minmax(gsl_rng * rng_p)
{
  /* alternating input */

  test_minmax_alt(GSL_DBL_EPSILON, 1000, 7, 7, GSL_MOVSTAT_END_PADZERO);
  test_minmax_alt(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_PADZERO);
  test_minmax_alt(GSL_DBL_EPSILON, 500, 1, 3, GSL_MOVSTAT_END_PADZERO);
  test_minmax_alt(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_PADZERO);
  test_minmax_alt(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_PADZERO);

  /* noisy sine wave input */

  test_minmax_sine(GSL_DBL_EPSILON, 1000, 5, 7, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 2000, 0, 2, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 500, 3, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_minmax_sine(GSL_DBL_EPSILON, 500, 5, 7, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 10, 20, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 3, 3, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_minmax_sine(GSL_DBL_EPSILON, 500, 5, 7, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 10, 20, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 3, 3, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 30, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 1000, 5, 30, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_sine(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);

  /* random input */

  test_minmax_random(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 1000, 5, 7, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 2000, 0, 2, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 3, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 10, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 5, 10, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_minmax_random(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 1000, 5, 7, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 2000, 0, 2, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 3, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 10, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 5, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_minmax_random(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 1000, 5, 7, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 2000, 0, 2, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 3, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 10, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 500, 5, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 10, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_minmax_random(GSL_DBL_EPSILON, 20, 50, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


/* rng/ran1.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* This is an implementation of the algorithm used in Numerical
   Recipe's ran1 generator.  It is MINSTD with a 32-element
   shuffle-box. */

static inline unsigned long int ran1_get (void *vstate);
static double ran1_get_double (void *vstate);
static void ran1_set (void *state, unsigned long int s);

static const long int m = 2147483647, a = 16807, q = 127773, r = 2836;

#define N_SHUFFLE 32
#define N_DIV (1 + 2147483646/N_SHUFFLE)

typedef struct
  {
    unsigned long int x;
    unsigned long int n;
    unsigned long int shuffle[N_SHUFFLE];
  }
ran1_state_t;

static inline unsigned long int
ran1_get (void *vstate)
{
  ran1_state_t *state = (ran1_state_t *) vstate;

  const unsigned long int x = state->x;

  const long int h = x / q;
  const long int t = a * (x - h * q) - h * r;

  if (t < 0)
    {
      state->x = t + m;
    }
  else
    {
      state->x = t;
    }

  {
    unsigned long int j = state->n / N_DIV;
    state->n = state->shuffle[j];
    state->shuffle[j] = state->x;
  }

  return state->n;
}

static double
ran1_get_double (void *vstate)
{
  float x_max = 1 - 1.2e-7f ; /* Numerical Recipes version of 1-FLT_EPS */

  float x = ran1_get (vstate) / 2147483647.0f ;

  if (x > x_max)
    return x_max ;

  return x ;
}

static void
ran1_set (void *vstate, unsigned long int s)
{
  ran1_state_t *state = (ran1_state_t *) vstate;
  int i;

  if (s == 0)
    s = 1;      /* default seed is 1 */

  for (i = 0; i < 8; i++)
    {
      long int h = s / q;
      long int t = a * (s - h * q) - h * r;
      if (t < 0)
        t += m;
      s = t;
    }

  for (i = N_SHUFFLE - 1; i >= 0; i--)
    {
      long int h = s / q;
      long int t = a * (s - h * q) - h * r;
      if (t < 0)
        t += m;
      s = t;
      state->shuffle[i] = s;
    }

  state->x = s;
  state->n = s;

  return;
}

static const gsl_rng_type ran1_type =
{"ran1",                        /* name */
 2147483646,                    /* RAND_MAX */
 1,                             /* RAND_MIN */
 sizeof (ran1_state_t),
 &ran1_set,
 &ran1_get,
 &ran1_get_double};

const gsl_rng_type *gsl_rng_ran1 = &ran1_type;


/**
 *
 * @file core_sgemv.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:49 2014
 *
 **/
#include <cblas.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_float
 *
 * CORE_sgemv performs one of the matrix-vector operations
 *
 *    y := alpha*A*x    + beta*y,   or
 *    y := alpha*A**T*x + beta*y,   or
 *    y := alpha*A**T*x + beta*y,
 *
 * where alpha and beta are scalars, x and y are vectors,
 * and A is an m by n matrix.
 *
 *******************************************************************************
 *
 * @param[in] trans
 *         @arg PlasmaNoTrans:   y := alpha*A*x    + beta*y
 *         @arg PlasmaTrans:     y := alpha*A**T*x + beta*y
 *         @arg PlasmaTrans: y := alpha*A**T*x + beta*y
 *
 * @param[in] m
 *         Number of rows of matrix A.
 *
 * @param[in] n
 *         Number of columns of matrix A.
 *
 * @param[in] alpha
 *         Scalar alpha.
 *
 * @param[in] A
 *         On entry, m by n matrix A. Dimension (lda,n).
 *
 * @param[in] lda
 *         Leading dimension of array A. lda >= max(1,m).
 *
 * @param[in] x
 *         On entry, vector x.
 *         If trans == PlasmaNoTrans, the n vector x has dimension 1 + (n-1)*abs(incx).
 *         Else, the m vector x has dimension 1 + (m-1)*abs(incx).
 *
 * @param[in] incx
 *         Increment between elements of x. incx must not be zero.
 *
 * @param[in] beta
 *         Scalar beta.
 *
 * @param[in,out] y
 *         On entry, vector y. On exit, y  is overwritten by updated vector y.
 *         If trans == PlasmaNoTrans, the m vector y has dimension 1 + (m-1)*abs(incy).
 *         Else, the n vector y has dimension 1 + (n-1)*abs(incy).
 *
 * @param[in] incy
 *         Increment between elements of y. incy must not be zero.
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemv = PCORE_sgemv
#define CORE_sgemv PCORE_sgemv
#endif
void CORE_sgemv(PLASMA_enum trans,
                int m, int n,
                float alpha, const float *A, int lda,
                                          const float *x, int incx,
                float beta,        float *y, int incy)
{
    cblas_sgemv(
        CblasColMajor,
        (CBLAS_TRANSPOSE)trans,
        m, n,
        (alpha), A, lda,
        x, incx,
        (beta), y, incy );
}


//This does CELL (~soma) stage of minimal GRU (gated recurrent unit) model.
//This requires each neuron to have 2 input time series, X and Xf,
//where X is the usual input and Xf the input for the forget-gate.
//Both X and Xf are the output of a linear IN stage (weights and baises).

//For dim=0: F[:,t] = sig(Xf[:,t] + Uf*Y[:,t-1])
//           H[:,t] = F[:,t].*Y[:,t-1]
//           Y[:,t] = H[:,t] + (1-f[:,t]).*tanh(X[:,t] + U*H[:,t])
//
//For dim=1: F[t,:] = sig(Xf[t,:] + Y[t-1,:]*Uf)
//           H[t,:] = F[t,:].*Y[t-1,:]
//           Y[t,:] = H[t,:] + (1-F[t,:]).*tanh(X[t,:] + H[t,:]*U)
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//F is the forget gate signal, H is an intermediate vector,
//U and Uf are NxN update matrices, and Y is the output.

//Note that, the neurons of a layer are independent only if U and Uf are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

//Unlike v2, this version takes only one X input: X = [X; Xf] for dim=0
//                                                X = [X Xf]  for dim=1
//This is essentially to allow treatment as a layer with Linux piping conventions.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int gru_min_s (float *Y, const float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min_d (double *Y, const double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min_inplace_s (float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min_inplace_d (double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min_s (float *Y, const float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t NT = N*T;
    size_t nT, tN, tN2;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_d (double *Y, const double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t NT = N*T;
    size_t nT, tN, tN2;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_inplace_s (float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t NT = N*T, N2 = 2*N;
    size_t nT, tN, tN2;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_inplace_min2_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_inplace_min2_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            fprintf(stderr,"d0, colmajor \n");
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0f-F[n])*tanhf(X[tN2+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0f-F[n])*tanhf(X[tN2+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_inplace_min2_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_inplace_d (double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t NT = N*T, N2 = 2*N;
    size_t nT, tN, tN2;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_inplace_min2_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_inplace_min2_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0-F[n])*tanh(X[tN2+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0-F[n])*tanh(X[tN2+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_inplace_min2_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <stdio.h>
#include <stdlib.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>

int
main(void)
{
  const size_t N = 1000;                       /* length of time series */
  const size_t K = 61;                         /* window size */
  const double alpha = 3.0;                    /* Gaussian kernel has +/- 3 standard deviations */
  gsl_vector *x = gsl_vector_alloc(N);         /* input vector */
  gsl_vector *y = gsl_vector_alloc(N);         /* filtered output vector */
  gsl_vector *dy = gsl_vector_alloc(N);        /* first derivative filtered vector */
  gsl_vector *d2y = gsl_vector_alloc(N);       /* second derivative filtered vector */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  gsl_filter_gaussian_workspace *gauss_p = gsl_filter_gaussian_alloc(K);
  size_t i;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double xi = (i > N / 2) ? 0.5 : 0.0;
      double ei = gsl_ran_gaussian(r, 0.1);

      gsl_vector_set(x, i, xi + ei);
    }

  /* apply filters */
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 0, x, y, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 1, x, dy, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 2, x, d2y, gauss_p);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);
      double dyi = gsl_vector_get(dy, i);
      double d2yi = gsl_vector_get(d2y, i);
      double dxi;

      /* compute finite difference of x vector */
      if (i == 0)
        dxi = gsl_vector_get(x, i + 1) - xi;
      else if (i == N - 1)
        dxi = gsl_vector_get(x, i) - gsl_vector_get(x, i - 1);
      else
        dxi = 0.5 * (gsl_vector_get(x, i + 1) - gsl_vector_get(x, i - 1));

      printf("%.12e %.12e %.12e %.12e %.12e\n",
             xi,
             yi,
             dxi,
             dyi,
             d2yi);
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(dy);
  gsl_vector_free(d2y);
  gsl_rng_free(r);
  gsl_filter_gaussian_free(gauss_p);

  return 0;
}


#pragma once

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <omp.h>
#include "matrix.h"

double l1_distance(double *vec_1, double *vec_2, int N);
void naive_ranking(double *matrix, double *result, int N);
void pagerank(double *matrix, double *result, int N,
              double tolerance, double damping_factor, int max_iter);


/* multilarge_nlinear/nielsen.c
 *
 * Copyright (C) 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This module contains routines for updating the Levenberg-Marquardt
 * damping parameter on each iteration using Nielsen's method:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * 3 routines are needed to implement the update procedure:
 *
 * 1. init   - initialize parameter prior to iteration
 * 2. accept - update parameter after a step has been accepted
 * 3. reject - update parameter after a step has been rejected
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

#define LM_ONE_THIRD         (0.333333333333333)

static int nielsen_init(const gsl_matrix * JTJ, const gsl_vector * diag,
                        double * mu, long * nu);
static int nielsen_accept(const double rho, double * mu, long * nu);
static int nielsen_reject(double * mu, long * nu);

static int
nielsen_init(const gsl_matrix * JTJ, const gsl_vector * diag,
             double * mu, long * nu)
{
  const double mu0 = 1.0e-3;
  size_t i;
  double max = -1.0;

  *nu = 2;

  if (JTJ == NULL)
    {
      /* not using cholesky solver */
      *mu = mu0;
      return GSL_SUCCESS;
    }

  /* set mu = mu0 * max(diag(J~^T J~)), with J~ = J D^{-1} */

  for (i = 0; i < JTJ->size1; ++i)
    {
      double JTJi = gsl_matrix_get(JTJ, i, i);
      double di = gsl_vector_get(diag, i);
      double val = JTJi / (di * di);
      max = GSL_MAX(max, val);
    }

  *mu = mu0 * max;

  return GSL_SUCCESS;
}

static int
nielsen_accept(const double rho, double * mu, long * nu)
{
  double b;

  /* reset nu */
  *nu = 2;

  b = 2.0 * rho - 1.0;
  b = 1.0 - b*b*b;
  *mu *= GSL_MAX(LM_ONE_THIRD, b);

  return GSL_SUCCESS;
}

static int
nielsen_reject(double * mu, long * nu)
{
  *mu *= (double) *nu;

  /* nu := 2*nu */
  *nu <<= 1;

  return GSL_SUCCESS;
}


/* integration/qawo.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "initialise.c"
#include "set_initial.c"
#include "reset.c"
#include "qpsrt.c"
#include "util.c"
#include "qpsrt2.c"
#include "qelg.c"
#include "positivity.c"

#include "qc25f.c"

int
gsl_integration_qawo (gsl_function * f,
                      const double a,
                      const double epsabs, const double epsrel,
                      const size_t limit,
                      gsl_integration_workspace * workspace,
                      gsl_integration_qawo_table * wf,
                      double *result, double *abserr)
{
  double area, errsum;
  double res_ext, err_ext;
  double result0, abserr0, resabs0, resasc0;
  double tolerance;

  double ertest = 0;
  double error_over_large_intervals = 0;
  double reseps = 0, abseps = 0, correc = 0;
  size_t ktmin = 0;
  int roundoff_type1 = 0, roundoff_type2 = 0, roundoff_type3 = 0;
  int error_type = 0, error_type2 = 0;

  size_t iteration = 0;

  int positive_integrand = 0;
  int extrapolate = 0;
  int extall = 0;
  int disallow_extrapolation = 0;

  struct extrapolation_table table;

  double b = a + wf->L ;
  double abs_omega = fabs (wf->omega) ;

  /* Initialize results */

  initialise (workspace, a, b);

  *result = 0;
  *abserr = 0;

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  /* Test on accuracy */

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      GSL_ERROR ("tolerance cannot be achieved with given epsabs and epsrel",
                 GSL_EBADTOL);
    }

  /* Perform the first integration */

  qc25f (f, a, b, wf, 0, &result0, &abserr0, &resabs0, &resasc0);

  set_initial_result (workspace, result0, abserr0);

  tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (result0));

  if (abserr0 <= 100 * GSL_DBL_EPSILON * resabs0 && abserr0 > tolerance)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("cannot reach tolerance because of roundoff error "
                 "on first attempt", GSL_EROUND);
    }
  else if ((abserr0 <= tolerance && abserr0 != resasc0) || abserr0 == 0.0)
    {
      *result = result0;
      *abserr = abserr0;

      return GSL_SUCCESS;
    }
  else if (limit == 1)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("a maximum of one iteration was insufficient", GSL_EMAXITER);
    }

  /* Initialization */

  initialise_table (&table);

  if (0.5 * abs_omega * fabs(b - a) <= 2)
    {
      append_table (&table, result0);
      extall = 1;
    }

  area = result0;
  errsum = abserr0;

  res_ext = result0;
  err_ext = GSL_DBL_MAX;

  positive_integrand = test_positivity (result0, resabs0);

  iteration = 1;

  do
    {
      size_t current_level;
      double a1, b1, a2, b2;
      double a_i, b_i, r_i, e_i;
      double area1 = 0, area2 = 0, area12 = 0;
      double error1 = 0, error2 = 0, error12 = 0;
      double resasc1, resasc2;
      double resabs1, resabs2;
      double last_e_i;

      /* Bisect the subinterval with the largest error estimate */

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      current_level = workspace->level[workspace->i] + 1;

      if (current_level >= wf->n)
        {
          error_type = -1 ; /* exceeded limit of table */
          break ;
        }

      a1 = a_i;
      b1 = 0.5 * (a_i + b_i);
      a2 = b1;
      b2 = b_i;

      iteration++;

      qc25f (f, a1, b1, wf, current_level, &area1, &error1, &resabs1, &resasc1);
      qc25f (f, a2, b2, wf, current_level, &area2, &error2, &resabs2, &resasc2);

      area12 = area1 + area2;
      error12 = error1 + error2;
      last_e_i = e_i;

      /* Improve previous approximations to the integral and test for
         accuracy.

         We write these expressions in the same way as the original
         QUADPACK code so that the rounding errors are the same, which
         makes testing easier. */

      errsum = errsum + error12 - e_i;
      area = area + area12 - r_i;

      tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (area));

      if (resasc1 != error1 && resasc2 != error2)
        {
          double delta = r_i - area12;

          if (fabs (delta) <= 1.0e-5 * fabs (area12) && error12 >= 0.99 * e_i)
            {
              if (!extrapolate)
                {
                  roundoff_type1++;
                }
              else
                {
                  roundoff_type2++;
                }
            }
          if (iteration > 10 && error12 > e_i)
            {
              roundoff_type3++;
            }
        }

      /* Test for roundoff and eventually set error flag */

      if (roundoff_type1 + roundoff_type2 >= 10 || roundoff_type3 >= 20)
        {
          error_type = 2;       /* round off error */
        }

      if (roundoff_type2 >= 5)
        {
          error_type2 = 1;
        }

      /* set error flag in the case of bad integrand behaviour at
         a point of the integration range */

      if (subinterval_too_small (a1, a2, b2))
        {
          error_type = 4;
        }

      /* append the newly-created intervals to the list */

      update (workspace, a1, b1, area1, error1, a2, b2, area2, error2);

      if (errsum <= tolerance)
        {
          goto compute_result;
        }

      if (error_type)
        {
          break;
        }

      if (iteration >= limit - 1)
        {
          error_type = 1;
          break;
        }

      /* set up variables on first iteration */

      if (iteration == 2 && extall)
        {
          error_over_large_intervals = errsum;
          ertest = tolerance;
          append_table (&table, area);
          continue;
        }

      if (disallow_extrapolation)
        {
          continue;
        }

      if (extall)
        {
          error_over_large_intervals += -last_e_i;

          if (current_level < workspace->maximum_level)
            {
              error_over_large_intervals += error12;
            }

          if (extrapolate)
            goto label70;
        }

      if (large_interval(workspace))
        {
          continue;
        }

      if (extall)
        {
          extrapolate = 1;
          workspace->nrmax = 1;
        }
      else
        {
          /* test whether the interval to be bisected next is the
             smallest interval. */
          size_t i = workspace->i;
          double width = workspace->blist[i] - workspace->alist[i];

          if (0.25 * fabs(width) * abs_omega > 2)
            continue;

          extall = 1;
          error_over_large_intervals = errsum;
          ertest = tolerance;
          continue;
        }

    label70:
      if (!error_type2 && error_over_large_intervals > ertest)
        {
          if (increase_nrmax (workspace))
            continue;
        }

      /* Perform extrapolation */

      append_table (&table, area);

      if (table.n < 3)
        {
          reset_nrmax(workspace);
          extrapolate = 0;
          error_over_large_intervals = errsum;
          continue;
        }

      qelg (&table, &reseps, &abseps);

      ktmin++;

      if (ktmin > 5 && err_ext < 0.001 * errsum)
        {
          error_type = 5;
        }

      if (abseps < err_ext)
        {
          ktmin = 0;
          err_ext = abseps;
          res_ext = reseps;
          correc = error_over_large_intervals;
          ertest = GSL_MAX_DBL (epsabs, epsrel * fabs (reseps));
          if (err_ext <= ertest)
            break;
        }

      /* Prepare bisection of the smallest interval. */

      if (table.n == 1)
        {
          disallow_extrapolation = 1;
        }

      if (error_type == 5)
        {
          break;
        }

      /* work on interval with largest error */

      reset_nrmax (workspace);
      extrapolate = 0;
      error_over_large_intervals = errsum;

    }
  while (iteration < limit);

  *result = res_ext;
  *abserr = err_ext;

  if (err_ext == GSL_DBL_MAX)
    goto compute_result;

  if (error_type || error_type2)
    {
      if (error_type2)
        {
          err_ext += correc;
        }

      if (error_type == 0)
        error_type = 3;

      if (result != 0 && area != 0)
        {
          if (err_ext / fabs (res_ext) > errsum / fabs (area))
            goto compute_result;
        }
      else if (err_ext > errsum)
        {
          goto compute_result;
        }
      else if (area == 0.0)
        {
          goto return_error;
        }
    }

  /*  Test on divergence. */

  {
    double max_area = GSL_MAX_DBL (fabs (res_ext), fabs (area));

    if (!positive_integrand && max_area < 0.01 * resabs0)
      goto return_error;
  }

  {
    double ratio = res_ext / area;

    if (ratio < 0.01 || ratio > 100 || errsum > fabs (area))
      error_type = 6;
  }

  goto return_error;

compute_result:

  *result = sum_results (workspace);
  *abserr = errsum;

return_error:

  if (error_type > 2)
    error_type--;

  if (error_type == 0)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 1)
    {
      GSL_ERROR ("number of iterations was insufficient", GSL_EMAXITER);
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("cannot reach tolerance because of roundoff error",
                 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
                 GSL_ESING);
    }
  else if (error_type == 4)
    {
      GSL_ERROR ("roundoff error detected in extrapolation table", GSL_EROUND);
    }
  else if (error_type == 5)
    {
      GSL_ERROR ("integral is divergent, or slowly convergent", GSL_EDIVERGE);
    }
  else if (error_type == -1)
    {
      GSL_ERROR ("exceeded limit of trigonometric table", GSL_ETABLE);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }

}