#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include <stdbool.h>
#include <math.h>
#include <omp.h>
#include <stdint.h>
#include "parmt_config.h"
#ifdef PARMT_USE_INTEL
#include <mkl.h>
#include <mkl_lapacke.h>
#include <mkl_cblas.h>
#include <ipps.h>
#else
#include <lapacke.h>
#include <cblas.h>
#endif
#include "parmt_mtsearch.h"
#include "iscl/array/array.h"
#include "iscl/linalg/linalg.h"
#include "iscl/memory/memory.h"
#include "iscl/signal/convolve.h"

#define LDG 8
#define LDCG 8
#define SIZEOF_DOUBLE sizeof(double)

#ifndef MAX
#define MAX(x,y) (((x) > (y)) ? (x) : (y))
#endif
#ifndef MIN
#define MIN(x,y) (((x) < (y)) ? (x) : (y))
#endif

enum covarianceMatrixType_enum
{
    INVERSE_COVARIANCE_MATRIX = 0,
    EIGEN_BASIS = 1
};

// Set the Green's functions matrix
static int computePadding64f(const int n);
static int computePadding32f(const int n);
static int setG32f(const int npts,
                   const float *__restrict__ Gxx,
                   const float *__restrict__ Gyy,
                   const float *__restrict__ Gzz,
                   const float *__restrict__ Gxy,
                   const float *__restrict__ Gxz,
                   const float *__restrict__ Gyz,
                   float *__restrict__ G);
static int setG64f(const int npts,
                   const double *__restrict__ Gxx,
                   const double *__restrict__ Gyy,
                   const double *__restrict__ Gzz,
                   const double *__restrict__ Gxy,
                   const double *__restrict__ Gxz,
                   const double *__restrict__ Gyz,
                   double *__restrict__ G);
static int setGxc64f(const int npts,
                     const double *__restrict__ Gxx,
                     const double *__restrict__ Gyy,
                     const double *__restrict__ Gzz,
                     const double *__restrict__ Gxy,
                     const double *__restrict__ Gxz,
                     const double *__restrict__ Gyz,
                     const double *__restrict__ d,
                     double *__restrict__ Gxc);
// Compute C*d
static int computeCd32f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const float *__restrict__ CeInv,
                        const float *__restrict__ d,
                        float *__restrict__ Cd);
static int computeCd64f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const double *__restrict__ CeInv,
                        const double *__restrict__ d,
                        double *__restrict__ Cd);
// Compute C*G
static int computeCG64f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const double *__restrict__ CeInv,
                        const double *__restrict__ G,
                        double *__restrict__ CG);
static int computeCG32f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const float *__restrict__ CeInv,
                        const float *__restrict__ G,
                        float *__restrict__ CG);
// Compute G^T C G
static int computeGtCG32f(const int npts,
                          const float *__restrict__ G,
                          const float *__restrict__ CG,
                          float *__restrict__ GtCG);
static int computeGtCG64f(const int npts,
                          const double *__restrict__ G,
                          const double *__restrict__ CG,
                          double *__restrict__ GtCG);
// Compute L1 objective function
static int performL1Search64f(const int nmt, const int ldm,
                              const int npts,
                              const int blockSize, const int mblock,
                              const int Mrows, const int Kcols,
                              const int klag,
                              const bool lminLags, const bool lwantLags,
                              const bool lrescale,
                              const double dnorm,
                              const double *__restrict__ Dmat,
                              const double *__restrict__ CG,
                              const double *__restrict__ Cd,
                              const double *__restrict__ mts,
                              int *__restrict__ lags,
                              double *__restrict__ phi,
                              double *__restrict__ var);
// Compute m^T G^T G m
static double computemtGTGmt64f(const double *__restrict__ m,
                                const double *__restrict__ GtG);

#pragma omp declare simd
static float computeLeastSquaresResidual32f(const float *__restrict__ m,
                                            const float *__restrict__ GtCG,
                                            const float *__restrict__ GtCd,
                                            const float dtCd);
#pragma omp declare simd
static double computeLeastSquaresResidual64f(const double *__restrict__ m,
                                             const double *__restrict__ GtCG,
                                             const double *__restrict__ GtCd,
                                             const double dtCd);

/*!
 * @brief Computes the likelihood function
 */
/*
int parmt_mtsearch64f( )
{

    return 0;
}
*/

int parmt_mtSearchKL64f(const int ldm, const int nlags, const int nmt,
                        const int npts, const int ldz,
                        const int rank,
                        const double sigma,
                        const double *__restrict__ Gxx,
                        const double *__restrict__ Gyy,
                        const double *__restrict__ Gzz,
                        const double *__restrict__ Gxy,
                        const double *__restrict__ Gxz,
                        const double *__restrict__ Gyz,
                        const double *__restrict__ sqrtEig,
                        const double *__restrict__ Z,
                        const double *__restrict__ mts,
                        const double *__restrict__ d,
                        double *__restrict__ phi)
{
    double *G, *V, *phiCerf, *phiWork, sigma2, xdiv, xdiv1, xdiv2, xdiv3, xdiv4;
    int i, ierr, ir, ldv;
    //const double sqrt2 = sqrt(2.0);
    const double sqrt2pi = sqrt(2.0*M_PI);
    //------------------------------------------------------------------------//
    ldv = ldz + (64 - ldz%64)/SIZEOF_DOUBLE;
    V = memory_calloc64f(ldv*npts);
    G = memory_calloc64f(LDG*npts);
    sigma2 = sigma*sigma;
    phiWork = memory_calloc64f(nmt);
    phiCerf = memory_calloc64f(nmt);
    ierr = setG64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", __func__);
        return -1;
    }
    ierr = array_set64f_work(nmt, 1.0, phi);
    if (nlags == 0)
    {
        // Extract
        for (ir=0; ir<rank; ir++)
        {
            // Compute outer product
            ierr = linalg_outerProduct64f_work(LAPACK_ROW_MAJOR,
                                               npts, &Z[ldz*ir],
                                               npts, &Z[ldz*ir],
                                               ldv, V);
            if (ierr != 0)
            {
                fprintf(stderr, "%s: Failed to compute outerproduct\n",
                        __func__);
            }
            // Compute objective function
            ierr = parmt_mtSearch_mahalanobis64f(ldm, nmt, npts,
                                                 ldv, false,
                                                 G, V,
                                                 mts, d, phiWork);
            xdiv1 = 1.0/(sqrt(2.0*sqrtEig[ir])*sigma);
            xdiv2 =-sqrt2pi/(sqrtEig[ir]*sigma);
            xdiv3 = 1.0/(2.0*sqrtEig[ir]*sigma2);
            xdiv4 = 2.0/sqrt(sqrtEig[ir]);
            for (i=0; i<nmt; i++)
            {
                phiCerf[i] = xdiv1*sqrt(phiWork[i]);
            }
            //
            vmdErfc(nmt, phiCerf, phiCerf, VML_HA);
            for (i=0; i<nmt; i++)
            {
                phiCerf[i] = (xdiv2*sqrt(phiWork[i]))*phiCerf[i];
            }
            for (i=0; i<nmt; i++)
            {
                phiWork[i] = xdiv4*exp(-xdiv3*phiWork[i]);
            }
            for (i=0; i<nmt; i++)
            {
                phi[i] = phi[i]*(phiCerf[i] + phiWork[i]);
            }
        }
    }
    // Scale the product by 1/(2pi^N) - recall we do not need to account for
    // the (-1)^rank term because of MKL's Ei convention.
    xdiv = 1.0/(sigma*(pow(sqrt2pi, npts)));
    cblas_dscal(nmt, xdiv, phi, 1);
    // release memory
    memory_free64f(&V);
    memory_free64f(&G);
    memory_free64f(&phiCerf);
    memory_free64f(&phiWork);
    return 0;
}

//============================================================================//
int parmt_mtSearchXC64f(const int ldm, const int nmt,
                        const int npts, const int blockSize,
                        const int maxlag, const bool lwantLag,
                        const double *__restrict__ Gxx,
                        const double *__restrict__ Gyy,
                        const double *__restrict__ Gzz,
                        const double *__restrict__ Gxy,
                        const double *__restrict__ Gxz,
                        const double *__restrict__ Gyz,
                        const double *__restrict__ mts,
                        const double *__restrict__ d,
                        double *__restrict__ phi,
                        int *__restrict__ lags)
{
    double GtG[36] __attribute__ ((aligned (64)));
    double m8[8] __attribute__ ((aligned (64)));
    double *DUmat, *DUmatT, *Gxc, *G, *Umat, *M, *uN, dNorm;
    int i, ic, idx, ierr, imt, jdx, jmt, l1, l2, lcref, lhalf,
        maxxc, mblock, mpts, MrowsXC, Ncols, nlag, pad;
    bool ltrunc;
    const double one = 1.0;
    const double zero = 0.0;
    const int Kcols = 6; // number of columns of G
    //------------------------------------------------------------------------//
    lcref = 2*npts - 1; // Cross-correlation size
    pad = computePadding64f(blockSize);
    mblock = blockSize + pad;
    mpts = lcref + computePadding64f(lcref);
    G = memory_calloc64f(LDG*npts);
    Gxc = memory_calloc64f(LDG*lcref);
    M = memory_calloc64f(mblock*8);
    Umat = memory_calloc64f(mblock*npts);
    uN = memory_calloc64f(mblock);
    DUmat = memory_calloc64f(mblock*lcref);
    DUmatT = memory_calloc64f(mpts*blockSize);
#ifdef __INTEL_COMPILER
    __assume_aligned(G, 64);
    __assume_aligned(Gxc, 64);
    __assume_aligned(M, 64);
    __assume_aligned(uN, 64);
    __assume_aligned(Umat, 64);
    __assume_aligned(DUmat, 64);
    __assume_aligned(DUmatT, 64);
#endif
    // Figure out the sizes for the cross-correlation
    MrowsXC = lcref;
    //Mrows = npts;
    ltrunc = false;
    lhalf = lcref/2;
    l1 = lhalf - maxlag;
    l2 = lhalf + maxlag;
    nlag = l2 - l1 + 1;
    if (maxlag > 0){ltrunc = true;}
    if (ltrunc)
    {
        if (maxlag >= npts)
        {
            printf("%s: Error maxlag can't exceed npts\n", __func__);
            return -1;
        }
    }
    // Set the data
    ierr = setG64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", __func__);
        return -1;
    }
    ierr = setGxc64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, d, Gxc);
    if (ierr != 0)
    {
        printf("%s: Failed to set Gxc\n", __func__);
        return -1;
    }
    // Compute G^T G
    cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                6, 6, npts, 1.0, G, LDG,
                G, LDG, 0.0, GtG, 6);
    for (i=0; i<8; i++){m8[i] = zero;}
    memory_free64f(&G);
    // Compute the normalization
    dNorm = cblas_dnrm2(npts, d, 1);
    // Cross-correlate the data
