/************************************************************/
/*********  Interval Censoring data analysis   ***********/
/*********  by Hao Liu on 6/7/07                  **********/
/************************************************************/

/*** accepted data file: first p colmuns are covariates, last 2 columns are observed time intervals, 9999=censored ***/

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_vector.h>

#define MAX_ITER 10000
#define TOL 1.0e-6
#define sim_num 300

/** sim_num is the bootstrap replicates **/

#define machine_p 1.0e-15
#define myPHO 0.5
#define myPI 0.01
#define Sigma 20.0

struct fun_params {
    double *dij;
    double *zij;
    double *pj;
    int nn;
    int np;
    int mk;
};

double fdf_eplike(int, const double *, double *, void *);
void myxij(double *, const double *, const double *, void *);
void deveplike(double *, double *, const double *, double *, double *, void *);

double maxp(double *, double *, double, const double *, double *, void *);
double normf(double *, double *, double, double, const double *, double *, void *);

extern int conmin(int, double *, double *, double *, int *, int *, double, int, double *, int, double, int,
                  double (*)(int, const double *, double *, void *), void *);

int main(int argc, char *argv[])
{
    FILE *fpi, *fpo1, *fpo2;
    if(argc<=4) {
        printf("need: 1) data file name, 2) sample size n, 3) number of covariate p, and 4) the output file1 5) files2...\n");
        return 0;
    }
    fpi=fopen(argv[1], "r");
    fpo1=fopen(argv[4], "w");
    fpo2=fopen(argv[5], "w");

    int nn=0, np=0, i, j;
    char *cs=malloc(1000*sizeof(char));

    sscanf(argv[2], "%i", &nn);
    sscanf(argv[3], "%i", &np);

    if (np>nn) {
        printf("sample size should be larger than the number of covariates.");
        return 0;
    }

    double *li= malloc(nn*sizeof(double)), *lia= malloc(nn*sizeof(double)), *li0=malloc(nn*sizeof(double));
    double *ri= malloc(nn*sizeof(double)), *ria= malloc(nn*sizeof(double)), *ri0=malloc(nn*sizeof(double));
    double *zij= malloc(nn*np*sizeof(double)), *zij0= malloc(nn*np*sizeof(double)), *dij=malloc(nn*nn*sizeof(double));
    double *q=malloc(nn*sizeof(double)), *p=malloc(nn*sizeof(double)), *gam= malloc(nn*nn*sizeof(double));

    double *pj=calloc(nn, sizeof(double)), *mypj=calloc(nn, sizeof(double));

    double alpha0=1.0, theta0=1.0, nvold;
    register double *tau= calloc(nn, sizeof(double));
    double past_time, now_time, mysize, tmp00;

    double curt, wt, u, likval1, likval2, likval;

	int j1, k, mk, ss, iter, status, iter2, cure;
    int *bsi=malloc(nn*sizeof(int)), *bsi2=malloc(nn*sizeof(int));

    double *ww=calloc(2*(np+1)*(np+8), sizeof(double)),
        *xx=calloc(2*(np+1), sizeof(double)), *gg=calloc(2*(np+1), sizeof(double)), fval;
    double acc=1e-11, eps=1e-5;;
    int nflag=-1, ifun=-1, ifunter=-1;

    struct fun_params *mydata=malloc(sizeof(struct fun_params));

    const gsl_rng_type *Ty;
    gsl_rng *rn;

    gsl_rng_env_setup();
    Ty = gsl_rng_default;
    rn = gsl_rng_alloc (Ty);
    gsl_rng_set(rn, 100);

    /** read the data, first line is the hearder and thus removed ***/
    fgets(cs, 1000, fpi);

    for (i = 0; i<nn; i++) {
        for (j=0;j<np;j++) fscanf(fpi, "%lf,", zij0+np*i+j);
        fscanf(fpi, "%lf,%lf", li0+i, ri0+i);
        bsi[i]=i;
    }
    fclose(fpi);
    mydata->nn = nn;
    mydata->np = np;

    /** sim_num is the bootstrap replicates **/

    for (ss=0;ss<sim_num;ss++){

        /** select the bootstrap sample ***/
        /*** random sample with replacement **/

        gsl_ran_sample (rn, bsi2, nn, bsi, nn, sizeof(int));
        for(i=0;i<nn;i++) {
            for (j=0;j<np;j++) zij[np*i+j] = zij0[np*bsi2[i]+j];
            lia[i]=li0[bsi2[i]];
            li[i]=li0[bsi2[i]];
            ria[i]=ri0[bsi2[i]];
            ri[i]=ri0[bsi2[i]];
        }

        mydata->zij = zij;
        gsl_sort(ria,1,nn);
        gsl_sort(lia,1,nn);

        /*** generate p, q data ***/
        p[0]=ria[0];
        i=j=k=0;
        do {
            while (lia[i]<=p[k] && i<nn) i++;
            q[k]=lia[i-1];
            k++;
            while (ria[j]<lia[i] && j<nn) j++;
            if (j<nn) p[k]=ria[j];
            else p[k]=ria[j-1];
        } while (i<nn && j<nn);

        mk=k-1;
        mydata->mk=mk;
        for (i=0; i<nn; i++)
            for (j = 0; j <= mk; j++)
                dij[i*(mk+1)+j]=(li[i]<=q[j] && p[j]<=ri[i]) ? 1.0: 0.0;
        mydata->dij= dij;

        /** Initial values **/

        for (j=0; j<mk; j++) pj[j] = 1.0/mk;
        (mydata->pj)=pj;

        for (j=0; j<=np; j++) xx[j] = 0.0;

        nflag=2;
        fval=9999.0;
        iter2=0;
        do {
            iter2++;
            likval2=fval;

            for (j = 0; j < mk; j++) tau[j]=1.0/(mydata->pj)[j];
            nvold = 0.1;

            /*** calculate expected Xij ***/
            myxij(gam, xx, pj, mydata);

            /*** Primal-dual interior-point method ***/

            nvold = maxp(pj, tau, nvold, xx, gam, mydata);

            /** minimize to get theta **/

            if (nflag!=0) for (j=0; j<=np; j++) xx[j] = 0.1;
            nflag= conmin(np+1, xx, &fval, gg, &ifun, &ifunter, 0.001, 300, ww, 400, 1e-18, 1, fdf_eplike, mydata);

            likval1=fval;

        } while(fabs(likval1-likval2)>0.0001 & iter2 < MAX_ITER);

        now_time = (double) clock ();

        /*** outputs ****/

        for(j=0; j<=np; j++) fprintf(fpo1, "%.10f, ", xx[j]);
        fprintf(fpo1, "%.10f, %.2f \n", fval, now_time/(double) CLOCKS_PER_SEC);
        for(j=0; j<mk; j++) fprintf(fpo2, "%.10f,", (mydata->pj)[j]);
        fprintf(fpo2, "%i\n", mk);
    }

    fclose(fpo1);
    fclose(fpo2);

    free(li);
    free(ri);
    free(zij);

    free(li0);
    free(ri0);
    free(zij0);

    free(lia);
    free(ria);
    free(p);
    free(q);
    free(mypj);

    free(gam);
    free(tau);

    free(xx);
    free(ww);
    free(gg);

    free(mydata);

    return 0;
}

/*** the expected likelihood, 0=alpha, 1=theta and the derivative ***/

double fdf_eplike(int np1, const double *theta, double *df, void * par)
{
    struct fun_params * param = (struct fun_params *) par;
    int i,j,j1, m=(param->mk), nn=(param->nn), tmpi;

    double alp0, sz, likval, msumval, tmpz, tmpz1, tmpa, tmpc, tmpd, tmp0, dfz1, dfz2;
    double *xij, *cij, *pps, pp;

    xij=calloc(m+1, sizeof(double));
    pps=calloc(m+1, sizeof(double));
    cij=calloc(m+1, sizeof(double));

    alp0=theta[0];

    for(j=0;j<m;j++) {
        pps[j]=0.0;
        for(j1=0;j1<j;j1++) pps[j] += ((param->pj)[j1]) ;
    }

    for(j=0;j<np1;j++) df[j]=0.0;

    likval=0.0;
    for(i=0;i<nn;i++) {
        tmpz1=0.0;
        for(j=1;j<np1;j++) tmpz1 += theta[j] * (param->zij)[i*(np1-1)+j-1];
        tmpz=exp(alp0 + tmpz1);

        tmpc=0.0;
        for(j=0;j<m;j++) {
            pp = (param->pj)[j];
            xij[j] = ((param->dij)[i*(m+1)+j]) * exp( -tmpz* pps[j]) *(1.0- exp(-pp*tmpz));
            tmpc += xij[j];
        }
        xij[m]= (param -> dij)[i*(m+1)+m] * exp(-tmpz);
        tmpc += xij[m];

        if (tmpc == 0.0) {
            for(j=0;j<np1;j++) df[j]=-9999999.0;
            return 9999999.0;
        }

        for(j=0;j<=m;j++) {
            cij[j]=0.0;
            for(j1=j+1; j1<m; j1++) cij[j] += xij[j1];
            xij[j] /=tmpc;
        }

        msumval=0.0;
        for(j=0;j<m;j++) {
            cij[j] /=tmpc;
            pp= (param -> pj)[j];
            likval += ( xij[j]* log(1.0- exp(- pp* tmpz )) - cij[j] * pp * tmpz);
            msumval += (( xij[j] *exp(-pp* tmpz) /(1.0- exp(-pp* tmpz)) - cij[j] )*pp);
        }
        df[0] += ((msumval-xij[m])*tmpz);

        for(j=1;j<np1;j++) df[j] += ((msumval-xij[m])*(param->zij)[i*(np1-1)+j-1]*tmpz);
        likval -= xij[m]*tmpz;
    }

    for(j=0;j<np1;j++) df[j] = - df[j]/((double) nn);

    free(cij);
    free(xij);
    free(pps);

    return -likval/((double) nn);
}

/*** Calculate the updated Xij ***/