/*
        #pragma omp parallel for \
         firstprivate(csum, M, R) \
         private (i, ic, idx, imt, jdx, jmt, Ncols) \
         shared (d, CG, Dmat, mts, phi, Mrows, mblock) \
         default (none) reduction(+:var)
*/
    for (jmt=0; jmt<nmt; jmt=jmt+blockSize)
    {
        Ncols = MIN(blockSize, nmt - jmt); // Number of columns of M
        // Set the moment tensor matrix
        for (i=0; i<6; i++)
        {
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                idx = ldm*imt + i;
                jdx = mblock*i;
                M[jdx+ic] = mts[idx];
            }
        }
        // Compute the normalization - Eqn A7 Herrmann 2011
        for (ic=0; ic<Ncols; ic++)
        {
            imt = jmt + ic;
            for (i=0; i<6; i++)
            {
                m8[i] = mts[ldm*imt+i];
            }
            uN[ic] = computemtGTGmt64f(m8, GtG);
            uN[ic] = sqrt(uN[ic]);
        }
        // Compute D*U
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                    MrowsXC, Ncols, Kcols, one, Gxc, LDG,
                    M, mblock, zero, DUmat, mblock);
        // Get DUmat into a more convenient column major form
        for (ic=0; ic<Ncols; ic++)
        {
            cblas_dcopy(lcref, &DUmat[ic], mblock, &DUmatT[mpts*ic], 1);
        }
        // Compute the normalized cross-correlations
        if (!ltrunc)
        {
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                // Compute the lag where the times range from [-npts+1:npts-1]
                maxxc = array_argmax64f(lcref, &DUmatT[ic*mpts], &ierr);
                if (lwantLag){lags[imt] =-npts + 1 + maxxc;}
                // Compute the normalized cross-correlation
                phi[imt] = DUmatT[ic*mpts+maxxc]/(dNorm*uN[ic]);
                // Convert from [-1,1] to [0,1]
                phi[imt] = 0.5*(1.0 + phi[imt]);
            }
        }
        // Compute the normalized cross-correlation with max lag
        else
        {
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                // Compute the lag where the times range from
                // [lhalf-maxlag:half+maxlag]
                maxxc = l1 + array_argmax64f(nlag, &DUmatT[ic*mpts+l1], &ierr);
                if (lwantLag){lags[imt] =-npts + 1 + maxxc;}
                phi[imt] = DUmatT[ic*mpts+maxxc]/(dNorm*uN[ic]);
                // Convert from [-1,1] to [0,1]
                phi[imt] = 0.5*(1.0 + phi[imt]);
            }
        }
    }
    // Free memory
    memory_free64f(&uN);
    memory_free64f(&Umat);
    memory_free64f(&DUmat);
    memory_free64f(&DUmatT);
    memory_free64f(&Gxc);
    memory_free64f(&M);
    return ierr;
}
//============================================================================//
int parmt_mtSearchL164f(const int ldm, const int nmt,
                        const int npts, const int blockSize,
                        const int nlags, const bool lwantLags,
                        const bool lrescale,
                        const double *__restrict__ Gxx,
                        const double *__restrict__ Gyy,
                        const double *__restrict__ Gzz,
                        const double *__restrict__ Gxy,
                        const double *__restrict__ Gxz,
                        const double *__restrict__ Gyz,
                        const double *__restrict__ CeInv,
                        const double *__restrict__ mts,
                        const double *__restrict__ d,
                        double *__restrict__ phi,
                        double *__restrict__ var,
                        int *__restrict__ lags)
{
    double *CG, *Cd, *G, *Dmat, dnorm;
    //double objfn, res, viMin;
    int i, ic, ierr, k, mblock, Mrows, nmtBlocks;
    //int idx, imt, jdx, jmt, kmt, Ncols, nthreads0;
    int pad;
    const double zero = 0.0;
    const double one = 1.0;
    const int Kcols = 6; // number of columns of G
    // Set space and make G matrix
    pad = computePadding64f(blockSize);
    mblock = blockSize + pad;
    CG = memory_calloc64f(LDCG*npts);
    G = memory_calloc64f(LDG*npts);
    Cd = memory_calloc64f(npts);
    Dmat = memory_calloc64f(mblock*npts);
    Mrows = npts; // Number of rows of G
/*
#ifdef PARMT_USE_INTEL
    mkl_set_num_threads(1);
#endif
*/
#ifdef __INTEL_COMPILER
    __assume_aligned(G, 64);
    __assume_aligned(CG, 64);
    __assume_aligned(Dmat, 64);
    __assume_aligned(Cd, 64);
    __assume_aligned(mts, 64);
#endif
    ierr = setG64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", __func__);
        return -1;
    }
    nmtBlocks = (int) ((double) (nmt)/(double) (blockSize) + 1.0);
    if (nmtBlocks*blockSize < nmt)
    {
        printf("%s: Internal error - all mts wont be visited\n", __func__);
        return -1;
    }
    // They want lags but there are none to compute
    if (lwantLags && nlags == 0 && lags != NULL)
    {
        array_zeros32i_work(nmt, lags);
    }
    // Introduce the regularizer as to compare ||C_d (Gm - d)||
    computeCG64f(npts, 1, true, CeInv, G, CG);
    computeCd64f(npts, 1, true, CeInv, d, Cd);
    dnorm = array_norm64f(npts, Cd, ONE_NORM, one, &ierr);
    if (!lwantLags || nlags == 0)
    {
        // Set the observations
        for (i=0; i<npts; i++)
        {
            for (ic=0; ic<blockSize; ic++)
            {
                Dmat[mblock*i+ic] = Cd[i];
            }
        }
        ierr = performL1Search64f(nmt, ldm,
                                  npts,
                                  blockSize, mblock,
                                  Mrows, Kcols,
                                  0,
                                  false, false,
                                  lrescale,
                                  dnorm,
                                  Dmat, CG, Cd, mts,
                                  lags, phi, var);
    }
    else
    {
/*
        varLoc = array_copy64f(npts, var, &ierr);
        rnorm = memory_calloc64f(mblock);
        unorm = memory_calloc64f(mblock);
        M = memory_calloc64f(mblock*8);
        R = memory_calloc64f(mblock*npts);
        U = memory_calloc64f(mblock*npts);
*/
        array_set64f_work(nmt, zero, phi);
        for (k=-nlags; k<=nlags; k++)
        {
            // Null out the the data matrix
            memset(Dmat, 0, (size_t) (mblock*npts)*sizeof(double));
            // Time advance in Greens function emulated by data delay
            if (k < 0)
            {
                for (i=-k; i<npts; i++)
                {
                    for (ic=0; ic<blockSize; ic++)
                    {
                        Dmat[mblock*i+ic] = Cd[i+k];
                    }
                }
            }
            // Time delay in Greens function emulated by data advance
            else if (k > 0)
            {
                for (i=k; i<npts; i++)
                {
                    for (ic=0; ic<blockSize; ic++)
                    {
                        Dmat[mblock*(i-k)+ic] = Cd[i];
                    }
                }
            }
            // Straight copy
            else
            {
                for (i=0; i<npts; i++)
                {
                    for (ic=0; ic<blockSize; ic++)
                    {
                        Dmat[mblock*i+ic] = Cd[i];
                    }
                }
            }
            // Optimize at this lag
            ierr = performL1Search64f(nmt, ldm,
                                      npts,
                                      blockSize, mblock,
                                      Mrows, Kcols,
                                      k,
                                      true, lwantLags,
                                      lrescale,
                                      dnorm,
                                      Dmat, CG, Cd, mts,
                                      lags, phi, var);
/*
            // Loop on moment tensor block
            for (kmt=0; kmt<nmtBlocks; kmt++)
            {
                jmt = kmt*blockSize;
                Ncols = MIN(blockSize, nmt - jmt); // Number of columns of M
                // Set the moment tensor matrix
                for (i=0; i<6; i++)
                {
                    #pragma omp simd
                    for (ic=0; ic<Ncols; ic++)
                    {
                        imt = jmt + ic;
                        idx = ldm*imt + i;
                        jdx = mblock*i;
                        M[jdx+ic] = mts[idx];
                    }
                }
                // Compute U = GM
                cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                            Mrows, Ncols, Kcols, one, CG, LDCG,
                            M, mblock, zero, U, mblock);
                memset(rnorm, 0, (size_t) Ncols*sizeof(double));
                memset(unorm, 0, (size_t) Ncols*sizeof(double));
                #pragma omp simd
                for (i=0; i<npts*Ncols; i++)
                {
                    R[i] = fabs(Dmat[i] - U[i]);
                }
                // Compute |d - u|_1
                for (i=0; i<npts; i++)
                {
                    viMin = varLoc[i];
                    #pragma omp simd reduction(min:viMin)
                    for (ic=0; ic<Ncols; ic++)
                    {
                        rnorm[ic] = rnorm[ic] + R[mblock*i+ic];
                        viMin = fmin(viMin, R[mblock*i+ic]);
                    }
                    varLoc[i] = viMin;
                }
                // Compute |u|_1
                for (i=0; i<npts; i++)
                {
                    #pragma omp simd
                    for (ic=0; ic<Ncols; ic++)
                    {
                        unorm[ic] = unorm[ic] + fabs(U[mblock*i+ic]);
                    }
                }
                // Apply the triangle inequality
                for (ic=0; ic<Ncols; ic++)
                {
                    rnorm[ic] = 1.0 - rnorm[ic]/(dnorm + unorm[ic]);
                }
                // Compute the L1 norm and take the minimum
                if (!lwantLags)
                {
                    for (ic=0; ic<Ncols; ic++)
                    {
                        imt = jmt + ic;
                        if (phi[imt] > rnorm[ic])
                        {
                            phi[imt] = rnorm[ic];
                            lags[imt] = k;
                        }
                    }
                }
                else
                {
                    for (ic=0; ic<Ncols; ic++)
                    {
                        imt = jmt + ic;
                        phi[imt] = fmax(phi[imt], rnorm[ic]);
                    }
                }
            } // Loop on moment tensor block
*/
        } // Loop on lags
/*
        memory_free64f(&varLoc);
        memory_free64f(&rnorm);
        memory_free64f(&unorm);
        memory_free64f(&M);
        memory_free64f(&R);
        memory_free64f(&U);
*/
    }
    memory_free64f(&CG);
    memory_free64f(&G);
    memory_free64f(&Dmat);
    return ierr;
}

//============================================================================//
int parmt_mtSearch64f(const int ldm, const int nlags, const int nmt,
                      const int npts, const int ldc,
                      const bool ldiag,
                      const double *__restrict__ Gxx,
                      const double *__restrict__ Gyy,
                      const double *__restrict__ Gzz,
                      const double *__restrict__ Gxy,
                      const double *__restrict__ Gxz,
                      const double *__restrict__ Gyz,
                      const double *__restrict__ CeInv,
                      const double *__restrict__ mts,
                      const double *__restrict__ d,
                      double *__restrict__ phi)
{
    double *G;
    int ierr;
    if (nmt < 1 || npts < 1 || mts == NULL || Gxx == NULL || Gyy == NULL ||
        Gzz == NULL || Gxy == NULL || Gxz == NULL || Gxy == NULL ||
        CeInv == NULL || d == NULL || phi == NULL)
    {
        if (nmt < 1){printf("%s: No moment tensors\n", __func__);}
        if (npts < 1){printf("%s: No data points\n", __func__);}
        if (Gxx == NULL){printf("%s: Gxx is NULL\n", __func__);}
        if (Gyy == NULL){printf("%s: Gyy is NULL\n", __func__);}
        if (Gzz == NULL){printf("%s: Gzz is NULL\n", __func__);}
        if (Gxy == NULL){printf("%s: Gxy is NULL\n", __func__);}
        if (Gxz == NULL){printf("%s: Gxz is NULL\n", __func__);}
        if (CeInv == NULL){printf("%s: CeInv is NULL\n", __func__);}
        if (mts == NULL){printf("%s: mts is NULL\n", __func__);}
        if (d == NULL){printf("%s: d is NULL\n", __func__);}
        if (phi == NULL){printf("%s: phi is NULL\n", __func__);}
        return -1;
    }
    if (!ldiag && ldc < npts)
    {
        printf("%s: Error ldc must be >= npts\n", __func__);
        return -1;
    }
    if (nlags > 0 && nlags > npts)
    {
        printf("%s: Error nlags can't exceed npts\n", __func__);
        return -1;
    }
    // Set space and make G matrix
    G = memory_calloc64f(LDG*npts);
    ierr = setG64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", __func__);
        return -1;
    }
    // straight application of Ce^{-1}
    if (nlags == 0)
    {
        // Diagonal
        if (ldiag)
        {
            ierr = parmt_mtSearch_mahalanobis64f(ldm, nmt, npts,
                                                 ldc, true,
                                                 G, CeInv,
                                                 mts, d, phi);
        }
        // General matrix
        else
        {
            ierr = parmt_mtSearch_mahalanobis64f(ldm, nmt, npts,
                                                 ldc, false,
                                                 G, CeInv,
                                                 mts, d, phi);
        }
    }
    else
    {
        // Diagonal weighting matrix
        if (ldiag)
        {
            ierr = parmt_mtSearch_shiftedMahalanobis64f(nlags, nmt,
                                                        npts, ldc,
                                                        true,
                                                        G, CeInv,
                                                        mts, d, phi);
        }
        else
        {
            ierr = parmt_mtSearch_shiftedMahalanobis64f(nlags, nmt,
                                                        npts, ldc,
                                                        false,
                                                        G, CeInv,
                                                        mts, d, phi);
        }
    }
    if (ierr != 0)
    {
        printf("%s: Error computing search\n", __func__);
    }
    // Free memory
    memory_free64f(&G);
    return ierr;
}
//============================================================================//
int parmt_mtSearch32f(const int ldm, const int nlags, const int nmt,
                      const int npts, const int ldc,
                      const bool ldiag,
                      const float *__restrict__ Gxx,
                      const float *__restrict__ Gyy,
                      const float *__restrict__ Gzz,
                      const float *__restrict__ Gxy,
                      const float *__restrict__ Gxz,
                      const float *__restrict__ Gyz,
                      const float *__restrict__ CeInv,
                      const float *__restrict__ mts,
                      const float *__restrict__ d,
                      float *__restrict__ phi)
{
    float *G;
    int ierr;
    if (nmt < 1 || npts < 1 || mts == NULL || Gxx == NULL || Gyy == NULL ||
        Gzz == NULL || Gxy == NULL || Gxz == NULL || Gxy == NULL ||
        CeInv == NULL || d == NULL || phi == NULL)
    {
        if (nmt < 1){printf("%s: No moment tensors\n", __func__);}
        if (npts < 1){printf("%s: No data points\n", __func__);}
        if (Gxx == NULL){printf("%s: Gxx is NULL\n", __func__);}
        if (Gyy == NULL){printf("%s: Gyy is NULL\n", __func__);}
        if (Gzz == NULL){printf("%s: Gzz is NULL\n", __func__);}
        if (Gxy == NULL){printf("%s: Gxy is NULL\n", __func__);}
        if (Gxz == NULL){printf("%s: Gxz is NULL\n", __func__);}
        if (CeInv == NULL){printf("%s: CeInv is NULL\n", __func__);}
        if (mts == NULL){printf("%s: mts is NULL\n", __func__);}
        if (d == NULL){printf("%s: d is NULL\n", __func__);}
        if (phi == NULL){printf("%s: phi is NULL\n", __func__);}
        return -1;
    }
    if (!ldiag && ldc < npts)
    {
        printf("%s: Error ldc must be >= npts\n", __func__);
        return -1;
    }
    if (nlags > 0 && nlags > npts)
    {
        printf("%s: Error nlags can't exceed npts\n", __func__);
        return -1;
    }
    // Set space and make G matrix
    G = memory_calloc32f(LDG*npts);
    ierr = setG32f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", __func__);
        return -1;
    }
    // straight application of Ce^{-1}
    if (nlags == 0)
    {
        if (ldiag)
        {
            ierr = parmt_mtSearch_mahalanobis32f(ldm, nmt, npts,
                                                 ldc, true,
                                                 G, CeInv,
                                                 mts, d, phi);
        }
        else
        {
            ierr = parmt_mtSearch_mahalanobis32f(ldm, nmt, npts,
                                                 ldc, false,
                                                 G, CeInv,
                                                 mts, d, phi);
        }
    }
    // shifting the data to the green's functions
    else
    {
        // Diagonal weighting matrix
        if (ldiag)
        {
            ierr = parmt_mtSearch_shiftedMahalanobis32f(nlags, nmt,
                                                        npts, ldc,
                                                        true,
                                                        G, CeInv,
                                                        mts, d, phi);
        }
        // Full inverse covariance matrix
        else
        {
            ierr = parmt_mtSearch_shiftedMahalanobis32f(nlags, nmt,
                                                        npts, ldc,
                                                        false,
                                                        G, CeInv,
                                                        mts, d, phi);
        }
    }
    if (ierr != 0)
    {
        printf("%s: Error computing search\n", __func__);
    }
    // Free memory
    memory_free32f(&G);
    return ierr;
}
//============================================================================//
int parmt_mtSearch_mahalanobis64f(const int ldm, const int nmt, const int npts,
                                  const int ldc, const bool ldiag,
                                  const double *__restrict__ G,
                                  const double *__restrict__ CeInv,
                                  const double *__restrict__ mts,
                                  const double *__restrict__ d,
                                  double *__restrict__ phi)
{
    double *Cd, *CG;
    double GtCG[36], GtCd[6], m[6] __attribute__ ((aligned (64)));
    double dtCd __attribute__ ((aligned (64))) = 0.0;
    int imt, M;
    //------------------------------------------------------------------------//
    //
    // Set space
    CG = memory_calloc64f(LDCG*npts);
    Cd = memory_calloc64f(npts);
    // Compute C G
    computeCG64f(npts, ldc, ldiag, CeInv, G, CG);
    // Compute G^T C G
    computeGtCG64f(npts, G, CG, GtCG);
    // Compute C d
    computeCd64f(npts, ldc, ldiag, CeInv, d, Cd);
    // Compute d^T C G = (G^T C d)^T
    M = npts;
    cblas_dgemv(CblasRowMajor, CblasTrans, M, 6, 1.0, CG, LDCG,
                d, 1, 0.0, GtCd, 1);
    // Compute d^T C d
    dtCd = cblas_ddot(npts, d, 1, Cd, 1);
#ifdef __INTEL_COMPILER
    __assume_aligned(mts, 64);
    __assume_aligned(phi, 64);
    #pragma omp parallel for simd \
     firstprivate (GtCG, GtCd, dtCd) \
     private(m) shared(mts, phi) \
     default(none)
#else
/*
    #pragma omp parallel for simd \
     firstprivate (GtCG, GtCd, dtCd) \
     private(m) shared(mts, phi) \
     default(none) aligned(phi, mts: 64)
*/
#endif
    for (imt=0; imt<nmt; imt++)
    {
        m[0] = mts[ldm*imt];
        m[1] = mts[ldm*imt+1];
        m[2] = mts[ldm*imt+2];
        m[3] = mts[ldm*imt+3];
        m[4] = mts[ldm*imt+4];
        m[5] = mts[ldm*imt+5];
        phi[imt] = computeLeastSquaresResidual64f(m, GtCG, GtCd, dtCd);
    }
    // free space
    memory_free64f(&CG);
    memory_free64f(&Cd);
    return 0;
}

int parmt_mtSearch_mahalanobis32f(const int ldm, const int nmt, const int npts,
                                  const int ldc, const bool ldiag,
                                  const float *__restrict__ G,
                                  const float *__restrict__ CeInv,
                                  const float *__restrict__ mts,
                                  const float *__restrict__ d,
                                  float *__restrict__ phi)
{
    float *Cd, *CG;
    float GtCG[36], GtCd[6], m[6] __attribute__ ((aligned (64)));
    float dtCd __attribute__ ((aligned (64))) = 0.0f;
    int imt, M;
    //------------------------------------------------------------------------//
    //
    // Set space
    CG = memory_calloc32f(LDCG*npts);
    Cd = memory_calloc32f(npts);
    // Compute C G
    computeCG32f(npts, ldc, ldiag, CeInv, G, CG);
    // Compute G^T C G
    computeGtCG32f(npts, G, CG, GtCG);
    // Compute C d
    computeCd32f(npts, ldc, ldiag, CeInv, d, Cd);
    // Compute d^T C G = (G^T C d)^T
    M = npts;
    cblas_sgemv(CblasRowMajor, CblasTrans, M, 6, 1.0f, CG, LDCG,
                d, 1, 0.0f, GtCd, 1);
    // Compute d^T C d
    dtCd = cblas_sdot(npts, d, 1, Cd, 1);
#ifdef __INTEL_COMPILER
    __assume_aligned(mts, 64);
    __assume_aligned(phi, 64);
    #pragma omp parallel for simd \
     firstprivate (GtCG, GtCd, dtCd) \
     private(m) shared(mts, phi) \
     default(none)
#else
/*
    #pragma omp parallel for simd \
     firstprivate (GtCG, GtCd, dtCd) \
     private(m) shared(mts, phi) \
     default(none) aligned(phi, mts: 64)
*/
#endif
    for (imt=0; imt<nmt; imt++)
    {
        m[0] = mts[ldm*imt];
        m[1] = mts[ldm*imt+1];
        m[2] = mts[ldm*imt+2];
        m[3] = mts[ldm*imt+3];
        m[4] = mts[ldm*imt+4];
        m[5] = mts[ldm*imt+5];
        phi[imt] = computeLeastSquaresResidual32f(m, GtCG, GtCd, dtCd);
    }
    // free space
    memory_free32f(&CG);
    memory_free32f(&Cd);
    return 0;
}
//============================================================================//
int parmt_mtSearch_shiftedMahalanobis64f(const int nlags, const int nmt,
                                         const int npts, const int ldc,
                                         const bool ldiag,
                                         const double *__restrict__ G,
                                         const double *__restrict__ CeInv,
                                         const double *__restrict__ mts,
                                         const double *__restrict__ d,
                                         double *__restrict__ phi)
{
    int ncopy;
    double *Cd, *CG, *dwork, *phiLoc;
    double GtCG[36], GtCd[6], m[6] __attribute__ ((aligned (64)));
    double objfn __attribute__ ((aligned (64))) = 0.0;
    double dtCd __attribute__ ((aligned (64))) = 0.0;
    int i, imt, k, M;
    bool linit;
    //------------------------------------------------------------------------//
    //
    // Try to avoid some problems
    if (nmt < 1 || npts < 1)
    {
        printf("%s: Invalid number of points %d %d\n", __func__, nmt, npts);
        return -1;
    }
    if (G == NULL || CeInv == NULL || mts == NULL || d == NULL || phi == NULL)
    {
        printf("%s: Null pointer detected\n", __func__);
        return -1;
    }
    if (memory_isAligned(CeInv, 64) != 1 || memory_isAligned(mts, 64) != 1)
    {
        printf("%s: CeInv and mts must be 64 bit aligned\n", __func__);
        return -1;
    }
    // Set space
    CG = memory_calloc64f(LDCG*npts);
    Cd = memory_calloc64f(npts);
    dwork = memory_calloc64f(npts);
    phiLoc = memory_calloc64f(nmt);
    // Compute C G
    computeCG64f(npts, ldc, ldiag, CeInv, G, CG);
    // Compute G^T C G
    computeGtCG64f(npts, G, CG, GtCG);
#ifdef __INTEL_COMPILER
    __assume_aligned(mts, 64);
    __assume_aligned(phiLoc, 64);
#endif
    // Loop on lags
    linit = false;
    #pragma omp parallel \
     firstprivate (dwork, Cd, dtCd, GtCd, GtCG, linit, phiLoc) \
     private (i, imt, k, m, M, ncopy, objfn) \
     shared (CeInv, CG, d, mts, phi) \
     default (none)
    {
    for (k=-nlags; k<=nlags; k++)
    {
        // shift greens fns left by shifting data right
        if (k < 0)
        {
            ncopy = npts + k;
            for (i=0; i<-k; i++){dwork[i] = 0.0;}
            cblas_dcopy(ncopy, d, 1, &dwork[-k], 1);
        }
        else
        {
            // shift greens fns right by shifting data left
            if (k > 0)
            {
                ncopy = npts - k;
                for (i=ncopy; i<npts; i++){dwork[i] = 0.0;}
                cblas_dcopy(ncopy, &d[k], 1, dwork, 1);
            }
            else
            {
                cblas_dcopy(npts, d, 1, dwork, 1);
            }
        }
        // Compute C d
        computeCd64f(npts, ldc, ldiag, CeInv, dwork, Cd);
        // Compute d^T C G = (G^T C d)^T
        M = npts;
        cblas_dgemv(CblasRowMajor, CblasTrans, M, 6, 1.0, CG, LDCG,
                    dwork, 1, 0.0, GtCd, 1);
        // Compute d^T C d
        dtCd = cblas_ddot(npts, dwork, 1, Cd, 1);
        if (linit)
        {
            for (imt=0; imt<nmt; imt++)
            {
                m[0] = mts[8*imt];
                m[1] = mts[8*imt+1];
                m[2] = mts[8*imt+2];
                m[3] = mts[8*imt+3];
                m[4] = mts[8*imt+4];
                m[5] = mts[8*imt+5];
                objfn = computeLeastSquaresResidual64f(m, GtCG, GtCd, dtCd);
                phiLoc[imt] = fmin(phiLoc[imt], objfn);
            }
        }
        else
        {
            for (imt=0; imt<nmt; imt++)
            {
                m[0] = mts[8*imt];
                m[1] = mts[8*imt+1];
                m[2] = mts[8*imt+2];
                m[3] = mts[8*imt+3];
                m[4] = mts[8*imt+4];
                m[5] = mts[8*imt+5];
                objfn = computeLeastSquaresResidual64f(m, GtCG, GtCd, dtCd);
                phiLoc[imt] = objfn;
            }
            linit = true;
        }
    }
    #pragma omp barrier
    // Reduction
    for (imt=0; imt<nmt; imt++)
    {
        phi[imt] = phiLoc[imt];
    }
    // End parallel section
    }
    // free space
    memory_free64f(&CG);
    memory_free64f(&Cd);
    memory_free64f(&dwork);
    memory_free64f(&phiLoc);
    return 0;
}