void myxij(double *lamij, const double *the, const double *mypj, void * parm)
{
    struct fun_params * para = (struct fun_params *) parm;

    double alp0, tmp0, tmpc, pp, tmpz, tmpz1;
    int i,j,j1, tmpi, m=(para->mk), nn=(para->nn), np=(para->np);
    double *gj=calloc(m+1, sizeof(double)), *xij=calloc(m+1, sizeof(double));
    double *dij=(para->dij), *zij=(para->zij);

    alp0=the[0];

    for(j=0;j<m;j++) {
        gj[j]=0.0;
        for(j1=0;j1<j;j1++) gj[j] += mypj[j1];
    }
    for(i=0;i<nn;i++) {
        tmpz1=0.0;
        for(j=0;j<np;j++) tmpz1 += the[j+1] * zij[j+i*np];
        tmpz=exp(alp0+ tmpz1);

        tmpc=0.0;
        for(j=0;j<m;j++) {
            pp = mypj[j];
            xij[j] = dij[i*(m+1)+j] * exp( -tmpz* gj[j]) *(1.0- exp(-pp*tmpz));
            tmpc += xij[j];
        }
        xij[m]= dij[i*(m+1)+m] * exp(-tmpz);
        tmpc += xij[m];

        for(j=0;j<m;j++)  lamij[i*m+j] = xij[j] / tmpc;
    }

    free(xij);
    free(gj);
}

/*** Calculate the first derivative and second derivative wrt p of the expected liklihood ***/
void deveplike(double *df, double *ddf, const double *the, double *gam, double * myp, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;
    double alp0, tmp1, tmpz1, tmp2, tmpa, pp;
    int i,j,k,j1, m=(para->mk), nn=(para->nn), np=(para->np);
    double *zij=(para->zij);

    alp0=the[0];
    for(j=0;j<m;j++) {

        df[j] = ddf[j] = 0.0;
        pp = myp[j];

        for(i=0;i<nn;i++) {

            tmpz1=0.0;
            for(k=0;k<np;k++) tmpz1 += the[k+1] * zij[k+i*np];
            tmp1=exp(alp0+ tmpz1);
            tmp2=exp(tmp1 * pp);

            tmpa =0.0;
            for(j1=j+1; j1<m; j1++) tmpa += gam[i*m+j1];
            df[j] += tmp1*( gam[i*m+j] /( tmp2- 1.0) - tmpa);
            ddf[j] -= (gam[i*m+j]*tmp1*tmp1*tmp2 / (tmp2 - 1.0) / (tmp2 - 1.0));
        }
    }
}

/*** NormF ***/

double normf(double *p, double *tau, double nv, double eta, const double *the, double *gam, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;
	double *fj, *ffj, sum1, tmp1, tmp2;
	int j, m=(para->mk);

    fj=calloc(m, sizeof(double));
    ffj=calloc(m, sizeof(double));

    deveplike(fj, ffj, the, gam, p, parm);

    sum1=0.0;
    for(j=0;j<m;j++) {
        tmp1 = (fj[j] + tau[j] - nv);
        tmp2 = p[j] * tau[j] - 1.0/eta;
        sum1 += (tmp1*tmp1 + tmp2*tmp2);
    }

    free(fj);
    free(ffj);
    return sqrt(sum1);
}

/**** Maximize for p ****/

double maxp(double *pj, double *tau, double nvold, const double *the, double *gam, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;

	int j, iter, m=(para->mk), nn=(para->nn), np=(para->np);
    double eta, nv, curnv, steps, sum1, sum2, tmp1, tmp2;
    double *fj, *ffj, *delp, *curp, *taunew, *curtau;

    fj = calloc(m, sizeof(double));
    ffj = calloc(m, sizeof(double));
    delp = calloc(m, sizeof(double));
    curp= calloc(m, sizeof(double));
    taunew = calloc(m, sizeof(double));
    curtau = calloc(m, sizeof(double));

    iter=0;
    do {
        iter++;

        // calculate the df=fj and diag(ddf)=ffj

        for(j=0;j<m;j++) eta += pj[j]*tau[j];
        eta = m*Sigma/eta;
        deveplike(fj, ffj, the, gam, pj, parm);

        sum1 = sum2 = 0.0;
        for(j=0;j<m;j++) {
            sum1 += ((1.0/eta + pj[j] * fj[j]) / (tau[j] - pj[j] * ffj[j]));
            sum2 += (pj[j] / (tau[j] - pj[j] * ffj[j]));
        }
        nv=sum1/sum2;

        // backtracking: find the largest positive step = steps

        steps=1.0;
        for(j=0;j<m;j++) {
            delp[j] =  (pj[j] * (fj[j]-nv) + 1.0/eta) / (tau[j] - pj[j] * ffj[j]);
            taunew[j] =   nv - ffj[j] * delp[j] - fj[j];
            tmp1 = taunew[j] - tau[j];
            if (tmp1 <0) steps = fmin(steps, -tau[j]/tmp1);
        }
        steps *= 0.99;

        // backtracking: to keep the minimal p > 0;
        tmp2 = 1.0;
        for(j=0;j<m;j++) tmp2=fmin(tmp2,  pj[j] + steps * delp[j]);
        while(tmp2 <= 0) {
            steps *= myPHO;
            tmp2 = 1.0;
            for(j=0;j<m;j++) tmp2=fmin(tmp2, pj[j] + steps * delp[j]);
        }

        // backtracking:

        for(j=0;j<m;j++) {
            curp[j] = pj[j] + steps * delp[j];
            curtau[j] = tau[j] + steps * (taunew[j]-tau[j]);
        }
        curnv=nvold + steps * (nv-nvold);
        tmp2 = normf(curp, curtau, curnv, eta, the, gam, parm);
        tmp1 = normf(pj, tau, nvold, eta, the, gam, parm);

        while(tmp2 > (1.0-myPI*steps)*tmp1) {
            steps *= myPHO;
            for(j=0;j<m;j++) {
                curp[j] = pj[j] + steps * delp[j];
                curtau[j] = tau[j] + steps * (taunew[j]-tau[j]);
            }
            curnv=nvold + steps * (nv-nvold);
            tmp2=normf(curp, curtau, curnv, eta, the, gam, parm);
        }

        deveplike(fj, ffj, the, gam, curp, parm);

        sum1=sum2=0.0;
        for(j=0;j<m;j++) {
            tmp1 = (fj[j] + curtau[j] - curnv);
            sum1 += (tmp1*tmp1);
            sum2 += curp[j] * curtau[j];
            pj[j] = curp[j];
            tau[j] = curtau[j];
        }

        nvold=curnv;
//         printf("iter=%i: sum1=%.20f sum2=%f \n", iter, sqrt(sum1), sum2/m);

    } while ( (sqrt(sum1)> TOL)& iter < MAX_ITER*10 );

    free(fj);
    free(ffj);
    free(delp);
    free(curtau);
    free(taunew);
    free(curp);

    return nvold;
}