int parmt_mtSearch_shiftedMahalanobis32f(const int nlags, const int nmt,
                                         const int npts, const int ldc,
                                         const bool ldiag,
                                         const float *__restrict__ G,
                                         const float *__restrict__ CeInv,
                                         const float *__restrict__ mts,
                                         const float *__restrict__ d,
                                         float *__restrict__ phi)
{
    int ncopy;
    float *Cd, *CG, *dwork, *phiLoc;
    float GtCG[36], GtCd[6], m[6] __attribute__ ((aligned (64)));
    float objfn __attribute__ ((aligned (64))) = 0.0f;
    float dtCd __attribute__ ((aligned (64))) = 0.0f;
    int i, imt, k, M;
    bool linit;
    //------------------------------------------------------------------------//
    //
    // Try to avoid some problems
    if (nmt < 1 || npts < 1)
    {
        printf("%s: Invalid number of points %d %d\n", __func__, nmt, npts);
        return -1;
    }
    if (G == NULL || CeInv == NULL || mts == NULL || d == NULL || phi == NULL)
    {
        printf("%s: Null pointer detected\n", __func__);
        return -1;
    }
    if (memory_isAligned(CeInv, 64) != 1 || memory_isAligned(mts, 64) != 1)
    {
        printf("%s: CeInv and mts must be 64 bit aligned\n", __func__);
        return -1;
    }
    // Set space
    CG = memory_calloc32f(LDCG*npts);
    Cd = memory_calloc32f(npts);
    dwork = memory_calloc32f(npts);
    phiLoc = memory_calloc32f(nmt);
    // Compute C G
    computeCG32f(npts, ldc, ldiag, CeInv, G, CG);
    // Compute G^T C G
    computeGtCG32f(npts, G, CG, GtCG);
#ifdef __INTEL_COMPILER
    __assume_aligned(mts, 64);
    __assume_aligned(phiLoc, 64);
#endif
    // Loop on lags
    linit = false;
    #pragma omp parallel \
     firstprivate (dwork, Cd, dtCd, linit, GtCd, GtCG, phiLoc) \
     private (i, imt, k, m, M, ncopy, objfn) \
     shared (CeInv, CG, d, mts, phi) \
     default (none)
    {
    for (k=-nlags; k<=nlags; k++)
    {
        // shift greens fns left by shifting data right
        if (k < 0)
        {
            ncopy = npts + k;
            for (i=0; i<-k; i++){dwork[i] = 0.0f;}
            cblas_scopy(ncopy, d, 1, &dwork[-k], 1);
        }
        else
        {
            // shift greens fns right by shifting data left
            if (k > 0)
            {
                ncopy = npts - k;
                for (i=ncopy; i<npts; i++){dwork[i] = 0.0f;}
                cblas_scopy(ncopy, &d[k], 1, dwork, 1);
            }
            else
            {
                cblas_scopy(npts, d, 1, dwork, 1);
            }
        }
        // Compute C d
        computeCd32f(npts, ldc, ldiag, CeInv, dwork, Cd);
        // Compute d^T C G = (G^T C d)^T
        M = npts;
        cblas_sgemv(CblasRowMajor, CblasTrans, M, 6, 1.0f, CG, LDCG,
                    dwork, 1, 0.0f, GtCd, 1);
        // Compute d^T C d
        dtCd = cblas_sdot(npts, dwork, 1, Cd, 1);
        if (linit)
        {
            for (imt=0; imt<nmt; imt++)
            {
                m[0] = mts[8*imt];
                m[1] = mts[8*imt+1];
                m[2] = mts[8*imt+2];
                m[3] = mts[8*imt+3];
                m[4] = mts[8*imt+4];
                m[5] = mts[8*imt+5];
                objfn = computeLeastSquaresResidual32f(m, GtCG, GtCd, dtCd);
                phiLoc[imt] = fminf(phiLoc[imt], objfn);
            }
        }
        else
        {
            for (imt=0; imt<nmt; imt++)
            {
                m[0] = mts[8*imt];
                m[1] = mts[8*imt+1];
                m[2] = mts[8*imt+2];
                m[3] = mts[8*imt+3];
                m[4] = mts[8*imt+4];
                m[5] = mts[8*imt+5];
                objfn = computeLeastSquaresResidual32f(m, GtCG, GtCd, dtCd);
                phiLoc[imt] = objfn;
            }
            linit = true;
        }
    }
    #pragma omp barrier
    // Reduction
    for (imt=0; imt<nmt; imt++)
    {
        phi[imt] = phiLoc[imt];
    }
    // End parallel section
    }
    // free space
    memory_free32f(&CG);
    memory_free32f(&Cd);
    memory_free32f(&dwork);
    memory_free32f(&phiLoc);
    return 0;
}
//============================================================================//
//                                PRIVATE FUNCTIONS                           //
//============================================================================//
/*!
 * @brief Sets the single precision Green's functions matrix
 *
 * @param[in] npts      number of points in Green's functions.
 * @param[in] Gxx       64 bit aligned Green's functions which are scaled by mxx
 *                      moment tensor term [npts]
 * @param[in] Gyy       64 bit aligned Green's functions which are scaled by myy
 *                      moment tensor term [npts]
 * @param[in] Gzz       64 bit aligned Green's functions which are scaled by mzz
 *                      moment tensor term [npts]
 * @param[in] Gxy       64 bit aligned Green's functions which are scaled by mxy
 *                      moment tensor term [npts]
 * @param[in] Gxz       64 bit aligned Green's functions which are scaled by mxz
 *                      moment tensor term [npts]
 * @param[in] Gyz       64 bit aligned Green's functions which are scaled by myz
 *                      moment tensor term [npts
 *
 * @param[out] G        64 bit aligned Green's functions matrix in row major
 *                      format that multiplies moment tensor array
 *                      $\{ m_{xx}, m_{yy}, m_{zz}, m_{xy}, m_{xz}, m_{yz} \}^T$
 *                      [npts x LDG]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int setG32f(const int npts,
                   const float *__restrict__ Gxx,
                   const float *__restrict__ Gyy,
                   const float *__restrict__ Gzz,
                   const float *__restrict__ Gxy,
                   const float *__restrict__ Gxz,
                   const float *__restrict__ Gyz,
                   float *__restrict__ G)
{
    int i;
    bool lalign;
    if (Gxx == NULL || Gyy == NULL || Gzz == NULL || Gxy == NULL ||
        Gxz == NULL || G == NULL || npts < 1)
    {
        if (Gxx == NULL){printf("%s: Error Gxx is NULL\n", __func__);}
        if (Gyy == NULL){printf("%s: Error Gyy is NULL\n", __func__);}
        if (Gzz == NULL){printf("%s: Error Gzz is NULL\n", __func__);}
        if (Gxy == NULL){printf("%s: Error Gxy is NULL\n", __func__);}
        if (Gxz == NULL){printf("%s: Error Gxz is NULL\n", __func__);}
        if (Gyz == NULL){printf("%s: Error Gyz is NULL\n", __func__);}
        if (npts < 1)
        {
            printf("%s: No points in Green's functions\n", __func__);
        }
        return -1;
    }
    lalign = true;
    if (memory_isAligned(Gxx, 64) != 1 || memory_isAligned(Gyy, 64) != 1 ||
        memory_isAligned(Gzz, 64) != 1 || memory_isAligned(Gxy, 64) != 1 ||
        memory_isAligned(Gxz, 64) != 1 || memory_isAligned(Gyz, 64) != 1 ||
        memory_isAligned(G, 64) != 1)
    {
        lalign = false;
    }
    if (lalign)
    {
        #pragma omp simd aligned(G, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz: 64)
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    else
    {
        #pragma omp simd
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    return 0;
}
//============================================================================//
static int setGxc64f(const int npts,
                     const double *__restrict__ Gxx,
                     const double *__restrict__ Gyy,
                     const double *__restrict__ Gzz,
                     const double *__restrict__ Gxy,
                     const double *__restrict__ Gxz,
                     const double *__restrict__ Gyz,
                     const double *__restrict__ d,
                     double *__restrict__ Gxc)
{
    double *gxxWork, *gyyWork, *gzzWork, *gxyWork, *gxzWork, *gyzWork;
    int ierr, lcref;
    // Set the data
    gxxWork = signal_convolve_correlate64f(npts, d, npts, Gxx,
                                           CONVCOR_FULL, &lcref, &ierr);
    gyyWork = signal_convolve_correlate64f(npts, d, npts, Gyy,
                                           CONVCOR_FULL, &lcref, &ierr);
    gzzWork = signal_convolve_correlate64f(npts, d, npts, Gzz,
                                           CONVCOR_FULL, &lcref, &ierr);
    gxyWork = signal_convolve_correlate64f(npts, d, npts, Gxy,
                                           CONVCOR_FULL, &lcref, &ierr);
    gxzWork = signal_convolve_correlate64f(npts, d, npts, Gxz,
                                           CONVCOR_FULL, &lcref, &ierr);
    gyzWork = signal_convolve_correlate64f(npts, d, npts, Gyz,
                                           CONVCOR_FULL, &lcref, &ierr);
    if (lcref != 2*npts - 1)
    {
        fprintf(stderr, "%s: Error invalid number of points\n", __func__);
        ierr = 1;
    }
    cblas_dcopy(lcref, gxxWork, 1, &Gxc[0], LDG);
    cblas_dcopy(lcref, gyyWork, 1, &Gxc[1], LDG);
    cblas_dcopy(lcref, gzzWork, 1, &Gxc[2], LDG);
    cblas_dcopy(lcref, gxyWork, 1, &Gxc[3], LDG);
    cblas_dcopy(lcref, gxzWork, 1, &Gxc[4], LDG);
    cblas_dcopy(lcref, gyzWork, 1, &Gxc[5], LDG);
    memory_free64f(&gxxWork);
    memory_free64f(&gyyWork);
    memory_free64f(&gzzWork);
    memory_free64f(&gxyWork);
    memory_free64f(&gxzWork);
    memory_free64f(&gyzWork);
    return ierr;
}
/*!
 * @brief Sets the double precision Green's functions matrix
 *
 * @param[in] npts      number of points in Green's functions.
 * @param[in] Gxx       64 bit aligned Green's functions which are scaled by mxx
 *                      moment tensor term [npts]
 * @param[in] Gyy       64 bit aligned Green's functions which are scaled by myy
 *                      moment tensor term [npts]
 * @param[in] Gzz       64 bit aligned Green's functions which are scaled by mzz
 *                      moment tensor term [npts]
 * @param[in] Gxy       64 bit aligned Green's functions which are scaled by mxy
 *                      moment tensor term [npts]
 * @param[in] Gxz       64 bit aligned Green's functions which are scaled by mxz
 *                      moment tensor term [npts]
 * @param[in] Gyz       64 bit aligned Green's functions which are scaled by myz
 *                      moment tensor term [npts
 *
 * @param[out] G        64 bit aligned Green's functions matrix in row major
 *                      format that multiplies moment tensor array
 *                      $\{ m_{xx}, m_{yy}, m_{zz}, m_{xy}, m_{xz}, m_{yz} \}^T$
 *                      [npts x LDG]
 *
 * @result 0 indicates success
 *
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int setG64f(const int npts,
                   const double *__restrict__ Gxx,
                   const double *__restrict__ Gyy,
                   const double *__restrict__ Gzz,
                   const double *__restrict__ Gxy,
                   const double *__restrict__ Gxz,
                   const double *__restrict__ Gyz,
                   double *__restrict__ G)
{
    int i;
    bool lalign;
    if (Gxx == NULL || Gyy == NULL || Gzz == NULL || Gxy == NULL ||
        Gxz == NULL || G == NULL || npts < 1)
    {
        if (Gxx == NULL){printf("%s: Error Gxx is NULL\n", __func__);}
        if (Gyy == NULL){printf("%s: Error Gyy is NULL\n", __func__);}
        if (Gzz == NULL){printf("%s: Error Gzz is NULL\n", __func__);}
        if (Gxy == NULL){printf("%s: Error Gxy is NULL\n", __func__);}
        if (Gxz == NULL){printf("%s: Error Gxz is NULL\n", __func__);}
        if (Gyz == NULL){printf("%s: ERror Gyz is NULL\n", __func__);}
        if (npts < 1)
        {
            printf("%s: No points in Green's functions\n", __func__);
        }
        return -1;
    }
    lalign = true;
    if (memory_isAligned(Gxx, 64) != 1 || memory_isAligned(Gyy, 64) != 1 ||
        memory_isAligned(Gzz, 64) != 1 || memory_isAligned(Gxy, 64) != 1 ||
        memory_isAligned(Gxz, 64) != 1 || memory_isAligned(Gyz, 64) != 1 ||
        memory_isAligned(G, 64) != 1)
    {
        lalign = false;
    }
    if (lalign)
    {
        #pragma omp simd aligned(G, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz: 64)
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    else
    {
        #pragma omp simd
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Computes the squared residual error for the given moment tensor
 *        by expanding
 *        \f[
 *          (G \textbf{m} - \textbf{d})^T C_e^{-1} (G \textbf{m} - \textbf{d} )
 *         = G^T C_e^{-1} G^T
 *         - 2 m^T G^T C \textbf{d}
 *         + \textbf{d}^T C_{e}^{-1} \textbf{d}
 *        \f]
 *
 * @param[in] m     moment tensor terms [6]
 * @param[in] GtCG  normal equation \f$ G^T C_{e}^{-1} G \f$ [8 x 8].
 * @param[in] GtCd  the \f$ G^T C_{e}^{-1} \textbf{d} \f$ term [6 x 1]
 * @param[in] dtCd  the \f$ \textbf{d}^T C_{e}^{-1} \texbf{d} \f$ term
 *
 * @result squared residual error
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
#pragma omp declare simd
static float computeLeastSquaresResidual32f(const float *__restrict__ m,
                                            const float *__restrict__ GtCG,
                                            const float *__restrict__ GtCd,
                                            const float dtCd)
{
    const float __attribute__ ((aligned (64))) mtwo =-2.0f;
    float l2 __attribute__ ((aligned (64))) = 0.0f;
    float mtGtd, mtGtG, two_mtGtd_dtd __attribute__ ((aligned (64)));
    // m^T G^T C d
    mtGtd = m[0]*GtCd[0] + m[1]*GtCd[1] + m[2]*GtCd[2]
          + m[3]*GtCd[3] + m[4]*GtCd[4] + m[5]*GtCd[5];
    // -2 m^T G^T C d + d^T C d
    two_mtGtd_dtd = mtwo*mtGtd + dtCd;
    // m^T G^T C G m
    mtGtG = m[0]*(m[0]*GtCG[0]  + m[1]*GtCG[1]  + m[2]*GtCG[2]
                + m[3]*GtCG[3]  + m[4]*GtCG[4]  + m[5]*GtCG[5])
          + m[1]*(m[0]*GtCG[6]  + m[1]*GtCG[7]  + m[2]*GtCG[8]
                + m[3]*GtCG[9]  + m[4]*GtCG[10] + m[5]*GtCG[11])
          + m[2]*(m[0]*GtCG[12] + m[1]*GtCG[13] + m[2]*GtCG[14]
                + m[3]*GtCG[15] + m[4]*GtCG[16] + m[5]*GtCG[17])
          + m[3]*(m[0]*GtCG[18] + m[1]*GtCG[19] + m[2]*GtCG[20]
                + m[3]*GtCG[21] + m[4]*GtCG[22] + m[5]*GtCG[23])
          + m[4]*(m[0]*GtCG[24] + m[1]*GtCG[25] + m[2]*GtCG[26]
                + m[3]*GtCG[27] + m[4]*GtCG[28] + m[5]*GtCG[29])
          + m[5]*(m[0]*GtCG[30] + m[1]*GtCG[31] + m[2]*GtCG[32]
                + m[3]*GtCG[33] + m[4]*GtCG[34] + m[5]*GtCG[35]);
    l2 = MAX(0.0f, mtGtG + two_mtGtd_dtd);
    return l2;
}
//============================================================================//
/*!
 * @brief Computes the squared residual error for the given moment tensor
 *        by expanding
 *        \f[
 *          (G \textbf{m} - \textbf{d})^T C_e^{-1} (G \textbf{m} - \textbf{d} )
 *         = G^T C_e^{-1} G^T
 *         - 2 m^T G^T C \textbf{d}
 *         + \textbf{d}^T C_{e}^{-1} \textbf{d}
 *        \f]
 *
 * @param[in] m     moment tensor terms [6]
 * @param[in] GtCG  normal equation \f$ G^T C_{e}^{-1} G \f$ [8 x 8].
 * @param[in] GtCd  the \f$ G^T C_{e}^{-1} \textbf{d} \f$ term [6 x 1]
 * @param[in] dtCd  the \f$ \textbf{d}^T C_{e}^{-1} \texbf{d} \f$ term
 *
 * @result squared residual error
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
#pragma omp declare simd
static double computeLeastSquaresResidual64f(const double *__restrict__ m,
                                             const double *__restrict__ GtCG,
                                             const double *__restrict__ GtCd,
                                             const double dtCd)
{
    const double __attribute__ ((aligned (64))) mtwo =-2.0;
    double l2 __attribute__ ((aligned (64))) = 0.0;
    double mtGtd, mtGtG, two_mtGtd_dtd __attribute__ ((aligned (64)));
    // m^T G^T C d
    mtGtd = m[0]*GtCd[0] + m[1]*GtCd[1] + m[2]*GtCd[2]
          + m[3]*GtCd[3] + m[4]*GtCd[4] + m[5]*GtCd[5];
    // -2 m^T G^T C d + d^T C d
    two_mtGtd_dtd = mtwo*mtGtd + dtCd;
    // m^T G^T C G m
    mtGtG = m[0]*(m[0]*GtCG[0]  + m[1]*GtCG[1]  + m[2]*GtCG[2]
                + m[3]*GtCG[3]  + m[4]*GtCG[4]  + m[5]*GtCG[5])
          + m[1]*(m[0]*GtCG[6]  + m[1]*GtCG[7]  + m[2]*GtCG[8]
                + m[3]*GtCG[9]  + m[4]*GtCG[10] + m[5]*GtCG[11])
          + m[2]*(m[0]*GtCG[12] + m[1]*GtCG[13] + m[2]*GtCG[14]
                + m[3]*GtCG[15] + m[4]*GtCG[16] + m[5]*GtCG[17])
          + m[3]*(m[0]*GtCG[18] + m[1]*GtCG[19] + m[2]*GtCG[20]
                + m[3]*GtCG[21] + m[4]*GtCG[22] + m[5]*GtCG[23])
          + m[4]*(m[0]*GtCG[24] + m[1]*GtCG[25] + m[2]*GtCG[26]
                + m[3]*GtCG[27] + m[4]*GtCG[28] + m[5]*GtCG[29])
          + m[5]*(m[0]*GtCG[30] + m[1]*GtCG[31] + m[2]*GtCG[32]
                + m[3]*GtCG[33] + m[4]*GtCG[34] + m[5]*GtCG[35]);
    l2 = MAX(0.0, mtGtG + two_mtGtd_dtd);
    return l2;
}
//============================================================================//
static double computemtGTGmt64f(const double *__restrict__ m,
                                const double *__restrict__ GtG)
{
    double mtGtG;
    // m^T G^T C G m
    mtGtG = m[0]*(m[0]*GtG[0]  + m[1]*GtG[1]  + m[2]*GtG[2]
                + m[3]*GtG[3]  + m[4]*GtG[4]  + m[5]*GtG[5])
          + m[1]*(m[0]*GtG[6]  + m[1]*GtG[7]  + m[2]*GtG[8]
                + m[3]*GtG[9]  + m[4]*GtG[10] + m[5]*GtG[11])
          + m[2]*(m[0]*GtG[12] + m[1]*GtG[13] + m[2]*GtG[14]
                + m[3]*GtG[15] + m[4]*GtG[16] + m[5]*GtG[17])
          + m[3]*(m[0]*GtG[18] + m[1]*GtG[19] + m[2]*GtG[20]
                + m[3]*GtG[21] + m[4]*GtG[22] + m[5]*GtG[23])
          + m[4]*(m[0]*GtG[24] + m[1]*GtG[25] + m[2]*GtG[26]
                + m[3]*GtG[27] + m[4]*GtG[28] + m[5]*GtG[29])
          + m[5]*(m[0]*GtG[30] + m[1]*GtG[31] + m[2]*GtG[32]
                + m[3]*GtG[33] + m[4]*GtG[34] + m[5]*GtG[35]);
    return mtGtG;
}
//============================================================================//
static int performL1Search64f(const int nmt, const int ldm,
                              const int npts,
                              const int blockSize, const int mblock,
                              const int Mrows, const int Kcols,
                              const int klag,
                              const bool lminLags, const bool lwantLags,
                              const bool lrescale,
                              const double dnorm,
                              const double *__restrict__ Dmat,
                              const double *__restrict__ CG,
                              const double *__restrict__ Cd,
                              const double *__restrict__ mts,
                              int *__restrict__ lags,
                              double *__restrict__ phi,
                              double *__restrict__ var)
{
    double *M, *R, *rnorm, *U, *unorm, *varLoc, *xden, robj, xnum, viMin;
    int i, ic, idx, jdx, ierr, imt, jmt, kmt, Ncols, nmtBlocks;
    const double one = 1.0;
    const double zero = 0.0;
    ierr = 0;
    nmtBlocks = (int) ((double) (nmt)/(double) (blockSize) + 1.0);
    if (nmtBlocks*blockSize < nmt)
    {
        printf("%s: Internal error - all mts wont be visited\n", __func__);
        return -1;
    }
    // Get the size of the numerator - recall Dmat is simply a copy of itself
    xnum = 1.0;
    if (lrescale)
    {
        ic = 0;
        i = 0;
        xnum = fabs(Dmat[mblock*i+ic]);
        for (i=0; i<npts; i++)
        {
            xnum = fmax(xnum, fabs(Dmat[mblock*i+ic]));
        }
    }
    #pragma omp parallel \
     firstprivate(dnorm, xnum) \
     private (i, ic, idx, imt, jdx, jmt, M, Ncols, R, rnorm, robj, viMin, \
              U, unorm, xden, varLoc) \
     shared (Cd, CG, Dmat, lags, mts, nmtBlocks, phi, var) \
     default (none) reduction(+:ierr)
    {
    varLoc = array_copy64f(npts, var, &ierr);
    rnorm = memory_calloc64f(mblock);
    unorm = memory_calloc64f(mblock);
    xden = memory_calloc64f(mblock);
    M = memory_calloc64f(mblock*8);
    R = memory_calloc64f(mblock*npts);
    U = memory_calloc64f(mblock*npts);
#ifdef __INTEL_COMPILER
    __assume_aligned(varLoc, 64);
    __assume_aligned(rnorm, 64);
    __assume_aligned(unorm, 64);
    __assume_aligned(xden, 64);
    __assume_aligned(M, 64);
    __assume_aligned(U, 64);
    __assume_aligned(R, 64);
    __assume_aligned(CG, 64);
    __assume_aligned(Cd, 64);
#endif
    #pragma omp for
    for (kmt=0; kmt<nmtBlocks; kmt++)
    {
        jmt = kmt*blockSize;
        Ncols = MIN(blockSize, nmt - jmt); // Number of columns of M
        // Set the moment tensor matrix
        for (i=0; i<6; i++)
        {
            #pragma omp simd aligned(M: 64)
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                idx = ldm*imt + i;
                jdx = mblock*i;
                M[jdx+ic] = mts[idx];
            }
        }
        // Compute U = GM
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                    Mrows, Ncols, Kcols, one, CG, LDCG,
                    M, mblock, zero, U, mblock);
        // Rescale the synthetic data
        if (lrescale)
        {
            i = 0;
            for (ic=0; ic<Ncols; ic++)
            {
                xden[ic] = fabs(U[mblock*i+ic]);
            }
            i = 1;
            for (i=0; i<npts; i++)
            {
                for (ic=0; ic<Ncols; ic++)
                {
                    xden[ic] = fmax(xden[ic], fabs(U[mblock*i+ic]));
                }
            }
            // Rescale the synthetics to the observed
            for (ic=0; ic<Ncols; ic++)
            {
                xden[ic] = xnum/xden[ic];
                cblas_dscal(npts, xden[ic], &U[ic], mblock);
                //int imax = cblas_idamax(npts, &U[ic], mblock);
                //int jmax = cblas_idamax(npts, &Dmat[ic], mblock);
                //printf("%e %e\n", U[imax*mblock+ic], Dmat[jmax*mblock+ic]);
            }
            //getchar();
        }
        memset(rnorm, 0, (size_t) Ncols*sizeof(double));
        memset(unorm, 0, (size_t) Ncols*sizeof(double));
        #pragma omp simd aligned(R, Dmat, U: 64)
        for (i=0; i<mblock*npts; i++)
        {
            R[i] = fabs(Dmat[i] - U[i]);
        }
        // Compute |d - u|_1
        for (i=0; i<npts; i++)
        {
            viMin = varLoc[i];
            #pragma omp simd aligned(rnorm, R: 64) reduction(min:viMin)
            for (ic=0; ic<Ncols; ic++)
            {
                rnorm[ic] = rnorm[ic] + R[mblock*i+ic];
                viMin = fmin(viMin, R[mblock*i+ic]);
            }
            varLoc[i] = viMin;
        }
        // Compute |u|_1
        for (i=0; i<npts; i++)
        {
            #pragma omp simd aligned(unorm, U: 64)
            for (ic=0; ic<Ncols; ic++)
            {
                unorm[ic] = unorm[ic] + fabs(U[mblock*i+ic]);
            }
        }
        // Apply the triangle inequality
        if (!lminLags)
        {
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                //printf("%d %e %e\n", imt, Dmat[npts/2+ic], unorm[ic]);
                phi[imt] = 1.0 - rnorm[ic]/(dnorm + unorm[ic]);
            }
        }
        else
        {
            if (!lwantLags)
            {
                for (ic=0; ic<Ncols; ic++)
                {
                    imt = jmt + ic;
                    robj = 1.0 - rnorm[ic]/(dnorm + unorm[ic]);
                    phi[imt] = fmax(phi[imt], robj);
                }
            }
            else
            {
                for (ic=0; ic<Ncols; ic++)
                {
                    imt = jmt + ic;
                    robj = 1.0 - rnorm[ic]/(dnorm + unorm[ic]);
                    if (robj > phi[imt])
                    {
                        phi[imt] = robj;
                        lags[imt] = klag;
                    }
                }
            }
        }
    } // Loop on moment tensor block
    #pragma omp critical
    {
    for (i=0; i<npts; i++)
    {
        var[i] = fmin(var[i], varLoc[i]);
    }
    } // End critical section
    memory_free64f(&varLoc);
    memory_free64f(&rnorm);
    memory_free64f(&unorm);
    memory_free64f(&xden);
    memory_free64f(&M);
    memory_free64f(&R);
    memory_free64f(&U);
    } // end parallel section
    return ierr;
}
//============================================================================//
/*!
 * @brief Computes Cd vector
 *
 * @param[in] npts   number of points
 * @param[in] ldc    leading dimension of CeInv.  if ldiag is true then
 *                   this should be 1 otherwise, this should be >= npts.
 * @param[in] CeInv  inverse of data error (covariance) matrix.  if
 *                   ldiag is true then this is diagonal of length [nobs].
 *                   if false then it is a symmetric [nobs x ldc] matrix
 *                   in row major format where ldc >= nobs.
 * @param[in] d      data vector [npts]
 *
 * @param[out] Cd    multiplication of C*d [npts]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeCd32f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const float *__restrict__ CeInv,
                        const float *__restrict__ d,
                        float *__restrict__ Cd)
{
    int i, M, N;
    // Compute C d
    if (CeInv != NULL)
    {
        if (ldiag)
        {
            #pragma omp simd aligned(Cd, CeInv, d: 64)
            for (i=0; i<npts; i++)
            {
                Cd[i] = CeInv[i]*d[i];
            }
        }
        else
        {
            M = npts;
            N = npts;
            cblas_sgemv(CblasRowMajor, CblasNoTrans, M, N, 1.0f, CeInv, ldc,
                        d, 1, 0.0f, Cd, 1);
        }
    }
    else
    {
        // Set C d but now C is assume to be the identity matrix
        cblas_scopy(npts, d, 1, Cd, 1);
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Computes Cd vector
 *
 * @param[in] npts   number of points
 * @param[in] ldc    leading dimension of CeInv.  if ldiag is true then
 *                   this should be 1 otherwise, this should be >= npts.
 * @param[in] CeInv  inverse of data error (covariance) matrix.  if
 *                   ldiag is true then this is diagonal of length [nobs].
 *                   if false then it is a symmetric [nobs x ldc] matrix
 *                   in row major format where ldc >= nobs.
 * @param[in] d      data vector [npts]
 *
 * @param[out] Cd    multiplication of C*d [npts]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeCd64f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const double *__restrict__ CeInv,
                        const double *__restrict__ d,
                        double *__restrict__ Cd)
{
    int i, M, N;
    // Compute C d
    if (CeInv != NULL)
    {
        if (ldiag)
        {
            #pragma omp simd aligned(Cd, CeInv, d: 64)
            for (i=0; i<npts; i++)
            {
                Cd[i] = CeInv[i]*d[i];
            }
        }
        else
        {
            M = npts;
            N = npts;
            cblas_dgemv(CblasRowMajor, CblasNoTrans, M, N, 1.0, CeInv, ldc,
                         d, 1, 0.0, Cd, 1);
        }
    }
    else
    {
        // Set C d but now C is assume to be the identity matrix
        cblas_dcopy(npts, d, 1, Cd, 1);
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Computes C*G matrix
 *
 * @param[in] npts     number of points in Green's functions
 * @param[in] ldc      leading dimension of CeInv.  if ldiag is true then
 *                     this should be 1 otherwise, this should be >= npts.
 * @param[in] ldiag    if true then CeInv is diagonal.  if false then
 *                     CeInv is a general [npts x npts] matrix.
 * @param[in] CeInv    inverse of data error (covariance) matrix.  if
 *                     ldiag is true then this is diagonal of length [nobs].
 *                     if false then it is a symmetric [nobs x ldc] matrix
 *                     in row major format where ldc >= nobs.
 * @param[in] G        Green's functions matrix in row major order [npts x LDG]
 *
 * @param[out] CG      row major C*G matrix [npts x LDCG]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeCG32f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const float *__restrict__ CeInv,
                        const float *__restrict__ G,
                        float *__restrict__ CG)
{
    int i, j, jndx, K, kndx, M;
    const int N = 6; // Number of columns of G
    if (CeInv != NULL)
    {
        if (ldiag)
        {
            // Set C G
            for (i=0; i<npts; i++)
            {
                for (j=0; j<6; j++)
                {
                    jndx = LDG*i + j;
                    kndx = LDCG*i + j;
                    CG[kndx] = CeInv[i]*G[jndx];
                }
            }
        }
        else
        {
            M = npts; // rows of CeInv
            K = npts; // columsn of CeInv
            cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                        M, N, K, 1.0f, CeInv, ldc,
                        G, LDG, 0.0f, CG, LDCG);
        }
    }
    else
    {
        // Set C G but now C is assume to be the identity matrix
        for (i=0; i<npts; i++)
        {
            for (j=0; j<6; j++)
            {
                jndx = LDG*i + j;
                kndx = LDCG*i + j;
                CG[kndx] = G[jndx];
            }
        }
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Computes C*G matrix
 *
 * @param[in] npts     number of points in Green's functions
 * @param[in] ldc      leading dimension of CeInv.  if ldiag is true then
 *                     this should be 1 otherwise, this should be >= npts.
 * @param[in] ldiag    if true then CeInv is diagonal.  if false then
 *                     CeInv is a general [npts x npts] matrix.
 * @param[in] CeInv    inverse of data error (covariance) matrix.  if
 *                     ldiag is true then this is diagonal of length [nobs].
 *                     if false then it is a symmetric [nobs x ldc] matrix
 *                     in row major format where ldc >= nobs.
 * @param[in] G        Green's functions matrix in row major order [npts x LDG]
 *
 * @param[out] CG      row major C*G matrix [npts x LDCG]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeCG64f(const int npts,
                        const int ldc,
                        const bool ldiag,
                        const double *__restrict__ CeInv,
                        const double *__restrict__ G,
                        double *__restrict__ CG)
{
    int i, j, jndx, K, kndx, M;
    const int N = 6; // Number of columns of G
    if (CeInv != NULL)
    {
        if (ldiag)
        {
            // Set C G
            for (i=0; i<npts; i++)
            {
                for (j=0; j<6; j++)
                {
                    jndx = LDG*i + j;
                    kndx = LDCG*i + j;
                    CG[kndx] = CeInv[i]*G[jndx];
                }
            }
        }
        else
        {
            M = npts; // rows of CeInv
            K = npts; // columns of CeInv
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                        M, N, K, 1.0, CeInv, ldc,
                        G, LDG, 0.0, CG, LDCG);
        }
    }
    else
    {
        // Set C G but now C is assume to be the identity matrix
        for (i=0; i<npts; i++)
        {
            for (j=0; j<6; j++)
            {
                jndx = LDG*i + j;
                kndx = LDCG*i + j;
                CG[kndx] = G[jndx];
            }
        }
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Computes G^T C G matrix
 *
 * @param[in] npts    number of points in Green's functions
 * @param[in] G       row major Green's functions matrix [npts x LDG]
 * @param[in] CG      regularized Green's function matrix [npts x LDCG]
 *
 * @param[out] GtCG   symmetric row major matrix containing G^T C G [6 x 6]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeGtCG32f(const int npts,
                          const float *__restrict__ G,
                          const float *__restrict__ CG,
                          float *__restrict__ GtCG)
{
    const int M = 6; // rows of trans(G)
    const int N = 6; // columns of CG
    int K = npts;    // columns of trans(G)
    cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                M, N, K, 1.0, G, LDG,
                CG, LDCG, 0.0, GtCG, 6);
    return 0;
}
//============================================================================//
/*!
 * @brief Computes G^T C G matrix
 *
 * @param[in] npts    number of points in Green's functions
 * @param[in] G       row major Green's functions matrix [npts x LDG]
 * @param[in] CG      regularized Green's function matrix [npts x LDCG]
 *
 * @param[out] GtCG   symmetric row major matrix containing G^T C G [6 x 6]
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
static int computeGtCG64f(const int npts,
                          const double *__restrict__ G,
                          const double *__restrict__ CG,
                          double *__restrict__ GtCG)
{
    const int M = 6; // rows of trans(G)
    const int N = 6; // columns of CG
    int K = npts;    // columns of trans(G)
    cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                M, N, K, 1.0, G, LDG,
                CG, LDCG, 0.0, GtCG, 6);
    return 0;
}
//============================================================================//
/*!
 * @brief A utility function for computing:
 *        \f$
 *          \log \left ( \sum_i^n exp(-x_i) \right )
 *        \f$.
 *
 * For an explanation of why I do this whacky arithmetic see:
 * http://jblevins.org/log/log-sum-exp
 *
 * @param[in] n     number of points in array x
 * @param[in] x     objective function values at n points.  notice that these
 *                  are all non-negative.
 *
 * @result log of the sum of the exponential terms
 *
 * @author Ben Baker
 *
 */
double mtsearch_logSumExp64f(const int n, const double *__restrict__ x)
{
    double arg, sum, hatx, lse;
    int i, iopt;
    // term dominant player in the integral is the `best' or smallest term
    iopt = cblas_idamin(n, x, 1);
    hatx = x[iopt];
    // have fun with exponentials and find the constant 'c' s.t.:
    //   log((exp(-x_1 + \hat{x}) + exp(-x_2 + \hat{x}) + \cdots )exp(-\hat{x}))
    // = log(exp(-x_1 + \hat{x} + exp(exp(-x_2 + \hat{x})) + \cdots) - \hat{x}
    sum = 0.0;
    #pragma omp simd reduction(+:sum)
    for (i=0; i<n; i++)
    {
        arg = hatx - x[i];
        sum = sum + exp(arg);
    }
    lse = log(sum) - hatx;
    return lse;
}

static int computePadding64f(const int n)
{
    size_t mod, pad;
    int ipad;
    // Set space and make G matrix
    pad = 0;
    mod = ((size_t) n*sizeof(double))%64;
    if (mod != 0)
    {
        pad = (64 - mod)/sizeof(double);
    }
    ipad = (int) pad;
    return ipad;
}

static int computePadding32f(const int n)
{
    size_t mod, pad;
    int ipad;
    // Set space and make G matrix
    pad = 0;
    mod = ((size_t) n*sizeof(float))%64;
    if (mod != 0)
    {
        pad = (64 - mod)/sizeof(float);
    }
    ipad = (int) pad;
    return ipad;
}