/*
 * Copyright 2021 The DAPHNE Consortium
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <runtime/local/context/DaphneContext.h>
#include <runtime/local/datastructures/DataObjectFactory.h>
#include <runtime/local/datastructures/DenseMatrix.h>

#include <cblas.h>

#include <cassert>
#include <cstddef>

// ****************************************************************************
// Struct for partial template specialization
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
struct MatMul {
    static void apply(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) = delete;
};

// ****************************************************************************
// Convenience function
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
void matMul(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) {
    MatMul<DTRes, DTLhs, DTRhs>::apply(res, lhs, rhs, ctx);
}

// ****************************************************************************
// (Partial) template specializations for different data/value types
// ****************************************************************************

// ----------------------------------------------------------------------------
// DenseMatrix <- DenseMatrix, DenseMatrix
// ----------------------------------------------------------------------------

template<>
struct MatMul<DenseMatrix<float>, DenseMatrix<float>, DenseMatrix<float>> {
    static void apply(DenseMatrix<float> *& res, const DenseMatrix<float> * lhs, const DenseMatrix<float> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nc1 == static_cast<int>(rhs->getNumRows())) && "#cols of lhs and #rows of rhs must be the same");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<float>>(nr1, nc2, false);

        if(nr1 == 1 && nc2 == 1) // Vector-Vector
            res->set(0, 0, cblas_sdot(nc1, lhs->getValues(), 1, rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip())));
        else if(nc2 == 1)        // Matrix-Vector
            cblas_sgemv(CblasRowMajor, CblasNoTrans, nr1, nc1, 1, lhs->getValues(),
                    static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip()), 0,res->getValues(),
                    static_cast<int>(res->getRowSkip()));
        else                     // Matrix-Matrix
            cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, nr1, nc2, nc1,
                    1, lhs->getValues(), static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                static_cast<int>(rhs->getRowSkip()), 0, res->getValues(), static_cast<int>(res->getRowSkip()));
    }
};

template<>
struct MatMul<DenseMatrix<double>, DenseMatrix<double>, DenseMatrix<double>> {
    static void apply(DenseMatrix<double> *& res, const DenseMatrix<double> * lhs, const DenseMatrix<double> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nc1 == static_cast<int>(rhs->getNumRows())) && "#cols of lhs and #rows of rhs must be the same");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<double>>(nr1, nc2, false);

        if(nr1 == 1 && nc2 == 1) // Vector-Vector
            res->set(0, 0, cblas_ddot(nc1, lhs->getValues(), 1, rhs->getValues(),
                static_cast<int>(rhs->getRowSkip())));
        else if(nc2 == 1)        // Matrix-Vector
            cblas_dgemv(CblasRowMajor, CblasNoTrans, nr1, nc1, 1, lhs->getValues(),
            static_cast<int>(lhs->getRowSkip()), rhs->getValues(),static_cast<int>(rhs->getRowSkip()), 0,
                res->getValues(), static_cast<int>(res->getRowSkip()));
        else                     // Matrix-Matrix
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, nr1, nc2, nc1,
                    1, lhs->getValues(), static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip()), 0, res->getValues(), static_cast<int>(res->getRowSkip()));
    }
};


//Filters each vector in X along dim.
//Filter IIR coefficients are given in vector A with length P+1,
//where P is the IIR filter order (P=0 means only a0).
//Filter FIR coefficients are given in vector B with length Q+1,
//where Q is the FIR filter order (Q=0 means only b0).

//For each vector: a0*Y[t] = (b0*X[t] + b1*X[t-1] + ... + bQ*X[t-Q]) - (a1*Y[t-1] + ... + aP*Y[t-P])

//The calling program must ensure that the sizes are correct, the filter is stable, etc.

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

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

int filter_s (float *Y, const float *X, float *A, float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim);
int filter_d (double *Y, const double *X, double *A, double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim);
int filter_c (float *Y, const float *X, float *A, float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim);
int filter_z (double *Y, const double *X, double *A, double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim);

int filter_s (float *Y, const float *X, float *A, float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in filter_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Deal with a0 (usually 1) and b0; also initialize Y
    float a = *A++, b = *B++;
    if (a==1.0f)
    {
        for (size_t p=P; p>0u; --p, ++A) { *A = -*A; }
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    }
    else
    {
        const float b_a = b / a;
        for (size_t p=P; p>0u; --p, ++A) { *A = -*A / a; }
        for (size_t q=Q; q>0u; --q, ++B) { *B /= a; }
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b_a * *X; }
        B -= Q;
    }
    A -= P; X -= N; Y -= N;

    if (N==0u || L==1u) {}
    else if (L==N)
    {
        //FIR
        if (L<3000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X) { ++Y; *Y = fmaf(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B) { ++Y; cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
        }
        Y -= Q;

        //IIR
        for (size_t l=0u; l<P; A-=l, ++l, ++Y)
        {
            for (size_t p=1u; p<=l; ++p, ++A) { *Y = fmaf(*A,*(Y-p),*Y); }
        }
        for (size_t l=P; l<L; ++l, A-=P, ++Y)
        {
            for (size_t p=1u; p<=P; ++p, ++A) { *Y = fmaf(*A,*(Y-p),*Y); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            for (size_t v=V; v>0u; --v)
            {
                //FIR
                if (L<30000u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X) { ++Y; *Y = fmaf(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                        else { X += Q; Y -= L-1u; }
                    }
                }
                else
                {
                    for (size_t q=1u; q<=Q; ++q, ++B) { ++Y; cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
                    X += L; Y -= Q;
                }
                B -= Q;

                //IIR
                for (size_t l=0u; l<P; A-=l, ++l, ++Y)
                {
                    for (size_t p=1u; p<=l; ++p, ++A) { *Y = fmaf(*A,*(Y-p),*Y); }
                }
                for (size_t l=P; l<L; ++l, A-=P, ++Y)
                {
                    for (size_t p=1u; p<=P; ++p, ++A) { *Y = fmaf(*A,*(Y-p),*Y); }
                }
            }
        }
        else
        {
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, Y-=K*L-1u)
                {
                    //FIR
                    for (size_t q=1u; q<=Q; ++q, ++B) { Y+=K; cblas_saxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                    Y -= Q*K; B -= Q;

                    //IIR
                    for (size_t l=0u; l<P; A-=l, ++l, Y+=K)
                    {
                        for (size_t p=1u; p<=l; ++p, ++A) { *Y = fmaf(*A,*(Y-K*p),*Y); }
                    }
                    for (size_t l=P; l<L; ++l, A-=P, Y+=K)
                    {
                        for (size_t p=1u; p<=P; ++p, ++A) { *Y = fmaf(*A,*(Y-K*p),*Y); }
                    }
                }
            }
        }
    }

    return 0;
}

int filter_d (double *Y, const double *X, double *A, double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in filter_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Deal with a0 (usually 1) and b0; also initialize Y
    double a = *A++, b = *B++;
    if (a==1.0)
    {
        for (size_t p=P; p>0u; --p, ++A) { *A = -*A; }
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    }
    else
    {
        const double b_a = b / a;
        for (size_t p=P; p>0u; --p, ++A) { *A = -*A / a; }
        for (size_t q=Q; q>0u; --q, ++B) { *B /= a; }
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b_a * *X; }
        B -= Q;
    }
    A -= P; X -= N; Y -= N;

    if (N==0u || L==1u) {}
    else if (L==N)
    {
        //FIR
        if (L<3000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X) { ++Y; *Y = fma(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B) { ++Y; cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
        }
        Y -= Q;

        //IIR
        for (size_t l=0u; l<P; A-=l, ++l, ++Y)
        {
            for (size_t p=1u; p<=l; ++p, ++A) { *Y = fma(*A,*(Y-p),*Y); }
        }
        for (size_t l=P; l<L; ++l, A-=P, ++Y)
        {
            for (size_t p=1u; p<=P; ++p, ++A) { *Y = fma(*A,*(Y-p),*Y); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            for (size_t v=V; v>0u; --v)
            {
                //FIR
                if (L<30000u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X) { ++Y; *Y = fma(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                        else { X += Q; Y -= L-1u; }
                    }
                }
                else
                {
                    for (size_t q=1u; q<=Q; ++q, ++B) { ++Y; cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
                    X += L; Y -= Q;
                }
                B -= Q;

                //IIR
                for (size_t l=0u; l<P; A-=l, ++l, ++Y)
                {
                    for (size_t p=1u; p<=l; ++p, ++A) { *Y = fma(*A,*(Y-p),*Y); }
                }
                for (size_t l=P; l<L; ++l, A-=P, ++Y)
                {
                    for (size_t p=1u; p<=P; ++p, ++A) { *Y = fma(*A,*(Y-p),*Y); }
                }
            }
        }
        else
        {
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, Y-=K*L-1u)
                {
                    //FIR
                    for (size_t q=1u; q<=Q; ++q, ++B) { Y+=K; cblas_daxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                    Y -= Q*K; B -= Q;

                    //IIR
                    for (size_t l=0u; l<P; A-=l, ++l, Y+=K)
                    {
                        for (size_t p=1u; p<=l; ++p, ++A) { *Y = fma(*A,*(Y-K*p),*Y); }
                    }
                    for (size_t l=P; l<L; ++l, A-=P, Y+=K)
                    {
                        for (size_t p=1u; p<=P; ++p, ++A) { *Y = fma(*A,*(Y-K*p),*Y); }
                    }
                }
            }
        }
    }

    return 0;
}

int filter_c (float *Y, const float *X, float *A, float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in filter_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    float ar, ai, br, bi, xr, xi, yr, yi;

    //Deal with a0 (usually 1) and negate a1 to aP
    //Also, initialize Y
    const float b0r = *B++, b0i = *B++;
    const float a0r = *A++, a0i = *A++, a02 = a0r*a0r + a0i*a0i;
    const float b_ar = (b0r*a0r + b0i*a0i) / a02;
    const float b_ai = (b0i*a0r - b0r*a0i) / a02;
    for (size_t p=P; p>0u; --p)
    {
        ar = -*A; ai = -*(A+1u);
        *A++ = (ar*a0r + ai*a0i) / a02;
        *A++ = (ai*a0r - ar*a0i) / a02;
    }
    for (size_t q=Q; q>0u; --q)
    {
        br = *B++; bi = *B--;
        *B++ = (br*a0r + bi*a0i) / a02;
        *B++ = (bi*a0r - br*a0i) / a02;
    }
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = b_ar*xr - b_ai*xi;
        *Y++ = b_ar*xi + b_ai*xr;
    }
    A -= 2u*P; B -= 2u*Q; X -= 2u*N; Y -= 2u*N;

    if (N==0u || L==1u) {}
    else if (L==N)
    {
        //FIR
        for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
        {
            br = *B++; bi = *B++;
            for (size_t l=q; l<L; ++l)
            {
                Y += 2u;
                xr = *X++; xi = *X++;
                *Y++ += br*xr - bi*xi;
                *Y-- += br*xi + bi*xr;
            }
        }
        Y -= 2u*Q;

        //IIR
        for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2)
        {
            for (size_t p=1u; p<=l; ++p)
            {
                ar = *A++; ai = *A++;
                yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                *Y += ar*yr - ai*yi;
                *(Y+1u) += ar*yi + ai*yr;
            }
        }
        for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2)
        {
            for (size_t p=1u; p<=P; ++p)
            {
                ar = *A++; ai = *A++;
                yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                *Y += ar*yr - ai*yi;
                *(Y+1u) += ar*yi + ai*yr;
            }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            for (size_t v=V; v>0u; --v)
            {
                //FIR
                for (size_t q=1u; q<=Q; ++q)
                {
                    br = *B++; bi = *B++;
                    for (size_t l=q; l<L; ++l)
                    {
                        Y += 2u;
                        xr = *X++; xi = *X++;
                        *Y++ += br*xr - bi*xi;
                        *Y-- += br*xi + bi*xr;
                    }
                    if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    else { X += 2u*Q; Y -= 2u*L - 2u; }
                }
                B -= 2u*Q;

                //IIR
                for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2)
                {
                    for (size_t p=1u; p<=l; ++p)
                    {
                        ar = *A++; ai = *A++;
                        yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                        *Y += ar*yr - ai*yi;
                        *(Y+1u) += ar*yi + ai*yr;
                    }
                }
                for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2)
                {
                    for (size_t p=1u; p<=P; ++p)
                    {
                        ar = *A++; ai = *A++;
                        yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                        *Y += ar*yr - ai*yi;
                        *(Y+1u) += ar*yi + ai*yr;
                    }
                }
            }
        }
        else
        {
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X-=2u*K*L-2u, Y-=2u*K*L-2u)
                {
                    //FIR
                    //for (size_t q=1u; q<=Q; X-=2u*K*(L-q), ++q)
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l, X+=2u*K)
                        {
                            Y += 2u*K;
                            xr = *X; xi = *(X+1);
                            *Y++ += br*xr - bi*xi;
                            *Y-- += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*K*(L-q); Y -= 2u*K*(L-q-1u); }
                        else { X += 2u*K*Q; Y -= 2u*K*(L-1u); }
                    }
                    B -= 2u*Q;

                    //IIR
                    for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2u*K)
                    {
                        for (size_t p=1u; p<=l; ++p)
                        {
                            ar = *A++; ai = *A++;
                            yr = *(Y-2u*K*p); yi = *(Y-2u*K*p+1u);
                            *Y += ar*yr - ai*yi;
                            *(Y+1u) += ar*yi + ai*yr;
                        }
                    }
                    for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2u*K)
                    {
                        for (size_t p=1u; p<=P; ++p)
                        {
                            ar = *A++; ai = *A++;
                            yr = *(Y-2u*K*p); yi = *(Y-2u*K*p+1u);
                            *Y += ar*yr - ai*yi;
                            *(Y+1u) += ar*yi + ai*yr;
                        }
                    }
                }
            }
        }
    }

    return 0;
}

int filter_z (double *Y, const double *X, double *A, double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t P, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in filter_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    double ar, ai, br, bi, xr, xi, yr, yi;

    //Deal with a0 (usually 1) and negate a1 to aP
    //Also, initialize Y
    const double b0r = *B++, b0i = *B++;
    const double a0r = *A++, a0i = *A++, a02 = a0r*a0r + a0i*a0i;
    const double b_ar = (b0r*a0r + b0i*a0i) / a02;
    const double b_ai = (b0i*a0r - b0r*a0i) / a02;
    for (size_t p=P; p>0u; --p)
    {
        ar = -*A; ai = -*(A+1u);
        *A++ = (ar*a0r + ai*a0i) / a02;
        *A++ = (ai*a0r - ar*a0i) / a02;
    }
    for (size_t q=Q; q>0u; --q)
    {
        br = *B++; bi = *B--;
        *B++ = (br*a0r + bi*a0i) / a02;
        *B++ = (bi*a0r - br*a0i) / a02;
    }
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = b_ar*xr - b_ai*xi;
        *Y++ = b_ar*xi + b_ai*xr;
    }
    A -= 2u*P; B -= 2u*Q; X -= 2u*N; Y -= 2u*N;

    if (N==0u || L==1u) {}
    else if (L==N)
    {
        //FIR
        for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
        {
            br = *B++; bi = *B++;
            for (size_t l=q; l<L; ++l)
            {
                Y += 2u;
                xr = *X++; xi = *X++;
                *Y++ += br*xr - bi*xi;
                *Y-- += br*xi + bi*xr;
            }
        }
        Y -= 2u*Q;

        //IIR
        for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2)
        {
            for (size_t p=1u; p<=l; ++p)
            {
                ar = *A++; ai = *A++;
                yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                *Y += ar*yr - ai*yi;
                *(Y+1u) += ar*yi + ai*yr;
            }
        }
        for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2)
        {
            for (size_t p=1u; p<=P; ++p)
            {
                ar = *A++; ai = *A++;
                yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                *Y += ar*yr - ai*yi;
                *(Y+1u) += ar*yi + ai*yr;
            }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            for (size_t v=V; v>0u; --v)
            {
                //FIR
                for (size_t q=1u; q<=Q; ++q)
                {
                    br = *B++; bi = *B++;
                    for (size_t l=q; l<L; ++l)
                    {
                        Y += 2u;
                        xr = *X++; xi = *X++;
                        *Y++ += br*xr - bi*xi;
                        *Y-- += br*xi + bi*xr;
                    }
                    if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    else { X += 2u*Q; Y -= 2u*L - 2u; }
                }
                B -= 2u*Q;

                //IIR
                for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2)
                {
                    for (size_t p=1u; p<=l; ++p)
                    {
                        ar = *A++; ai = *A++;
                        yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                        *Y += ar*yr - ai*yi;
                        *(Y+1u) += ar*yi + ai*yr;
                    }
                }
                for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2)
                {
                    for (size_t p=1u; p<=P; ++p)
                    {
                        ar = *A++; ai = *A++;
                        yr = *(Y-2u*p); yi = *(Y-2u*p+1u);
                        *Y += ar*yr - ai*yi;
                        *(Y+1u) += ar*yi + ai*yr;
                    }
                }
            }
        }
        else
        {
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X-=2u*K*L-2u, Y-=2u*K*L-2u)
                {
                    //FIR
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l, X+=2u*K)
                        {
                            Y += 2u*K;
                            xr = *X; xi = *(X+1);
                            *Y++ += br*xr - bi*xi;
                            *Y-- += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*K*(L-q); Y -= 2u*K*(L-q-1u); }
                        else { X += 2u*K*Q; Y -= 2u*K*(L-1u); }
                    }
                    B -= 2u*Q;

                    //IIR
                    for (size_t l=0u; l<P; ++l, A-=2u*l-2u, Y+=2u*K)
                    {
                        for (size_t p=1u; p<=l; ++p)
                        {
                            ar = *A++; ai = *A++;
                            yr = *(Y-2u*K*p); yi = *(Y-2u*K*p+1u);
                            *Y += ar*yr - ai*yi;
                            *(Y+1u) += ar*yi + ai*yr;
                        }
                    }
                    for (size_t l=P; l<L; ++l, A-=2u*P, Y+=2u*K)
                    {
                        for (size_t p=1u; p<=P; ++p)
                        {
                            ar = *A++; ai = *A++;
                            yr = *(Y-2u*K*p); yi = *(Y-2u*K*p+1u);
                            *Y += ar*yr - ai*yi;
                            *(Y+1u) += ar*yi + ai*yr;
                        }
                    }
                }
            }
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


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

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

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

/*
gsl_spblas_dgemv()
  Multiply a sparse matrix and a vector

Inputs: alpha - scalar factor
        A     - sparse matrix
        x     - dense vector
        beta  - scalar factor
        y     - (input/output) dense vector

Return: y = alpha*op(A)*x + beta*y
*/