/*
** ECM - eigenvector centrality mapping
**
** G.Lohmann, MPI-KYB, updated Oct 2018
*/

#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

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

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define SQR(x) ((x) * (x))
#define ReLU(x) ((x) > 0 ? (x) : 0)

extern void   VMatrixECM(gsl_matrix_float *,float *,int,int,VImage);
extern void   VMatrixProjection(gsl_matrix_float *X,float *ev,int,int seed);
extern VImage VoxelMap(VAttrList list,VImage mask,size_t nvox);
extern size_t NumVoxels(VImage);
extern gsl_matrix_float *ReadDataECM(VAttrList,VImage,VImage,VShort,VShort,int,size_t);
extern VImage WriteOutput(VAttrList,VImage,float *,size_t);
extern int Irreducible(gsl_matrix_float *X,int type);

void NormVec(float *x,size_t nvox)
{
 size_t i;
 float sum=0;
 sum = 0;
 for (i=0; i<nvox; i++) sum += (x[i]*x[i]);
 sum = sqrt(sum);
 for (i=0; i<nvox; i++) x[i] /= sum;
}

/* needed for ECM-RLC, negative */
void SignFlip(gsl_matrix_float *X)
{
  float u=0;
  size_t nt = X->size2;
  size_t i,j,n=nt/2;
  for (i=0; i<X->size1; i++) {
    for (j=n; j<nt; j++) {
      u = gsl_matrix_float_get(X,i,j);
      gsl_matrix_float_set(X,i,j,-u);
    }
  }
}

/* fast, memory efficient power iteration, used in ECM-RLC, ECM-Add1 */
void PowerIteration(gsl_matrix_float *X,float *ev,int type,int maxiter)
{
  int iter;
  float vsum=0,d=0;
  size_t i;
  size_t nvox = X->size1;
  size_t nt = X->size2;
  float scale = 1.0/(float)nt;

  fprintf(stderr," power iteration...\n");
  gsl_vector_float *y = gsl_vector_float_calloc(nvox);
  gsl_vector_float *z = gsl_vector_float_calloc(nt);

  Irreducible(X,type);  /* check if corr matrix is irreducible */

  NormVec(ev,nvox);
  for (i=0; i<nvox; i++) y->data[i] = ev[i];

  /* power iterations */
  for (iter=0; iter<maxiter; iter++) {

    /* R = (X^t X)/n */
    gsl_blas_sgemv (CblasTrans,1.0,X,y,0.0,z);
    if (type == 7) SignFlip(X);
    gsl_blas_sgemv (CblasNoTrans,1.0,X,z,0.0,y);
    if (type == 7) SignFlip(X);
    gsl_vector_float_scale(y,scale);

    /* ECM-Add1,  (R+E)v = Rv + Ev */
    if (type == 1) {
      vsum = 0;
      for (i=0; i<nvox; i++) vsum += ev[i];
      for (i=0; i<nvox; i++) y->data[i] += vsum;
    }

    /* normalize */
    NormVec(y->data,nvox);

    /* check convergence */
    d = 0;
    for (i=0; i<nvox; i++) {
      d += SQR(ev[i] - y->data[i]);
      ev[i] = y->data[i];
    }
    fprintf(stderr," %5d   %.6f\n",(int)iter,d);

    if (iter > 2 && d < 1.0e-6) break;
  }
  gsl_vector_float_free(y);
  gsl_vector_float_free(z);
}

float *VECM(gsl_matrix_float *X,int type,VBoolean project,int seed,int maxiter,VImage map)
{
  size_t i,j;
  size_t nvox = X->size1;
  size_t nt = X->size2;
  if (type == 0 || type == 7) nt /= 2;

  /* ini eigenvector */
  float u=0;
  float *eigvec = (float *) VCalloc(nvox,sizeof(float));
  for (i=0; i<nvox; i++) eigvec[i] = 1.0/(float)nt;

  /* no matrix projection */
  if (!project) {

    switch (type) {

    case 0:   /*  ECM-RLC positive */
      for (i=0; i<X->size1; i++) {
	for (j=0; j<nt; j++) {
	  u = gsl_matrix_float_get(X,i,j);
	  gsl_matrix_float_set(X,i,j+nt,fabs(u));
	}
      }
      PowerIteration(X,eigvec,type,maxiter);
      break;

    case 1:   /* ECM-Add1 */
      PowerIteration(X,eigvec,type,maxiter);
      break;

    case 6:   /* no filter, risky */
      PowerIteration(X,eigvec,1,maxiter);  /* initialize using ecm-add1 */
      PowerIteration(X,eigvec,0,maxiter);
      break;

      /* ECM-RLC negative */
    case 7:
      for (i=0; i<X->size1; i++) {
	for (j=0; j<nt; j++) {
	  u = gsl_matrix_float_get(X,i,j);
	  gsl_matrix_float_set(X,i,j,fabs(u));
	  gsl_matrix_float_set(X,i,j+nt,u);
	}
      }
      PowerIteration(X,eigvec,type,maxiter);
      break;

    default:  /* dense matrix representation */
      VMatrixECM(X,eigvec,type,maxiter,map);
      break;

    }
  }

  /* Matrix projection, ECM-project */
  else {
    VMatrixProjection(X,eigvec,type,seed);
  }

  /* rescale for better visualization */
  float nx = (float)nvox;
  nx = sqrt(nx);
  for (i=0; i<nvox; i++) eigvec[i] *= nx;

  return eigvec;
}

VDictEntry TYPDict[] = {
  { "rlc",     0, 0,0,0,0 },
  { "add",     1, 0,0,0,0 },
  { "pos",     2, 0,0,0,0 },
  { "abs",     3, 0,0,0,0 },
  { "neg",     4, 0,0,0,0 },
  { "gauss",   5, 0,0,0,0 },
  { NULL, 0,0,0,0,0 }
};

int main (int argc,char *argv[])
{
  static VString  mask_filename = "";
  static VShort   first  = 0;
  static VShort   length = 0;
  static VShort   type   = 0;
  static VShort   maxiter= 20;
  static VBoolean project = FALSE;
  static VInteger  seed   = 99402622;
  static VShort   nproc  = 0;
  static VOptionDescRec  options[] = {
    {"mask",VStringRepn,1,(VPointer) &mask_filename,VRequiredOpt,NULL,"Region of interest mask"},
    {"first",VShortRepn,1,(VPointer) &first,VOptionalOpt,NULL,"First timestep to use"},
    {"length",VShortRepn,1,(VPointer) &length,VOptionalOpt,NULL,"Length of time series to use, '0' to use full length"},
    {"metric",VShortRepn,1,(VPointer) &type,VOptionalOpt,TYPDict,"Type of correlation metric"},
    {"iterations",VShortRepn,1,(VPointer) &maxiter,VOptionalOpt,NULL,"Maximum number of power iterations"},
    {"project",VBooleanRepn,1,(VPointer) &project,VOptionalOpt,NULL,"Whether to do matrix projection"},
    {"seed",VIntegerRepn,1,(VPointer) &seed,VOptionalOpt,NULL,"Seed for random number generation (only for ECM-project)"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"}
  };
  FILE *in_file=NULL,*out_file=NULL;
  VString in_filename=NULL;
  char *prg_name=GetLipsiaName("vecm");
  fprintf(stderr, "%s\n", prg_name);

  /* parse command line */
  VParseFilterCmdZ (VNumber (options),options,argc,argv,&in_file,&out_file,&in_filename);
  if (type < 0 || type > 8) VError(" unknown metric");
  if (type < 6) fprintf(stderr," Correlation metric %d:  %s\n",type,TYPDict[type].keyword);
  if (project && (type < 2 || type == 7))
    VError(" matrix projection not needed for metric %d (%s)\n",type,TYPDict[type].keyword);

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  if (type > 3 || project == TRUE) fprintf(stderr,"using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* read functional data */
  VAttrList list = VReadAttrListZ(in_file,in_filename,0L,TRUE,FALSE);
  if (list == NULL) VError(" error reading input file %s",in_file);
  VAttrList geolist = VGetGeoInfo(list);

  /* read mask */
  VAttrList listm = VReadAttrList(mask_filename,0L,TRUE,FALSE);
  VImage mask = VReadImage(listm);
  if (mask == NULL) VError(" no mask found");
  size_t nvox = NumVoxels(mask);

  /* voxel map */
  VImage map = VoxelMap(list,mask,nvox);

  /* read data into X */
  gsl_matrix_float *X = ReadDataECM(list,mask,map,first,length,type,nvox);

  /* main process */
  float *eigvec = VECM(X,(int)type,project,(VLong)seed,(int)maxiter,map);

  /* create output image */
  VImage dest = WriteOutput(list,map,eigvec,nvox);
  VSetAttr(VImageAttrList(dest),"name",NULL,VStringRepn,"ECM");
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,dest);

  /* update geoinfo, 4D to 3D */
  if (geolist != NULL) {
    double *D = VGetGeoDim(geolist,NULL);
    D[0] = 3;  /* 3D */
    D[4] = 1;  /* just one timestep */
    VSetGeoDim(geolist,D);
    VSetGeoInfo(geolist,out_list);
  }

  /* write to disk */
  VHistory(VNumber(options),options,prg_name,&list,&out_list);
  if (! VWriteFile (out_file, out_list)) exit (1);
  fprintf (stderr, "%s: done.\n", argv[0]);
  return 0;
}


/* These functions compute linear preprocessing for
the UE using LAPACKE and CBLAS modules of
LAPACK libraries.
MMSE and MMSE whitening filters are available.
Functions are using RowMajor storage of the
matrices, like in conventional C. Traditional
Fortran functions of LAPACK employ ColumnMajor
data storage. */

#include<stdio.h>
#include<math.h>
#include<complex.h>
#include <stdlib.h>
#include <cblas.h>
#include <string.h>
#include <linux/version.h>
#if RHEL_RELEASE_CODE >= 1796
#include <lapacke/lapacke_utils.h>
#include <lapacke/lapacke.h>
#else
#include <lapacke_utils.h>
#include <lapacke.h>
#endif
//#define DEBUG_PREPROC

void transpose (int N, float complex *A, float complex *Result)
{
  // COnputes C := alpha*op(A)*op(B) + beta*C,
  enum CBLAS_TRANSPOSE transa = CblasTrans;
  enum CBLAS_TRANSPOSE transb = CblasNoTrans;
  int rows_opA = N; // number of rows in op(A) and in C
  int col_opB = N; //number of columns of op(B) and in C
  int col_opA = N; //number of columns in op(A) and rows in op(B)
  int col_B; //number of columns in B
  float complex alpha = 1.0+I*0;
  int lda  = rows_opA;
  float complex beta = 0.0+I*0;
  int ldc = rows_opA;
  int i;
  float complex* B;

  int ldb = col_opB;

  if (transb == CblasNoTrans) {
    B = (float complex*)calloc(ldb*col_opB,sizeof(float complex));
    col_B= col_opB;
  }
  else {
    B = (float complex*)calloc(ldb*col_opA, sizeof(float complex));
    col_B = col_opA;
  }
  float complex* C = (float complex*)malloc(ldc*col_opB*sizeof(float complex));

  for (i=0; i<lda*col_B; i+=N+1)
    B[i]=1.0+I*0;

  cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, B, ldb, &beta, C, ldc);

  memcpy(Result, C, N*N*sizeof(float complex));

  free(B);
  free(C);
 }