int
gsl_spblas_dgemv(const CBLAS_TRANSPOSE_t TransA, const double alpha,
                 const gsl_spmatrix *A, const gsl_vector *x,
                 const double beta, gsl_vector *y)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if ((TransA == CblasNoTrans && N != x->size) ||
      (TransA == CblasTrans && M != x->size))
    {
      GSL_ERROR("invalid length of x vector", GSL_EBADLEN);
    }
  else if ((TransA == CblasNoTrans && M != y->size) ||
           (TransA == CblasTrans && N != y->size))
    {
      GSL_ERROR("invalid length of y vector", GSL_EBADLEN);
    }
  else
    {
      size_t j, p;
      size_t incX, incY;
      size_t lenX, lenY;
      double *X, *Y;
      double *Ad;
      size_t *Ap, *Ai, *Aj;

      if (TransA == CblasNoTrans)
        {
          lenX = N;
          lenY = M;
        }
      else
        {
          lenX = M;
          lenY = N;
        }

      /* form y := beta*y */

      Y = y->data;
      incY = y->stride;

      if (beta == 0.0)
        {
          size_t jy = 0;
          for (j = 0; j < lenY; ++j)
            {
              Y[jy] = 0.0;
              jy += incY;
            }
        }
      else if (beta != 1.0)
        {
          size_t jy = 0;
          for (j = 0; j < lenY; ++j)
            {
              Y[jy] *= beta;
              jy += incY;
            }
        }

      if (alpha == 0.0)
        return GSL_SUCCESS;

      /* form y := alpha*op(A)*x + y */
      Ap = A->p;
      Ad = A->data;
      X = x->data;
      incX = x->stride;

      if ((GSL_SPMATRIX_ISCCS(A) && (TransA == CblasNoTrans)) ||
          (GSL_SPMATRIX_ISCRS(A) && (TransA == CblasTrans)))
        {
          Ai = A->i;

          for (j = 0; j < lenX; ++j)
            {
              for (p = Ap[j]; p < Ap[j + 1]; ++p)
                {
                  Y[Ai[p] * incY] += alpha * Ad[p] * X[j * incX];
                }
            }
        }
      else if ((GSL_SPMATRIX_ISCCS(A) && (TransA == CblasTrans)) ||
               (GSL_SPMATRIX_ISCRS(A) && (TransA == CblasNoTrans)))
        {
          Ai = A->i;

          for (j = 0; j < lenY; ++j)
            {
              for (p = Ap[j]; p < Ap[j + 1]; ++p)
                {
                  Y[j * incY] += alpha * Ad[p] * X[Ai[p] * incX];
                }
            }
        }
      else if (GSL_SPMATRIX_ISTRIPLET(A))
        {
          if (TransA == CblasNoTrans)
            {
              Ai = A->i;
              Aj = A->p;
            }
          else
            {
              Ai = A->p;
              Aj = A->i;
            }

          for (p = 0; p < A->nz; ++p)
            {
              Y[Ai[p] * incY] += alpha * Ad[p] * X[Aj[p] * incX];
            }
        }
      else
        {
          GSL_ERROR("unsupported matrix type", GSL_EINVAL);
        }

      return GSL_SUCCESS;
    }
} /* gsl_spblas_dgemv() */


/*
 * particle_filters.c
 *
 *  Created on: 18 Sep 2020
 *      Author: heine
 */
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_eigen.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#include "particle_filters.h"