void conjugate_transpose (int N, float complex *A, float complex *Result)
{
  // Computes C := alpha*op(A)*op(B) + beta*C,
  enum CBLAS_TRANSPOSE transa = CblasConjTrans;
  enum CBLAS_TRANSPOSE transb = CblasNoTrans;
  int rows_opA = N; // number of rows in op(A) and in C
  int col_opB = N; //number of columns of op(B) and in C
  int col_opA = N; //number of columns in op(A) and rows in op(B)
  int col_B; //number of columns in B
  float complex alpha = 1.0+I*0;
  int lda  = rows_opA;
  float complex beta = 0.0+I*0;
  int ldc = rows_opA;
  int i;
  float complex* B;
  int ldb = col_opB;

  if (transb == CblasNoTrans) {
    B = (float complex*)calloc(ldb*col_opB,sizeof(float complex));
    col_B= col_opB;
  }
  else {
    B = (float complex*)calloc(ldb*col_opA, sizeof(float complex));
    col_B = col_opA;
  }
  float complex* C = (float complex*)malloc(ldc*col_opB*sizeof(float complex));

  for (i=0; i<lda*col_B; i+=N+1)
    B[i]=1.0+I*0;

  cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, B, ldb, &beta, C, ldc);

  memcpy(Result, C, N*N*sizeof(float complex));

  free(B);
  free(C);
 }

void H_hermH_plus_sigma2I (int N, int M, float complex *A, float sigma2, float complex *Result)
{
  //C := alpha*op(A)*op(B) + beta*C,
  enum CBLAS_TRANSPOSE transa = CblasConjTrans;
  enum CBLAS_TRANSPOSE transb = CblasNoTrans;
  int rows_opA = N; // number of rows in op(A) and in C
  int col_opB = N; //number of columns of op(B) and in C
  int col_opA = N; //number of columns in op(A) and rows in op(B)
  int col_C = N; //number of columns in B
  float complex alpha = 1.0+I*0;
  int lda  = col_opA;
  float complex beta = 1.0 + I*0;
  int ldc = col_opA;
  int i;

  float complex* C = (float complex*)calloc(ldc*col_opB, sizeof(float complex));

  for (i=0; i<lda*col_C; i+=N+1)
    C[i]=sigma2*(1.0+I*0);

  cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, A, lda, &beta, C, ldc);

  memcpy(Result, C, N*M*sizeof(float complex));
  free(C);

 }

 void HH_herm_plus_sigma2I (int M, int N, float complex *A, float sigma2, float complex *Result)
{
  //C := alpha*op(A)*op(B) + beta*C,
  enum CBLAS_TRANSPOSE transa = CblasNoTrans;
  enum CBLAS_TRANSPOSE transb = CblasConjTrans;
  int k = N; //number of columns in op(A) and rows in op(B),k
  float complex alpha = 1.0+I*0;
  int lda  = N;
  int ldb  = N;
  int ldc = M;
  int i;

  float complex* C = (float complex*)calloc(M*M, sizeof(float complex));

  for (i=0; i<M*M; i+=M+1)
    C[i]=1.0+I*0;

  cblas_cgemm(CblasRowMajor, transa, transb, M, M, k, &alpha, A, lda, A, ldb, &sigma2, C, ldc);

  memcpy(Result, C, M*M*sizeof(float complex));
  free(C);

}

void eigen_vectors_values (int N, float complex *A, float complex *Vectors, float *Values_Matrix)
{
  // This function computes ORTHONORMAL eigenvectors and eigenvalues of matrix A,
  // where Values_Matrix is a diagonal matrix of eigenvalues.
  // A=Vectors*Values_Matrix*Vectors'
  char jobz = 'V';
  char uplo = 'U';
  int order_A = N;
  int lda = N;
  int i;
  float* Values = (float*)malloc(sizeof(float)*1*N);

  LAPACKE_cheev(LAPACK_ROW_MAJOR, jobz, uplo, order_A, A, lda, Values);

  memcpy(Vectors, A, N*N*sizeof(float complex));

  for (i=0; i<lda; i+=1)
    Values_Matrix[i*(lda+1)]=Values[i];

  free(Values);
}

 void lin_eq_solver (int N, float complex* A, float complex* B, float complex* Result)
{
  int n = N;
  int lda = N;
  int ldb = N;
  int nrhs = N;

  char transa = 'N';
  int* IPIV = malloc(N*N*sizeof(int));

  // Compute LU-factorization
  LAPACKE_cgetrf(LAPACK_ROW_MAJOR, n, nrhs, A, lda, IPIV);

  // Solve AX=B
  LAPACKE_cgetrs(LAPACK_ROW_MAJOR, transa, n, nrhs, A, lda, IPIV, B, ldb);

  // cgetrs( "N", N, 4, A, lda, IPIV, B, ldb, INFO )

  memcpy(Result, B, N*N*sizeof(float complex));

  free(IPIV);

}

void mutl_matrix_matrix_row_based(float complex* M0, float complex* M1, int rows_M0, int col_M0, int rows_M1, int col_M1, float complex* Result ){
  enum CBLAS_TRANSPOSE transa = CblasNoTrans;
  enum CBLAS_TRANSPOSE transb = CblasNoTrans;
  int rows_opA = rows_M0; // number of rows in op(A) and in C
  int col_opB = col_M1; //number of columns of op(B) and in C
  int col_opA = col_M0; //number of columns in op(A) and rows in op(B)
  float complex alpha =1.0;
  int lda  = col_M0;
  float complex beta = 0.0;
  int ldc = col_M1;
  int ldb = col_M1;

#ifdef DEBUG_PREPROC
  int i=0;
  printf("rows_M0 %d, col_M0 %d, rows_M1 %d, col_M1 %d\n", rows_M0, col_M0, rows_M1, col_M1);

  for(i=0; i<rows_M0*col_M0; ++i)
    printf(" rows_opA = %d, col_opB = %d, W_MMSE[%d] = (%f + i%f)\n", rows_opA, col_opB,  i , creal(M0[i]), cimag(M0[i]));

  for(i=0; i<rows_M1*col_M1; ++i)
    printf(" M1[%d] = (%f + i%f)\n", i , creal(M1[i]), cimag(M1[i]));
#endif

  cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, M0, lda, M1, ldb, &beta, Result, ldc);

#ifdef DEBUG_PREPROC
  for(i=0; i<rows_opA*col_opB; ++i)
    printf(" result[%d] = (%f + i%f)\n", i , creal(Result[i]), cimag(Result[i]));
#endif

}
void mutl_matrix_matrix_col_based(float complex* M0, float complex* M1, int rows_M0, int col_M0, int rows_M1, int col_M1, float complex* Result ){
  enum CBLAS_TRANSPOSE transa = CblasNoTrans;
  enum CBLAS_TRANSPOSE transb = CblasNoTrans;
  int rows_opA = rows_M0; // number of rows in op(A) and in C
  int col_opB = col_M1; //number of columns of op(B) and in C
  int col_opA = col_M0; //number of columns in op(A) and rows in op(B)
  float complex alpha =1.0;
  int lda  = col_M0;
  float complex beta = 0.0;
  int ldc = rows_M1;
  int ldb = rows_M1;

#ifdef DEBUG_PREPROC
  int i = 0;
  printf("rows_M0 %d, col_M0 %d, rows_M1 %d, col_M1 %d\n", rows_M0, col_M0, rows_M1, col_M1);

  for(i=0; i<rows_M0*col_M0; ++i)
    printf(" rows_opA = %d, col_opB = %d, W_MMSE[%d] = (%f + i%f)\n", rows_opA, col_opB,  i , creal(M0[i]), cimag(M0[i]));

  for(i=0; i<rows_M1*col_M1; ++i)
    printf(" M1[%d] = (%f + i%f)\n", i , creal(M1[i]), cimag(M1[i]));
#endif

  cblas_cgemm(CblasColMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, M0, lda, M1, ldb, &beta, Result, ldc);

#ifdef DEBUG_PREPROC
  for(i=0; i<rows_opA*col_opB; ++i)
    printf(" result[%d] = (%f + i%f)\n", i , creal(Result[i]), cimag(Result[i]));
#endif
}

/*FILTERS */
void compute_MMSE(float complex* H, int order_H, float sigma2, float complex* W_MMSE)
{
  int N = order_H;
  float complex* H_hermH_sigmaI = malloc(N*N*sizeof(float complex));
  float complex* H_herm =  malloc(N*N*sizeof(float complex));

  H_hermH_plus_sigma2I(N, N, H, sigma2, H_hermH_sigmaI);

#ifdef DEBUG_PREPROC
  int i =0;
  for(i=0;i<N*N;i++)
    printf(" H_hermH_sigmaI[%d] = (%f + i%f)\n", i , creal(H_hermH_sigmaI[i]), cimag(H_hermH_sigmaI[i]));
#endif

  conjugate_transpose (N, H, H_herm); //equals H_herm

#ifdef DEBUG_PREPROC
  for(i=0;i<N*N;i++)
    printf(" H_herm[%d] = (%f + i%f)\n", i , creal(H_herm[i]), cimag(H_herm[i]));
#endif

  lin_eq_solver(N, H_hermH_sigmaI, H_herm, W_MMSE);

#ifdef DEBUG_PREPROC
  for(i=0;i<N*N;i++)
    printf(" W_MMSE[%d] = (%f + i%f)\n", i , creal(W_MMSE[i]), cimag(W_MMSE[i]));
#endif

  free(H_hermH_sigmaI);
  free(H_herm);
}

#if 0
void compute_white_filter(float complex* H_re,
                          int order_H,
                          float sigma2,
                          float complex* W_Wh_0_re,
                          float complex* W_Wh_1_re){

  int aatx, aarx, re;
  int i,j;
  int M =n_rx;
  int N = n_tx;
  int sigma2=noise_power;

  float complex *H0_re = malloc(n_rx*(n_tx>>2)*sizeof(float complex));
  float complex *H1_re = malloc(n_rx*(n_tx>>2)*sizeof(float complex));
  float complex *R_corr_col_n_0_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *R_corr_col_n_1_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *U_0_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *U_1_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *U_0_herm_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *U_1_herm_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *D_0_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *D_1_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *W_Wh_0_re = malloc(n_rx*n_tx*sizeof(float complex));
  float complex *W_Wh_1_re = malloc(n_rx*n_tx*sizeof(float complex));

  for (aatx=0; aatx<n_tx/2; aatx++){
    for (aarx=0; aarx<n_rx; aarx++) {
      H0_re[aatx*n_rx + aarx] = H_re[aatx*n_rx + aarx][re]; // H0 gets [0 1 2 3; 4,5,6,7].' coefficients of H
      H1_re[aatx*n_rx + aarx] = H_re[aatx*n_rx + aarx + 8][re]; // H1 gets [8 9 10 11; 12, 13, 14, 15].' coefficients of H
        if (re == 0)
        printf("ant %d, H_re = (%f + i%f) \n", aatx*n_rx + aarx, creal(H[aatx*n_rx + aarx][re]), cimag(H[aatx*n_rx + aarx][re]));
      }
   }

    //HH_herm_plus_sigma2I(n_rx, (n_tx>>2), H1_re, sigma2, R_corr_col_n_0_re);
    HH_herm_plus_sigma2I(n_rx, (n_tx>>2), H0_re, sigma2, R_corr_col_n_1_re);

    eigen_vectors_values(n_rx, R_corr_col_n_0_re, U_0_re, D_0_re);
    eigen_vectors_values(n_rx, R_corr_col_n_1_re, U_1_re, D_1_re);

    transpose (n_rx, U_0_re, U_0_herm_re);
    transpose (n_rx, U_1_re, U_1_herm_re);

    sigma = (float)(sqrt((double)(sigma2)));

    /*The inverse of a diagonal matrix is obtained by replacing each element in the diagonal with its reciprocal.
    A square root of a diagonal matrix is given by the diagonal matrix, whose diagonal entries are just the square
     roots of the original matrix.*/

    D_0_re_inv_sqrt[0] = sqrt_float(1/D_0_re_inv[0]);
    D_0_re_inv_sqrt[5] = sqrt_float(1/D_0_re_inv[5]);
    D_0_re_inv_sqrt[10] = sqrt_float(1/D_0_re_inv[10]);
    D_0_re_inv_sqrt[15] = sqrt_float(1/D_0_re_inv[15]);

    D_1_re_inv[0] = sqrt_float(1/D_1_re_inv[0]);
    D_1_re_inv[5] = sqrt_float(1/D_1_re_inv[5]);
    D_1_re_inv[10] = sqrt_float(1/D_1_re_inv[10]);
    D_1_re_inv[15] = sqrt_float(1/D_1_re_inv[15]);

    now only to multiply

  free(H0);
  free(H1);
  free(R_corr_col_n_0);
  free(R_corr_col_n_1);
}
#endif

float sqrt_float(float x, float sqrt_x)
{
  sqrt_x = (float)(sqrt((double)(x)));
  return sqrt_x;
}