void MLbootstrapfilter(double* y, double sig_std, double obs_std, int data_length,
    double x0, int Nobs, short N_levels, long* sample_sizes, double* x_hats,
    double *filtering_time, double *Rinv0, double *Rinv1, double *worst_case_sign_ratio) {

  const char prog_bar[51] = "-------------------------------------------------";

  clock_t start;
  int bar_segment_count = 0;

  long N = 0;
  for (short i = 0; i < N_levels; i++) {
    N += sample_sizes[i];
//    printf("N%i = %lu, ", i, sample_sizes[i]);
  }
//  printf("N = %lu\n", N);

  double *X = (double*) malloc(N * sizeof(double));
  double *X_res = (double*) malloc(N * sizeof(double));
  double *W = (double*) malloc(N * sizeof(double));
  double *absW = (double*) malloc(N * sizeof(double));
  short *signs = (short*) malloc(N * sizeof(short));
  short *signs_res = (short*) malloc(N * sizeof(short));
  long *ind = (long*) malloc(N * sizeof(long));
  short *level_indicator = (short*) malloc(N * sizeof(short));
  double x_hat = 0;
  double p_hat = 0;

  printf(
      "%s\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
      prog_bar);
  fflush(stdout);

  start = clock();

  /*
   * Initial sample and level indicator
   */
  long sample_counter = 0;
  short current_level = 0;
  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng, clock());
  for (long i = 0; i < N; i++) {

    X[i] = gsl_ran_gaussian(rng, sig_std) + x0; // init sample
    W[i] = (double) 1.0 / (double) N; // init weights

    /* Construct level indicator array */
    signs_res[i] = 1;
    if (sample_counter >= sample_sizes[current_level]) {
      current_level++;
      sample_counter = 0;
    } else {
      sample_counter++;
    }
    level_indicator[i] = current_level;
  }

  double normaliser = 0;
  double abs_normaliser = 0;
  double diff;
  double ESS, ESSABS;
  double tmp_x_hat;
  double positive_mass, negative_mass;

  FILE* bpf_out = fopen("bpf_out.txt", "w");

  double* raw_like = (double*) malloc(2 * N * sizeof(double));
  long *permutation = (long*) malloc(N * sizeof(long));

  double lap1 = 0, lap2 = 0, lap3 = 0, lap4 = 0;
  clock_t iteration_timer;
  clock_t resampling_timer;
  clock_t level_timer_start;
  double level_0_particle_cost = 0;
  double level_1_particle_cost = 0;
  double rsamp_time_per_prcl = 0;
  double time_increment;
  double scaler;
  double num, den;
  worst_case_sign_ratio[0] = 100;
  double sign_balance;

  /* FILTER MAIN LOOP */
  for (int n = 0; n < data_length; n++) {

    iteration_timer = clock(); // start the stopwatch
    level_timer_start = clock();
    /*
     * Weight calculation
     */
    normaliser = 0;
    abs_normaliser = 0;
    for (long i = 0; i < N; i++) {
      if (level_indicator[i] == 0) {
        /* Use the diagonal observation covariance */
        raw_like[i] = likelihood(n, X[i], y, 0, Rinv0, Nobs, 1);
        raw_like[N + i] = 0;
      } else {
        /* Calculate both, the diagonal and full covariance to get level 1
         * differences */
        raw_like[i] = full_likelihood(n, X[i], y, Rinv1, Nobs, 1);
        raw_like[N + i] = likelihood(n, X[i], y, 0, Rinv0, Nobs, 1);
      }
      if (i == sample_sizes[0]) {
        /* When level indicator changes take the time per particle for level 0 */
        time_increment = (double) (clock() - level_timer_start) / CLOCKS_PER_SEC
            / (double) sample_sizes[0];
        level_0_particle_cost += time_increment;
        level_timer_start = clock();
      }
    }
    /* Time per particle for level 1 */
    time_increment = (double) (clock() - level_timer_start) / CLOCKS_PER_SEC
        / (double) sample_sizes[1];
    level_1_particle_cost += time_increment;
    lap1 += (double) (clock() - iteration_timer) / CLOCKS_PER_SEC;

    /*
     * Find scaler for the level 0 likelihood
     * */
    if (N_levels == 2) { // Do this only for non-unilevel
      num = 0;
      den = 0;
      for (int i = (int)sample_sizes[0]; i < N; i++) {
        num += raw_like[i] * raw_like[N + i];
        den += raw_like[N + i] * raw_like[N + i];
      }
      scaler = num / den;
      scaler = isnan(scaler) ? 1 : scaler;
      /*
       * Scale all level 0 weights by 'scaler'
       * */
      for (long i = 0; i < sample_sizes[0]; i++) // level 0
        raw_like[i] *= scaler;

      for (long i = sample_sizes[0]; i < N; i++) // Level 1
        raw_like[N + i] *= scaler;
    }

    /* Actual weight calculation */
    for (long i = 0; i < N; i++) {

      W[i] = (raw_like[i] - raw_like[N + i]) / (double) sample_sizes[level_indicator[i]]
          * (double) signs_res[i];

      absW[i] = fabs(W[i]);
      signs[i] = W[i] > 0 ? 1 : -1;
      normaliser += W[i];
      abs_normaliser += absW[i];
    }

    /* Normalise */
    tmp_x_hat = 0;
    positive_mass = 0;
    negative_mass = 0;
    for (long i = 0; i < N; i++) {
      W[i] /= normaliser;
      if (W[i] > 0) {
        positive_mass += W[i];
      } else {
        negative_mass += fabs(W[i]);
      }
      absW[i] /= abs_normaliser;
      tmp_x_hat += X[i] * W[i];
    }
    sign_balance = positive_mass / negative_mass;
    if (sign_balance < worst_case_sign_ratio[0]) {
      worst_case_sign_ratio[0] = sign_balance;
    }

    ESS = 0;
    ESSABS = 0;
    for (long i = 0; i < N; i++) {
      ESS += W[i] * W[i];
      ESSABS += absW[i] * absW[i];
    }
    ESS = (double) 1.0 / ESS;
    ESSABS = (double) 1.0 / ESSABS;

    lap2 += (double) (clock() - iteration_timer) / CLOCKS_PER_SEC;

    /* Resample */
    resampling_timer = clock();
    resample(N, absW, ind, rng);
    random_permuter(permutation, N, rng);
    rsamp_time_per_prcl += (double) (clock() - resampling_timer) / CLOCKS_PER_SEC / (double) N;

    lap3 += (double) (clock() - iteration_timer) / CLOCKS_PER_SEC;

    normaliser = 0;
    for (long i = 0; i < N; i++) {
      X_res[permutation[i]] = X[ind[i]];
      signs_res[permutation[i]] = signs[ind[i]];
      normaliser += signs[ind[i]];
    }
    for (long i = 0; i < N; i++) {
      W[i] = (double) signs_res[i] / normaliser;
    }

    /* Calculate the output: posterior mean */
    x_hat = 0;
    for (long i = 0; i < N; i++) {
      x_hat += X_res[i] * W[i];
    }
    x_hats[n] = x_hat;

    /* Calculate the output: posterior variance */
    p_hat = 0;
    for (long i = 0; i < N; i++) {
      diff = X_res[i] - x_hat;
      p_hat += diff * diff * W[i];
    }

    /* Mutate */
    for (long i = 0; i < N; i++)
      X[i] = X_res[i] + gsl_ran_gaussian(rng, sig_std);

    if (floor(n * (double) 50 / data_length) > bar_segment_count) {
      printf("█");
      fflush(stdout);
      bar_segment_count++;
    }
    lap4 += (double) (clock() - iteration_timer) / CLOCKS_PER_SEC;

  }

  filtering_time[0] = (double) (clock() - start) / CLOCKS_PER_SEC / (double) data_length;
  filtering_time[1] = level_0_particle_cost / (double) data_length;
  filtering_time[2] = level_1_particle_cost / (double) data_length;
  filtering_time[3] = rsamp_time_per_prcl / (double) data_length;
  printf(" %5.2f sec\n", filtering_time[0]);
  printf(" %5.2f  %5.2f  %5.2f  %5.2f\n", lap1, lap2, lap3, lap4);

  fclose(bpf_out);

  free(X);
  free(X_res);
  free(W);
  free(absW);
  free(signs);
  free(signs_res);
  free(ind);
  free(level_indicator);
  free(raw_like);
  free(permutation);
  gsl_rng_free(rng);

}

void bootstrapfilter(double* y, double sig_std, double obs_std, int data_length, double x0,
                     int Nobs, long N, double* x_hats, double *filtering_time, double *Rinv, short full) {

  const char prog_bar[51] = "-------------------------------------------------";
  clock_t start;
  int bar_segment_count = 0;

  printf("Basic BPF with N = %lu\n", N);

  double *X = (double*) malloc(N * sizeof(double));
  double *X_res = (double*) malloc(N * sizeof(double));
  double *W = (double*) malloc(N * sizeof(double));
  long *ind = (long*) malloc(N * sizeof(long));
  double x_hat = 0;
  double p_hat = 0;

  printf(
      "%s\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
      prog_bar);
  fflush(stdout);

  start = clock();

  /*
   * Initial sample and level indicator
   */
  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng, clock());
  for (long i = 0; i < N; i++) {
    X[i] = gsl_ran_gaussian(rng, sig_std) + x0;
    W[i] = (double) 1.0 / (double) N;
  }

  double normaliser = 0;
  double diff;
  double ESS;

  FILE* bpf_out = fopen("classicbpf_out.txt", "w");

  /* Classic BPF main loop */
  for (int n = 0; n < data_length; n++) {

    /* Weight calculation */
    normaliser = 0;

    /*
     * Weight calculation
     */
    for (long i = 0; i < N; i++) {
      if (full > 0) {
        W[i] = full_likelihood(n, X[i], y, Rinv, Nobs, 1);
      } else {
        W[i] = likelihood(n, X[i], y, 0, Rinv, Nobs, 1);
      }
      normaliser += W[i];
    }
    for (long i = 0; i < N; i++) {
      W[i] /= normaliser;
    }

    ESS = 0;
    for (long i = 0; i < N; i++) {
      ESS += W[i] * W[i];
    }
    ESS = (double) 1.0 / ESS;

    /* Resample */
    resample(N, W, ind, rng);
    for (long i = 0; i < N; i++) {
      X_res[i] = X[ind[i]];
    }

    /* Calculate the output: posterior mean */
    x_hat = 0;
    for (long i = 0; i < N; i++) {
      x_hat += X_res[i] / (double) N;
    }
    x_hats[n] = x_hat;
    /* Calculate the output: posterior variance */
    p_hat = 0;
    for (long i = 0; i < N; i++) {
      diff = X_res[i] - x_hat;
      p_hat += diff * diff / (double) N;
    }

    /* Mutate */
    for (long i = 0; i < N; i++)
      X[i] = X_res[i] + gsl_ran_gaussian(rng, sig_std);

    fprintf(bpf_out, "%i %f %f %f\n", n, x_hat, p_hat, ESS);
    fflush(bpf_out);

    if (floor(n * (double) 50 / data_length) > bar_segment_count) {
      printf("█");
      fflush(stdout);
      bar_segment_count++;
    }
  }
  filtering_time[0] = (double) (clock() - start) / CLOCKS_PER_SEC / (double) data_length;
  filtering_time[1] = 0;
  filtering_time[2] = 0;
  filtering_time[3] = 0;
  printf(" %5.2f sec\n", filtering_time[0]);

  fclose(bpf_out);

  free(X);
  free(X_res);
  free(W);
  free(ind);
  gsl_rng_free(rng);
}

double likelihood(int n, double x, double *y, int band, double *Rinv, int Nobs, double det) {

  double diff;
  double log_likelihood = 0;
  for (int j = 0; j < Nobs; j++) {
    diff = y[n * Nobs + j] - x;
    log_likelihood += (y[n * Nobs + j] - x) * Rinv[j * Nobs + j] * diff;
  }
  log_likelihood /= -(double) 2.0;
  return exp(log_likelihood) / det; // / sqrt(pow((2 * M_PI), Nobs) * det);

}

double full_likelihood(int n, double x, double *y, double *Rinv, int Nobs, double det) {

  double diff;
  double log_likelihood = 0;
  for (int j = 0; j < Nobs; j++) {
    diff = y[n * Nobs + j] - x;
    for (int i = 0; i < Nobs; i++) {
      log_likelihood += (y[n * Nobs + i] - x) * Rinv[j * Nobs + i] * diff;
    }
  }
  log_likelihood /= -(double) 2.0;
  return exp(log_likelihood); // / sqrt(pow((2 * M_PI), Nobs) * det);

}

void random_permuter(long *permutation, long N, gsl_rng *r) {

  for (long i = 0; i < N; i++)
    permutation[i] = i;

  long j;
  long tmp;
  for (long i = N - 1; i > 0; i--) {
    j = gsl_rng_uniform_int(r, i + 1);
    tmp = permutation[j];
    permutation[j] = permutation[i];
    permutation[i] = tmp;
  }

}

void resample(long size, double *w, long *ind, gsl_rng *r) {

  /* Generate the exponentials */
  double *e = (double*) malloc((size + 1) * sizeof(double));
  double g = 0;
  for (long i = 0; i <= size; i++) {
    e[i] = gsl_ran_exponential(r, 1.0);
    g += e[i];
  }
  /* Generate the uniform order statistics */
  double *u = (double *) malloc((size + 1) * sizeof(double));
  u[0] = 0;
  for (long i = 1; i <= size; i++)
    u[i] = u[i - 1] + e[i - 1] / g;

  /* Do the actual sampling with inverse cdf */
  double cdf = w[0];
  long j = 0;
  for (long i = 0; i < size; i++) {
    while (cdf < u[i + 1]) {
      j++;
      cdf += w[j];
    }
    ind[i] = j;
  }

  free(e);
  free(u);
}


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

/* Author:  J. Theiler (modifications by G. Jungman) */

/*
 * See Hart et al, Computer Approximations, John Wiley and Sons, New York (1968)
 * (This applies only to the erfc8 stuff, which is the part
 *  of the original code that survives. I have replaced much of
 *  the other stuff with Chebyshev fits. These are simpler and
 *  more precise than the original approximations. [GJ])
 */
#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_erf.h"

#include "check.h"

#include "chebyshev.h"
#include "cheb_eval.c"

#define LogRootPi_  0.57236494292470008706

static double erfc8_sum(double x)
{
  /* estimates erfc(x) valid for 8 < x < 100 */
  /* This is based on index 5725 in Hart et al */

  static double P[] = {
      2.97886562639399288862,
      7.409740605964741794425,
      6.1602098531096305440906,
      5.019049726784267463450058,
      1.275366644729965952479585264,
      0.5641895835477550741253201704
  };
  static double Q[] = {
      3.3690752069827527677,
      9.608965327192787870698,
      17.08144074746600431571095,
      12.0489519278551290360340491,
      9.396034016235054150430579648,
      2.260528520767326969591866945,
      1.0
  };
  double num=0.0, den=0.0;
  int i;

  num = P[5];
  for (i=4; i>=0; --i) {
      num = x*num + P[i];
  }
  den = Q[6];
  for (i=5; i>=0; --i) {
      den = x*den + Q[i];
  }

  return num/den;
}

inline
static double erfc8(double x)
{
  double e;
  e = erfc8_sum(x);
  e *= exp(-x*x);
  return e;
}

inline
static double log_erfc8(double x)
{
  double e;
  e = erfc8_sum(x);
  e = log(e) - x*x;
  return e;
}

#if 0
/* Abramowitz+Stegun, 7.2.14 */
static double erfcasympsum(double x)
{
  int i;
  double e = 1.;
  double coef = 1.;
  for (i=1; i<5; ++i) {
    /* coef *= -(2*i-1)/(2*x*x); ??? [GJ] */
    coef *= -(2*i+1)/(i*(4*x*x*x*x));
    e += coef;
    /*
    if (fabs(coef) < 1.0e-15) break;
    if (fabs(coef) > 1.0e10) break;

    [GJ]: These tests are not useful. This function is only
    used below. Took them out; they gum up the pipeline.
    */
  }
  return e;
}
#endif /* 0 */

/* Abramowitz+Stegun, 7.1.5 */
static int erfseries(double x, gsl_sf_result * result)
{
  double coef = x;
  double e    = coef;
  double del;
  int k;
  for (k=1; k<30; ++k) {
    coef *= -x*x/k;
    del   = coef/(2.0*k+1.0);
    e += del;
  }
  result->val = 2.0 / M_SQRTPI * e;
  result->err = 2.0 / M_SQRTPI * (fabs(del) + GSL_DBL_EPSILON);
  return GSL_SUCCESS;
}

/* Chebyshev fit for erfc((t+1)/2), -1 < t < 1
 */
static double erfc_xlt1_data[20] = {
  1.06073416421769980345174155056,
 -0.42582445804381043569204735291,
  0.04955262679620434040357683080,
  0.00449293488768382749558001242,
 -0.00129194104658496953494224761,
 -0.00001836389292149396270416979,
  0.00002211114704099526291538556,
 -5.23337485234257134673693179020e-7,
 -2.78184788833537885382530989578e-7,
  1.41158092748813114560316684249e-8,
  2.72571296330561699984539141865e-9,
 -2.06343904872070629406401492476e-10,
 -2.14273991996785367924201401812e-11,
  2.22990255539358204580285098119e-12,
  1.36250074650698280575807934155e-13,
 -1.95144010922293091898995913038e-14,
 -6.85627169231704599442806370690e-16,
  1.44506492869699938239521607493e-16,
  2.45935306460536488037576200030e-18,
 -9.29599561220523396007359328540e-19
};
static cheb_series erfc_xlt1_cs = {
  erfc_xlt1_data,
  19,
  -1, 1,
  12
};

/* Chebyshev fit for erfc(x) exp(x^2), 1 < x < 5, x = 2t + 3, -1 < t < 1
 */
static double erfc_x15_data[25] = {
  0.44045832024338111077637466616,
 -0.143958836762168335790826895326,
  0.044786499817939267247056666937,
 -0.013343124200271211203618353102,
  0.003824682739750469767692372556,
 -0.001058699227195126547306482530,
  0.000283859419210073742736310108,
 -0.000073906170662206760483959432,
  0.000018725312521489179015872934,
 -4.62530981164919445131297264430e-6,
  1.11558657244432857487884006422e-6,
 -2.63098662650834130067808832725e-7,
  6.07462122724551777372119408710e-8,
 -1.37460865539865444777251011793e-8,
  3.05157051905475145520096717210e-9,
 -6.65174789720310713757307724790e-10,
  1.42483346273207784489792999706e-10,
 -3.00141127395323902092018744545e-11,
  6.22171792645348091472914001250e-12,
 -1.26994639225668496876152836555e-12,
  2.55385883033257575402681845385e-13,
 -5.06258237507038698392265499770e-14,
  9.89705409478327321641264227110e-15,
 -1.90685978789192181051961024995e-15,
  3.50826648032737849245113757340e-16
};
static cheb_series erfc_x15_cs = {
  erfc_x15_data,
  24,
  -1, 1,
  16
};

/* Chebyshev fit for erfc(x) x exp(x^2), 5 < x < 10, x = (5t + 15)/2, -1 < t < 1
 */
static double erfc_x510_data[20] = {
  1.11684990123545698684297865808,
  0.003736240359381998520654927536,
 -0.000916623948045470238763619870,
  0.000199094325044940833965078819,
 -0.000040276384918650072591781859,
  7.76515264697061049477127605790e-6,
 -1.44464794206689070402099225301e-6,
  2.61311930343463958393485241947e-7,
 -4.61833026634844152345304095560e-8,
  8.00253111512943601598732144340e-9,
 -1.36291114862793031395712122089e-9,
  2.28570483090160869607683087722e-10,
 -3.78022521563251805044056974560e-11,
  6.17253683874528285729910462130e-12,
 -9.96019290955316888445830597430e-13,
  1.58953143706980770269506726000e-13,
 -2.51045971047162509999527428316e-14,
  3.92607828989125810013581287560e-15,
 -6.07970619384160374392535453420e-16,
  9.12600607264794717315507477670e-17
};
static cheb_series erfc_x510_cs = {
  erfc_x510_data,
  19,
  -1, 1,
  12
};

#if 0
inline
static double
erfc_asymptotic(double x)
{
  return exp(-x*x)/x * erfcasympsum(x) / M_SQRTPI;
}
inline
static double
log_erfc_asymptotic(double x)
{
  return log(erfcasympsum(x)/x) - x*x - LogRootPi_;
}
#endif /* 0 */

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

int gsl_sf_erfc_e(double x, gsl_sf_result * result)
{
  const double ax = fabs(x);
  double e_val, e_err;

  /* CHECK_POINTER(result) */

  if(ax <= 1.0) {
    double t = 2.0*ax - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&erfc_xlt1_cs, t, &c);
    e_val = c.val;
    e_err = c.err;
  }
  else if(ax <= 5.0) {
    double ex2 = exp(-x*x);
    double t = 0.5*(ax-3.0);
    gsl_sf_result c;
    cheb_eval_e(&erfc_x15_cs, t, &c);
    e_val = ex2 * c.val;
    e_err = ex2 * (c.err + 2.0*fabs(x)*GSL_DBL_EPSILON);
  }
  else if(ax < 10.0) {
    double exterm = exp(-x*x) / ax;
    double t = (2.0*ax - 15.0)/5.0;
    gsl_sf_result c;
    cheb_eval_e(&erfc_x510_cs, t, &c);
    e_val = exterm * c.val;
    e_err = exterm * (c.err + 2.0*fabs(x)*GSL_DBL_EPSILON + GSL_DBL_EPSILON);
  }
  else {
    e_val = erfc8(ax);
    e_err = (x*x + 1.0) * GSL_DBL_EPSILON * fabs(e_val);
  }

  if(x < 0.0) {
    result->val  = 2.0 - e_val;
    result->err  = e_err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
  }
  else {
    result->val  = e_val;
    result->err  = e_err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
  }

  return GSL_SUCCESS;
}

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

  if(x*x < 10.0*GSL_ROOT6_DBL_EPSILON) {
    const double y = x / M_SQRTPI;
    /* series for -1/2 Log[Erfc[Sqrt[Pi] y]] */
    const double c3 = (4.0 - M_PI)/3.0;
    const double c4 = 2.0*(1.0 - M_PI/3.0);
    const double c5 = -0.001829764677455021;  /* (96.0 - 40.0*M_PI + 3.0*M_PI*M_PI)/30.0  */
    const double c6 =  0.02629651521057465;   /* 2.0*(120.0 - 60.0*M_PI + 7.0*M_PI*M_PI)/45.0 */
    const double c7 = -0.01621575378835404;
    const double c8 =  0.00125993961762116;
    const double c9 =  0.00556964649138;
    const double c10 = -0.0045563339802;
    const double c11 =  0.0009461589032;
    const double c12 =  0.0013200243174;
    const double c13 = -0.00142906;
    const double c14 =  0.00048204;
    double series = c8 + y*(c9 + y*(c10 + y*(c11 + y*(c12 + y*(c13 + c14*y)))));
    series = y*(1.0 + y*(1.0 + y*(c3 + y*(c4 + y*(c5 + y*(c6 + y*(c7 + y*series)))))));
    result->val = -2.0 * series;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  /*
  don't like use of log1p(); added above series stuff for small x instead, should be ok [GJ]
  else if (fabs(x) < 1.0) {
    gsl_sf_result result_erf;
    gsl_sf_erf_e(x, &result_erf);
    result->val  = log1p(-result_erf.val);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  */
  else if(x > 8.0) {
    result->val = log_erfc8(x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result result_erfc;
    gsl_sf_erfc_e(x, &result_erfc);
    result->val  = log(result_erfc.val);
    result->err  = fabs(result_erfc.err / result_erfc.val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

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

  if(fabs(x) < 1.0) {
    return erfseries(x, result);
  }
  else {
    gsl_sf_result result_erfc;
    gsl_sf_erfc_e(x, &result_erfc);
    result->val  = 1.0 - result_erfc.val;
    result->err  = result_erfc.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

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

  {
    const double ex2 = exp(-x*x/2.0);
    result->val  = ex2 / (M_SQRT2 * M_SQRTPI);
    result->err  = fabs(x * result->val) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

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

  {
    gsl_sf_result result_erfc;
    int stat = gsl_sf_erfc_e(x/M_SQRT2, &result_erfc);
    result->val  = 0.5 * result_erfc.val;
    result->err  = 0.5 * result_erfc.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat;
  }
}

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

#include "eval.h"

double gsl_sf_erfc(double x)
{
  EVAL_RESULT(gsl_sf_erfc_e(x, &result));
}

double gsl_sf_log_erfc(double x)
{
  EVAL_RESULT(gsl_sf_log_erfc_e(x, &result));
}

double gsl_sf_erf(double x)
{
  EVAL_RESULT(gsl_sf_erf_e(x, &result));
}

double gsl_sf_erf_Z(double x)
{
  EVAL_RESULT(gsl_sf_erf_Z_e(x, &result));
}

double gsl_sf_erf_Q(double x)
{
  EVAL_RESULT(gsl_sf_erf_Q_e(x, &result));
}


/*
** Implementation of LISA algorithm
** for statistical inference of fMRI images
**
** 2nd level inference using a GLM design matrix,
**   e.g. for anova designs
**
** G.Lohmann, 2018
*/
#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/mu.h>
#include <viaio/option.h>
#include <viaio/os.h>
#include <viaio/VImage.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>

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

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

#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void   VIsolatedVoxels(VImage src,float threshold);
extern void   VHistogram(gsl_histogram *histogram,VString filename);
extern void   VCheckImage(VImage src);
extern void   FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double);
extern double ttest1(double *data1,int n);
extern void   ImageStats(VImage src,double *,double *,double *hmin,double *hmax);
extern void   VBilateralFilter(VImage src,VImage dest,int radius,double var1,double var2,int);
extern double VImageVar(VImage src);
extern void   VImageCount(VImage src);
extern void   VGetHistRange(VImage src,double *hmin,double *hmax);
extern void   VZScale(VImage src,float mode,float stddev);
extern float  VGetMode(VImage src);

extern void XReadExchange(VString filename,int *exchange,int n);
extern void GLM2(VImage *src,gsl_matrix *X,gsl_vector *contrast,int *,int *permtable,int *signtable,int signswitch,VImage dest);
extern gsl_matrix *XRead2ndLevel(VString);
extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);
extern int SignSwitch (gsl_matrix *X,gsl_vector *contrast,int *);
extern void genperm(long seed,int *exchange,int,int **,int **,int,int,gsl_vector *);
extern void HistoUpdate(VImage,gsl_histogram *);
extern gsl_matrix *VReadCovariates(VString,VBoolean);

int main (int argc, char *argv[])
{
  static VArgVector in_files1;
  static VString  out_filename="";
  static VString  design_filename="";
  static VString  exchange_filename="";
  static VString  nuisance_filename="";
  static VString  mask_filename="";
  static VArgVector cont;
  static VFloat   alpha = 0.05;
  static VShort   radius = 2;
  static VFloat   rvar = 2.0;
  static VFloat   svar = 2.0;
  static VShort   numiter = 2;
  static VShort   numperm = 5000;
  static VLong    seed = 99402622;
  static VBoolean centering = FALSE;
  static VBoolean demean  = TRUE;
  static VBoolean cleanup = TRUE;
  static VShort   nproc = 0;
  static VOptionDescRec options[] = {
    {"in", VStringRepn, 0, & in_files1, VRequiredOpt, NULL,"Input files" },
    {"out", VStringRepn, 1, & out_filename, VRequiredOpt, NULL,"Output file" },
    {"design",VStringRepn,1,(VPointer) &design_filename,VRequiredOpt,NULL,"Design file (2nd level)"},
    {"grp", VStringRepn, 1, & exchange_filename, VOptionalOpt, NULL,"Group Ids needed for exchangeability" },
    {"contrast", VFloatRepn, 0, (VPointer) &cont, VRequiredOpt, NULL, "contrast vector"},
    {"nuisance", VStringRepn, 1, & nuisance_filename, VOptionalOpt, NULL,"Nuisance regressors" },
    {"demean",VBooleanRepn,1,(VPointer) &demean,VOptionalOpt,NULL,"Whether to subtract mean in nuisance regressors"},
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"FDR significance level"},
    {"perm",VShortRepn,1,(VPointer) &numperm,VOptionalOpt,NULL,"Number of permutations"},
    {"mask", VStringRepn, 1, (VPointer) &mask_filename, VRequiredOpt, NULL, "Mask"},
    {"seed",VLongRepn,1,(VPointer) &seed,VOptionalOpt,NULL,"Seed for random number generation"},
    {"radius",VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Bilateral parameter (radius in voxels)"},
    {"rvar",VFloatRepn,1,(VPointer) &rvar,VOptionalOpt,NULL,"Bilateral parameter (radiometric)"},
    {"svar",VFloatRepn,1,(VPointer) &svar,VOptionalOpt,NULL,"Bilateral parameter (spatial)"},
    {"filteriterations",VShortRepn,1,(VPointer) &numiter,VOptionalOpt,NULL,"Bilateral parameter (number of iterations)"},
    {"cleanup",VBooleanRepn,1,(VPointer) &cleanup,VOptionalOpt,NULL,"Whether to remove isolated voxels"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"},
  };
  FILE *fp=NULL;
  VString in_filename;
  VAttrList list1=NULL,out_list=NULL,geolist=NULL;
  VImage *src1;
  int i,j,nimages,npix=0;
  double u=0;
  char *prg_name=GetLipsiaName("vlisa_2ndlevel");
  fprintf (stderr, "%s\n", prg_name);

  /* parse command line */
  if (! VParseCommand (VNumber (options), options, & argc, argv)) {
    VReportUsage (argv[0], VNumber (options), options, NULL);
    exit (EXIT_FAILURE);
  }
  if (argc > 1) {
    VReportBadArgs (argc, argv);
    exit (EXIT_FAILURE);
  }

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

  /* images  */
  VImage mask = VReadImageFile(mask_filename);
  if (mask==NULL) VError("Error reading mask file %s",mask_filename);

  nimages = in_files1.number;
  src1 = (VImage *) VCalloc(nimages,sizeof(VImage));
  for (i = 0; i < nimages; i++) {
    in_filename = ((VString *) in_files1.vector)[i];
    list1   = VReadAttrList(in_filename,0L,TRUE,FALSE);
    VMaskMinval(list1,mask,0.0);
    src1[i] = VReadImage(list1);
    if (src1[i] == NULL) VError(" no input image found");
    if (VPixelRepn(src1[i]) != VFloatRepn) VError(" input pixel repn must be float");
    if (i == 0) npix = VImageNPixels(src1[i]);
    else if (npix != VImageNPixels(src1[i])) VError(" inconsistent image dimensions");
    fprintf(stderr," %3d:  %s\n",i,in_filename);

    /* use geometry info from 1st file */
    if (geolist == NULL) geolist = VGetGeoInfo(list1);
  }

  /* get nuisance file */
  gsl_matrix *XN = NULL;
  if (strlen(nuisance_filename) > 1) {
    XN = VReadCovariates(nuisance_filename,demean);
    fprintf(stderr," nuisance covariates dimensions:  %d x %d\n",(int)XN->size1,(int)XN->size2);
    if (XN->size1 != nimages)
      VError(" number of input images (%d) does not match number of rows in nuisance covariates file (%d)",nimages,(int)XN->size1);
  }

  /* get design file and its inverse */
  gsl_matrix *X0 = XRead2ndLevel(design_filename);
  fprintf(stderr," design file dimensions:  %d x %d\n",(int)X0->size1,(int)X0->size2);
  if (X0->size1 != nimages)
    VError(" number of input images (%d) does not match number of rows in design file (%d)",nimages,(int)X0->size1);

  /* read contrast vector */
  if (cont.number != X0->size2)
    VError(" Dimension of contrast vector (%d) does not match number of columns in design file (%d)",cont.number,X0->size2);
  gsl_vector *contrast0 = gsl_vector_alloc(cont.number);
  for (i=0; i < cont.number; i++) {
    double u = ((VFloat *)cont.vector)[i];
    gsl_vector_set(contrast0, i, u);
  }

  /* concatenate design matrix and nuisance covariates */
  gsl_matrix *X = NULL;
  gsl_vector *contrast = NULL;
  int *permflag = NULL;

  /* include nuisance covariates, if present */
  if (XN != NULL) {
    permflag = (int *) VCalloc(XN->size2 + X0->size2,sizeof(int));
    for (j=0; j<XN->size2 + X0->size2; j++) permflag[j] = 0;
    for (j=0; j<X0->size2; j++) permflag[j] = 1;

    X = gsl_matrix_calloc(X0->size1, X0->size2 + XN->size2);
    for (i=0; i<X->size1; i++) {
      for (j=0; j<X0->size2; j++) {
	u = gsl_matrix_get(X0,i,j);
	gsl_matrix_set(X,i,j,u);
      }
      for (j=0; j<XN->size2; j++) {
	u = gsl_matrix_get(XN,i,j);
	gsl_matrix_set(X,i,j+X0->size2,u);
      }
    }
    contrast = gsl_vector_calloc(cont.number + XN->size2);
    gsl_vector_set_zero(contrast);
    for (j=0; j<contrast0->size; j++) gsl_vector_set(contrast,j,contrast0->data[j]);
  }

  /* no nuisance covariates */
  else {
    if (X0->size2 == 1) VError(" Please use 'vlisa_onesample' for onesample tests");
    permflag = (int *) VCalloc(X0->size2,sizeof(int));
    for (j=0; j<X0->size2; j++) permflag[j] = 1;
    X = gsl_matrix_calloc(X0->size1,X0->size2);
    gsl_matrix_memcpy(X,X0);
    contrast = gsl_vector_calloc(cont.number);
    gsl_vector_memcpy(contrast,contrast0);
  }

  /* read exchangeability information */
  int *exchange = (int *) VCalloc(X->size1,sizeof(int));
  for (i=0; i<X->size1; i++) exchange[i] = 1;  /* no restrictions w.r.t. exchangeability */
  if (strlen(exchange_filename) > 1) {
    XReadExchange(exchange_filename,exchange,(int)X->size1);
  }

  /* ini random permutations and sign switching */
  int signswitch = SignSwitch(X,contrast,permflag);
  int nperm=0;
  int **permtable = (int **) VCalloc(numperm,sizeof(int *));
  int **signtable = (int **) VCalloc(numperm,sizeof(int *));
  genperm((long)seed,exchange,signswitch,permtable,signtable,(int)nimages,(int)numperm,contrast);

  /* estimate null variance based on first 30 permutations */
  double hmin=0,hmax=0;
  float stddev=1.0;
  if (numperm > 0) {
    int tstperm = 30;
    if (tstperm > numperm) tstperm = numperm;
    double varsum=0,nx=0;

#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
    for (nperm = 0; nperm < tstperm; nperm++) {
      VImage zmap = VCreateImageLike(src1[0]);
      GLM2(src1,X,contrast,permflag,permtable[nperm],signtable[nperm],signswitch,zmap);
#pragma omp critical
      {
	varsum += VImageVar(zmap);
	nx++;
      }
      VDestroyImage(zmap);
    }
    double meanvar = varsum/nx;
    stddev = sqrt(meanvar);
  }

  /* no permutation */
  int *nopermtable = (int *) VCalloc(nimages,sizeof(int));
  int *nosigntable = (int *) VCalloc(nimages,sizeof(int));
  for (i=0; i<nimages; i++) nopermtable[i] = i;
  for (i=0; i<nimages; i++) nosigntable[i] = 1;
  VImage dst1  = VCreateImageLike (src1[0]);
  VImage zmap1 = VCreateImageLike(src1[0]);
  GLM2(src1,X,contrast,permflag,nopermtable,nosigntable,signswitch,zmap1);

  if (numperm == 0) {
    double z = VImageVar(zmap1);
    stddev = (float)(sqrt(z)); /* update stddev */
  }
  float mode=0;
  if (centering) mode = VGetMode(zmap1);
  if (numperm > 0) VZScale(zmap1,mode,stddev);
  VBilateralFilter(zmap1,dst1,(int)radius,(double)(rvar),(double)svar,(int)numiter);

  /* ini histograms */
  VGetHistRange(dst1,&hmin,&hmax);
  size_t nbins = 20000;
  gsl_histogram *hist0 = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (hist0,hmin,hmax);
  gsl_histogram *histz = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (histz,hmin,hmax);
  HistoUpdate(dst1,histz);

  /* random permutations */
#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
  for (nperm = 0; nperm < numperm; nperm++) {
    if (nperm%20 == 0) fprintf(stderr," perm  %4d  of  %d\r",nperm,(int)numperm);

    VImage zmap = VCreateImageLike(src1[0]);
    VImage dst  = VCreateImageLike (zmap);
    GLM2(src1,X,contrast,permflag,permtable[nperm],signtable[nperm],signswitch,zmap);
    float mode=0;
    if (centering) mode = VGetMode(zmap);
    VZScale(zmap,mode,stddev);
    VBilateralFilter(zmap,dst,(int)radius,(double)(rvar),(double)svar,(int)numiter);

#pragma omp critical
    {
      HistoUpdate(dst,hist0);
    }
    VDestroyImage(dst);
    VDestroyImage(zmap);
  }

  /* apply fdr */
  VImage fdrimage = VCopyImage (dst1,NULL,VAllBands);
  if (numperm > 0) {
    FDR(dst1,fdrimage,hist0,histz,(double)alpha);

    if (cleanup && alpha < 1.0) {
      VIsolatedVoxels(fdrimage,(float)(1.0-alpha));
    }
  }

  /* write output to disk */
  out_list = VCreateAttrList ();
  VHistory(VNumber(options),options,prg_name,&list1,&out_list);
  VSetGeoInfo(geolist,out_list);
  VAppendAttr (out_list,"image",NULL,VImageRepn,fdrimage);
  fp = VOpenOutputFile (out_filename, TRUE);
  if (! VWriteFile (fp, out_list)) exit (1);
  fclose(fp);
  fprintf (stderr, "\n%s: done.\n", argv[0]);
  exit(0);
}


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_ieee_utils.h>

#include "test_funcs.h"

int
test_fdf(const char * desc, gsl_multimin_function_fdf *f, initpt_function initpt, const gsl_multimin_fdfminimizer_type *T);

int
main (void)
{
  const gsl_multimin_fdfminimizer_type *fdfminimizers[5];
  const gsl_multimin_fdfminimizer_type ** T;

  gsl_ieee_env_setup ();

  fdfminimizers[0] = gsl_multimin_fdfminimizer_steepest_descent;
  fdfminimizers[1] = gsl_multimin_fdfminimizer_conjugate_pr;
  fdfminimizers[2] = gsl_multimin_fdfminimizer_conjugate_fr;
  fdfminimizers[3] = gsl_multimin_fdfminimizer_vector_bfgs;
  fdfminimizers[4] = 0;

  T = fdfminimizers;

  while (*T != 0)
    {
      test_fdf("Roth", &roth, roth_initpt,*T);
      test_fdf("Wood", &wood, wood_initpt,*T);
      test_fdf("Rosenbrock", &rosenbrock, rosenbrock_initpt,*T);
      T++;
    }

  T = fdfminimizers;

  while (*T != 0)
    {
      test_fdf("NRoth", &Nroth, roth_initpt,*T);
      test_fdf("NWood", &Nwood, wood_initpt,*T);
      test_fdf("NRosenbrock", &Nrosenbrock, rosenbrock_initpt,*T);
      T++;
    }

  exit (gsl_test_summary());
}

int
test_fdf(const char * desc,
         gsl_multimin_function_fdf *f,
	 initpt_function initpt,
	 const gsl_multimin_fdfminimizer_type *T)
{
  int status;
  size_t iter = 0;
  double step_size;

  gsl_vector *x = gsl_vector_alloc (f->n);

  gsl_multimin_fdfminimizer *s;

  (*initpt) (x);

  step_size = 0.1 * gsl_blas_dnrm2 (x);

  s = gsl_multimin_fdfminimizer_alloc(T, f->n);

  gsl_multimin_fdfminimizer_set (s, f, x, step_size, 0.1);

#ifdef DEBUG
  printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
  printf("g "); gsl_vector_fprintf (stdout, s->gradient, "%g");
#endif

  do
    {
      iter++;
      status = gsl_multimin_fdfminimizer_iterate(s);

#ifdef DEBUG
      printf("%i: \n",iter);
      printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
      printf("g "); gsl_vector_fprintf (stdout, s->gradient, "%g");
      printf("f(x) %g\n",s->f);
      printf("dx %g\n",gsl_blas_dnrm2(s->dx));
      printf("\n");
#endif

      status = gsl_multimin_test_gradient(s->gradient,1e-3);
    }
  while (iter < 5000 && status == GSL_CONTINUE);

  status |= (fabs(s->f) > 1e-5);

  gsl_test(status, "%s, on %s: %i iterations, f(x)=%g",
	   gsl_multimin_fdfminimizer_name(s),desc, iter, s->f);

  gsl_multimin_fdfminimizer_free(s);
  gsl_vector_free(x);

  return status;
}