#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Suites
#define EIGEN_USE_MKL_ALL
#include <boost/test/unit_test.hpp>
#include"basis.hpp"
#include"operators.hpp"
#include"tpoperators.hpp"

using namespace boost::unit_test;
using boost::unit_test_framework::test_suite;
using namespace Many_Body;
BOOST_AUTO_TEST_SUITE(basistesting)

BOOST_AUTO_TEST_SUITE(operatortesting)
BOOST_AUTO_TEST_CASE(operatoroperations)
{
  using namespace Eigen;
  using Mat=Operators::Mat;
  const size_t L=3;

   ElectronBasis<L> e1(1);
   //std::cout << int(L/2) << std::endl;
   ElectronBasis<L> e2(2);
      double t1=1;
   double t2=1;
   double u=1;
   // std::cout<< e1;
   //   std::cout<< e2;
   TensorProduct<ElectronBasis<L>, ElectronBasis<L>> TP(e1, e2);
   //        std::cout<< TP;
   //Mat E11=Operators::EKinOperator(TP,  t1);
   //   std::cout<< E11;
   Mat E1=Operators::EKinOperatorL(TP, e1, t1);
   Mat E2=Operators::EKinOperatorR(TP, e2, t2);
    Mat C=Operators::CalculateCouplungOperator(TP, e2, u);
    Mat H=E1+E2+ C;

    Eigen::MatrixXd HH =Eigen::MatrixXd(H);
    std::cout << HH << std::endl;
    Eigen::VectorXd ev(TP.dim);
    // diag(HH, ev);

    // std::cout << ev << std::endl;
   // std::cout << NR;
   // Mat EK=Operators::EKinOperator(e);

   //    std::cout << EK;
   // VectorXd v(5);
  //  MatrixXcd mat = MatrixXcd::Random(5, 5);
  // MatrixXcd mat4= mat+mat.adjoint();
  //  MatrixXcd mat2=mat4;

  //  Many_Body::diagherm(mat4, v);
  //     MatrixXcd mat3=mat4.adjoint();
  //  std::cout<< mat3*mat2*mat4<< std::endl;
  //   std::cout<< v<< std::endl;
  //   numberoperator<ElectronBasis> n2;
  //      kineticoperator<ElectronBasis> cdagc1(e);
  // 	kineticoperator<ElectronBasis> cdagc2;

  // 	double t=3;
  // 	souble l=1;
  // 	hamiltonian<ElectronBasis> H1{t*n1+ l*cdagc1};
  // 		hamiltonian<ElectronBasis > H2{t*n2+ l*cdagc2};
  // 		H2(e);
  // 		cout<< H2.energies();
 // 				cout<< H1.energies();

}
}
// BOOST_AUTO_TEST_CASE(electrondimension)
// {
// }
BOOST_AUTO_TEST_SUITE_END()
// EOF


#include <complex>
#include <iostream>
#include <cstdint>
#include <vector>
#include <cassert>
#include <random>

using namespace std;

typedef int64_t Torus64;

typedef int32_t Torus32;

const double DBL2M64 = 1./(1l<<32)/(1l<<32);

#if 1
//fixed point real number:
//96 bit integer v
//representing the value v/2^64 mod 2^32

#ifndef intmul
#define intmul intmul_best
#endif

struct Real96 {
    __uint128_t v;

    Real96(uint64_t value=0);
    Real96(uint64_t lo, uint64_t hi);
};

Real96::Real96(uint64_t value) {
    v=value;
}

Real96::Real96(uint64_t lo, uint64_t hi) {
    v=hi;
    v<<=64;
    v|=lo;
}

ostream& operator<<(ostream& out, const Real96& r) {
    out << "["<< (int64_t(r.v>>64)) << "].[" << (int64_t(r.v&0xFFFFFFFFFFFFFFFFUL)) << "] (" << double(__int128_t(r.v))*DBL2M64 << ")";
    return out;
}

void add(Real96& dest, const Real96& a, const Real96& b) {
    dest.v=a.v+b.v;
}
Real96 operator+(const Real96& a, const Real96& b) {
    Real96 reps; add(reps,a,b); return reps;
}
void operator+=(Real96& a, const Real96& b) {
    add(a,a,b);
}

void sub(Real96& dest, const Real96& a, const Real96& b) {
    dest.v=a.v-b.v;
}
void neg(Real96& dest, const Real96& a) {
    dest.v=-a.v;
}
Real96 operator-(const Real96& a, const Real96& b) {
    Real96 reps; sub(reps,a,b); return reps;
}
void operator-=(Real96& a, const Real96& b) {
    sub(a,a,b);
}
Real96 operator-(const Real96& a) {
    Real96 reps; neg(reps,a); return reps;
}

void extmul(Real96& dest, int a, const Real96& b) {
    dest.v=a*b.v;
}

void  intmul_ref(Real96& dest, const Real96& a, const Real96& b) {
    __int128_t av = a.v;
    //__uint128_t avu = a.v;
    __int128_t bv = b.v;
    //__uint128_t bvu = b.v;
    //__uint128_t alo = av&0xFFFFFFFFFFFFFFFFUL;
    __uint128_t blo = bv&0xFFFFFFFFFFFFFFFFUL;
    int32_t ahi = av>>64;
    __int128_t bhi = bv>>127;
    assert(bhi==0 || bhi==-1);
    assert(ahi<1l<<31 || ahi>=-1l<<31);

    __int128_t w0 = (uint64_t(a.v)*blo)>>64; //between [0 and 2^64-2]
    w0 += ahi*blo - (bhi&av);
    dest.v = w0;

}

void __attribute ((noinline)) intmul_asm_fk(Real96& dest, const Real96& a, const Real96& b) {
    //const Real96* aad = &a;
    //const Real96* bad = &b;
    //Real96* destad = &dest;
    uint64_t tmpa;
    uint64_t tmpb;
    uint64_t tmpc;
    uint64_t tmpd;
    uint64_t tmpe;
    uint64_t tbval;
    __asm volatile (
	"movq      %[b],%[tbval]\n"
        "movq      8(%[a]), %[tmpb]\n"           // tmpb=ahi
        "sarq      $63, %[tmpb]\n"               // tmpb=ahi<0?-1:0
        "movq      (%[tbval]), %%rdx\n"          // rdx=blo
        "mulx      (%[a]), %[tmpa], %[tmpe]\n"   // [e:a] = alo*blo = [u:v]
        "mulx      8(%[a]), %[tmpa], %[tmpd]\n"  // [d:a] = ahi*blo = [w:x]
        "negq      %%rdx\n"
        "andq      %%rdx, %[tmpb]\n"             // tmpb=ahi<0?-blo:0
        "addq      %[tmpe], %[tmpa]\n"           // x+=u
        "movq      8(%[tbval]), %[tmpc]\n"       // c=bhi
        "adcq      %[tmpb], %[tmpd]\n"           // w+=-blo if ahi<0
        "btq       $63, %[tmpc]\n"               // test sign of bhi
        "jnc       1f\n"
        "subq      (%[a]), %[tmpa]\n"            // x-=alo
        "sbbq      8(%[a]), %[tmpd]\n"           // w-=ahi+carry
        "1:\n"
        "movq      %[tmpa], (%[dest])\n"
        "movq      %[tmpd], 8(%[dest])\n"
	: [tmpa] "=r" (tmpa),
	  [tmpb] "=r" (tmpb),
	  [tmpc] "=r" (tmpc),
	  [tmpd] "=r" (tmpd),
	  [tmpe] "=r" (tmpe),
	  [tbval] "=r" (tbval)
	: [dest] "r" (&dest),
	  [a] "r"(&a),
	  [b] "r"(&b)
	: "%rdx","memory"
	   );
}

#include <bmi2intrin.h>

typedef union {
    __uint128_t vu128;
    unsigned long long vu64[2];
    __int128_t v128;
    long long v64[2];
} UINT128;

void intmul_best(Real96& dest, const Real96& a, const Real96& b) {
    const UINT128* aa = (const UINT128*) &a;
    const UINT128* ab = (const UINT128*) &b;
    //UINT128* adest = (UINT128*) &dest;
    UINT128 tab;
    UINT128 tcd;
    //UINT128 tef;
    _mulx_u64(ab->vu64[0],aa->vu64[0],&tcd.vu64[0]);  // td=u
    tab.vu64[0]=_mulx_u64(ab->vu64[0],aa->vu64[1],&tab.vu64[1]);
    tcd.vu64[1]=(aa->v64[1]>>63)&(-ab->vu64[0]); //(aa->vu64[1]&0x8000000000000000UL)?(-ab->vu64[0]):0;
    tab.vu128+=tcd.vu128;
    if (ab->vu64[1]&0x8000000000000000UL) tab.vu128-=aa->vu128;
    //tcd.vu64[0]=ab->vu64[1]&aa->vu64[0];
    //tcd.vu64[1]=ab->vu64[1]&aa->vu64[1];
    //tab.vu128-=tcd.vu128;
#ifdef NDEBUG
    dest.v=tab.vu128;
#else
    intmul_ref(dest,a,b);
    assert(dest.v==tab.vu128);
#endif
}

Real96 operator*(int a, const Real96& b) {
    Real96 reps; extmul(reps,a,b); return reps;
}
void operator*=(Real96& b, int a) {
    extmul(b,a,b);
}
Real96 operator*(const Real96& a, const Real96& b) {
    Real96 reps; intmul(reps,a,b); return reps;
}
void operator*=(Real96& a, const Real96& b) {
    intmul(a,a,b);
}

Real96 t64tor96(Torus64 v) {
    Real96 reps;
    int64_t vv = v;
    reps.v = vv;
    return reps;
}

Real96 dtor96(double v) {
    Real96 reps;
    int64_t iv = floor(v);
    v = v-iv;
    assert(v>=0 && v<1.);
    v *= (1ul<<32);
    v *= (1ul<<32);
    uint64_t lv = v;
    __int128_t pv = iv;
    pv <<= 64;
    pv |= lv;
    reps.v = pv;
    return reps;
}

bool very_close(const Real96& a,const Real96& b) {
    bool reps = (abs(__int128_t(a.v-b.v))<10000);
    if (!reps) {
	cerr << "not close: " << a << " vs. " << b << endl;
    }
    return reps;
}

#else
typedef double Real96;

Real96 t64tor96(Torus64 v) {
    return double(v)*DBL2M64;
}

Real96 dtor96(double v) {
    return v;
}

bool very_close(const Real96& a,const Real96& b) {
    bool reps = (abs(a-b)<1e-5);
    if (!reps) {
	cerr << "not close: " << a << " vs. " << b << endl;
    }
    return reps;
}

#endif

//-----------------------------------------------------------------------

typedef complex<Real96> Cplx96;

//-----------------------------------------------------------------------

#include <NTL/RR.h>
using namespace NTL;

RR PI_RR = ComputePi_RR();

Real96 accurate_cos(int i,int n) { //cos(2pi*i/n)
    i = ((i%n) + n)%n;
    if (i==0) return Real96(-1,0);
    ZZ cosi = RoundToZZ(cos(PI_RR*2*i/n)*pow(to_RR(2),to_RR(64)));
    if (cosi>=0)
	return Real96(to_long(cosi));
    else
	return Real96(to_long(cosi+power(to_ZZ(2),64)),-1);

/*
    if (i>=3*n/4) return dtor96(cos(2.*M_PI*(n-i)/double(n)));
    if (i>=2*n/4) return dtor96(-cos(2.*M_PI*(i-n/2)/double(n)));
    if (i>=1*n/4) return dtor96(-cos(2.*M_PI*(n/2-i)/double(n)));
    return dtor96(cos(2.*M_PI*(i)/double(n)));
    */
}

Real96 accurate_sin(int i,int n) { //sin(2pi*i/n)
    i = ((i%n) + n)%n;
    if (i==n/4) return Real96(-1,0);
    ZZ sini = RoundToZZ(sin(PI_RR*2*i/n)*pow(to_RR(2),to_RR(64)));
    if (sini>=0)
	return Real96(to_long(sini));
    else
	return Real96(to_long(sini+power(to_ZZ(2),64)),-1);
/*
    if (i>=3*n/4) return dtor96(-sin(2.*M_PI*(n-i)/double(n)));
    if (i>=2*n/4) return dtor96(-sin(2.*M_PI*(i-n/2)/double(n)));
    if (i>=1*n/4) return dtor96(sin(2.*M_PI*(n/2-i)/double(n)));
    return dtor96(sin(2.*M_PI*(i)/double(n)));
    */
}

//reverse the bits of i (mod n)
int rev(int i, int n) {
    int reps=0;
    for (int j=1; j<n; j*=2) {
	reps = 2*reps + (i%2);
	i/=2;
    }
    return reps;
}

bool very_close(const Cplx96& a,const Cplx96& b) {
    bool reps = (very_close(a.real(),b.real()) && very_close(a.imag(),b.imag()));
    if (!reps) {
	cerr << "not close: " << a << " vs. " << b << endl;
    }
    return reps;
}

// FFT from Torus64^N to Cplx96^(N/2)  mod X^N+1
// N = 2048 (note: n=2N ici)

//at the beginning of iteration nn
// a_{j,i} has P_{i%nn}(omega^j)
// where j between [rev(1) and rev(3)[
// and i between [0 and nn[
void ifft_check(int n, int nn, const Cplx96* acur, const vector<Real96>& a, const vector<Cplx96>& powomega) {
    int ns4=n/4;
    cerr << "Checking iteration " << nn << endl;
    for (int i=0; i<ns4; i++) {
	cout << "i: " << i << "   " << acur[i] << endl;
    }
    int m = n/nn;
    int rev1m = rev(1,m);
    int rev3m = rev(3,m);
    int idex = 0;
    for (int revj=rev1m; revj<rev3m; revj++) {
	int j = rev(revj,m);
	cerr << "check-- j: " << j << endl;
	for (int i=0; i<nn; i++) {
	    cerr << "check--- i: " << i << "(mod " << nn << ")" << endl;
	    const Cplx96& test_cur = acur[idex];
	    //sum_[t=i%nn] a_t omega^jt
	    Cplx96 pij(0,0);
	    for (int k=i; k<n; k+=nn) {
		//cout << "ak: " << a[k] << endl;
		pij += a[k] * powomega[(k*j) % n];
	    }
	    assert(very_close(test_cur,pij));
	    idex++;
	}
    }
}

//at the beginning of iteration halfnn:
//   m=n/halfnn
//   P_{j%m}(omb^i)
//   for j in [rev(1,m) to rev(3,m)[
//   and i in [0,halfnn[
void fft_check(
	int n, int halfnn,
	const Cplx96* pcur,
	const vector<Cplx96>& p,
	const vector<Cplx96>& powombar
	) {
    int ns4=n/4;
    cerr << "DIRECT FFT: Checking iteration " << halfnn  << endl;
    for (int i=0; i<ns4; i++) {
	cout << "i: " << i << "   " << pcur[i] << endl;
    }
    int m = n/halfnn;
    int rev1m = rev(1,m);
    int rev3m = rev(3,m);
    int idex = 0;
    for (int revj=rev1m; revj<rev3m; revj++) {
	int j = rev(revj,m);
	cerr << "check-- j: " << j << "(mod " << m << ")" << endl;
	for (int i=0; i<halfnn; i++) {
	    cerr << "check--- i: " << i << endl;
	    //P_sum_[k=j%m] p_k omb^ik-j
	    Cplx96 pij(0,0);
	    for (int k=j; k<n; k+=m) {
		//if (halfnn==8 && j==1 && i==1) cerr << "pij(" << pij << ")" << "+= p_"<<k<<"("<<p[k]<<") * omb["<<i*(k-j)<<"]("<< powombar[(i*(k-j)) % n] <<")" << endl;
		pij += p[k] * powombar[(i*(k-j)) % n];
	    }
	    assert(very_close(pcur[idex],pij));
	    idex++;
	}
    }
}

void precomp_iFFT(vector<Cplx96>& powomega, int n) {
    powomega.resize(n);
    for (int i=0; i<n; i++)
	powomega[i]=Cplx96(accurate_cos(i,n),accurate_sin(i,n));
}

void precomp_FFT(vector<Cplx96>& powombar, int n) {
    powombar.resize(n);
    for (int i=0; i<n; i++)
	powombar[i]=Cplx96(accurate_cos(i,n),accurate_sin((n-i)%n,n));
}

// P -> P(omega)
void iFFT(Cplx96* out, const Torus64* in, int n, const vector<Cplx96>& powomega) {
    //const int N = n/2;
    const int ns4 = n/4;

#ifndef NDEBUG
    vector<Real96> a; a.resize(n);
    for (int i=0; i<n/2; i++)
	a[i]=t64tor96(in[i]/2);
    for (int i=0; i<n/2; i++)
	a[n/2+i]=-a[i];
#endif

    //interpret the input coefs as real and imaginary parts:
    //RRRRRRRRRRIIIIIIIII
    //multiply by omega^j
    for (int j=0; j<ns4; j++)
	out[j] = Cplx96(t64tor96(in[j]),t64tor96(in[j+ns4]))*powomega[j];

    //at the beginning of iteration nn
    // a_{j,i} has P_{i%nn}(omega^j)
    // where j between [rev(1) and rev(3)[
    // and i between [0 and nn[
    for (int nn=ns4; nn>=2; nn/=2) {
	int halfnn = nn/2;
#ifndef NDEBUG
	cerr << "Starting iteration " << nn << endl;
	int m = n/nn;
	ifft_check(n, nn, out, a, powomega);
#endif
	for (int block=0; block<ns4; block+=nn) {
#ifndef NDEBUG
	    int j = rev(rev(1,m)+block,m);
	    cerr << "-- block j: " << j  << " --> " << j << "," << j+halfnn/2 << endl;
#endif
	    for (int off=0; off<halfnn; off++) {
#ifndef NDEBUG
		cerr << "--- i: " << off << " using: omg^" << (2*(ns4/halfnn)*off)%n << endl;
#endif
		Cplx96 t1 = out[block+off];
		Cplx96 t2 = out[block+off+halfnn];
		out[block+off]=t1+t2;
		out[block+off+halfnn]=(t1-t2)*powomega[(2*(ns4/halfnn)*off)%n];
	    }
	}
    }
    {
#ifndef NDEBUG
	int nn = 1;
	ifft_check(n, nn, out, a, powomega);
#endif
    }
}

// P(omega) -> P
void FFT(Torus64* out, Cplx96* in, int n, const vector<Cplx96>& powombar) {
    //const int N = n/2;
    const int ns4 = n/4;

#ifndef NDEBUG
    vector<Cplx96> a; a.resize(n);
    for (int i=0; i<n; i++) a[i]=Cplx96(0);
    int rev1m = rev(1,n);
    int rev3m = rev(3,n);
    for (int revj=rev1m; revj<rev3m; revj++) {
	int j = rev(revj,n);
	cout << "assign:" << j << " " << revj-rev1m << endl;
	a[j]=in[revj-rev1m];
	a[n-j]=Cplx96(a[j].real(),-a[j].imag());
    }
#endif

#ifndef NDEBUG
	cerr << "Checking iteration 1" << endl;
	fft_check(n, 1, in, a, powombar);
#endif

    //at the beginning of iteration nn
    // a_{j,i} has P_{i%nn}(omega^j)
    // where j between [rev(1) and rev(3)[
    // and i between [0 and nn[
    for (int nn=2; nn<=ns4; nn*=2) {
	int halfnn = nn/2;
	for (int block=0; block<ns4; block+=nn) {
	    //#ifndef NDEBUG
	    //	    int j = rev(rev(1,m)+block,m);
	    //	    cerr << "-- block j: " << j  << " --> " << j << "," << j+halfnn/2 << endl;
	    //#endif
	    for (int off=0; off<halfnn; off++) {
		//#ifndef NDEBUG
		//		cerr << "--- i: " << off << " using: omg^" << (2*(ns4/halfnn)*off)%n << endl;
		//#endif
		Cplx96 t1 = in[block+off];
		Cplx96 t2 = in[block+off+halfnn]*powombar[(2*(ns4/halfnn)*off)%n];
		in[block+off]=t1+t2;
		in[block+off+halfnn]=(t1-t2);
	    }
	}
#ifndef NDEBUG
	cerr << "Ending iteration " << nn << endl;
	fft_check(n, nn, in, a, powombar);
#endif
    }

    //interpret the input coefs as real and imaginary parts:
    //RRRRRRRRRRIIIIIIIII
    //multiply by omega^j
    for (int j=0; j<ns4; j++) {
	in[j] *= powombar[j];
	out[j] = in[j].real().v>>10;  // /ns4;  //divide by N/2
	out[j+ns4] = in[j].imag().v>>10; // /ns4; //divide by N/2
    }

    {
	//#ifndef NDEBUG
	//	int nn = 1;
	//	ifft_check(n, nn, out, a, powomega);
	//#endif
    }
}

int main(int argc, char** argv) {

#if 1

    const int N = 2048;
    const int Ns2 = N/2;
#ifndef NDEBUG
    const int NBTRIALS=1;
#else
    const int NBTRIALS=10000;
#endif

#ifndef NDEBUG
    //test cosinus
    for (int i=0; i<=2*N; i++) {
	//if (i!=1024) continue;
	Real96 c = accurate_cos(i,2*N);
	Real96 s = accurate_sin(i,2*N);
	cout << "i: " << i  << ", " << cos(2*i*M_PI/64.) << endl;
	cout << "c " << c << endl;
	cout << "s " << s << endl;
	cout << "c*c " << c*c << endl;
	cout << "s*s " << s*s << endl;
	Real96 t = c*c+s*s;
	cout << "cos test" <<  i << ":" << t << endl;
	assert(very_close(t,Real96(0,1)));
    }
#endif

    //test de la IFFT
    Cplx96 out[Ns2];
    Torus64 in[N];
    Torus64 revout[N];

    vector<Cplx96> powomega;
    vector<Cplx96> powombar;

    std::default_random_engine generator;
    std::uniform_int_distribution<Torus64> distribution(numeric_limits<Torus64>::min(),numeric_limits<Torus64>::max());

    for (int i=0; i<N; i++) {
	in[i]=distribution(generator);
    }

    precomp_iFFT(powomega,2*N);
    precomp_FFT(powombar,2*N);

#ifndef NDEBUG
    //test powomega
    for (int i=0; i<2*N; i++) {
	//if (i!=1024) continue;
	cout << "powomega: " << i << " : " << powomega[i] << endl;
	assert(very_close(powomega[i]*powomega[(2*N-i)%(2*N)],Cplx96(Real96(0,1))));
	assert(very_close(powomega[i]*powombar[i],Cplx96(Real96(0,1))));
    }

#endif

    clock_t t0 = clock();
    for (int i=0; i<NBTRIALS; i++)
	iFFT(out,in,2*N,powomega);
    clock_t t1 = clock();
    for (int i=0; i<NBTRIALS; i++)
	FFT(revout,out,2*N,powombar);
    clock_t t2 = clock();
#ifndef NDEBUG
    for (int i=0; i<N; i++)
	cout << hex << revout[i] << " "<< in[i] << " " << (revout[i]^in[i]) << endl;
#endif
    cout << "time IFFT: " << (t1-t0)/double(NBTRIALS) << "mus" << endl;
    cout << "time FFT: " << (t2-t1)/double(NBTRIALS) << "mus" << endl;

#endif

#if 0
    const int NBTRIALS=100;

    //test d'un keyswitch binaire
    int nlvl2=2048;
    int tlvl2=32;
    int klvl1=2;
    int Nlvl1=1024;
    int llvl1=4;

    int KS_size=Nlvl1*(klvl1+1)*(klvl1+1)*tlvl2*nlvl2;
    cout << "KS_size: " << KS_size << " * 32bits" << endl;
    Torus32* KS_rrr = new Torus32[Nlvl1*(klvl1+1)*(klvl1+1)*tlvl2*nlvl2];
    Torus32** KS_rr = new Torus32*[(klvl1+1)*tlvl2*nlvl2];
    Torus32*** KS_r = new Torus32**[tlvl2*nlvl2];
    Torus32**** KS = new Torus32***[nlvl2];
    for (int i=0; i<Nlvl1*(klvl1+1)*(klvl1+1)*tlvl2*nlvl2; i++) KS_rrr[i]=i; //random
    for (int i=0; i<(klvl1+1)*tlvl2*nlvl2; i++) KS_rr[i]=KS_rrr+i*(Nlvl1*(klvl1+1));
    for (int i=0; i<tlvl2*nlvl2; i++) KS_r[i]=KS_rr+i*(klvl1+1);
    for (int i=0; i<nlvl2; i++) KS[i]=KS_r+i*tlvl2;

    Torus64* samples_r = new Torus64[(nlvl2+1)*llvl1];
    Torus64** samples = new Torus64*[llvl1];
    for (int i=0; i<(nlvl2+1)*llvl1; i++) samples_r[i]=i; //random
    for (int i=0; i<llvl1; i++) samples[i]=samples_r+i*(nlvl2+1);

    Torus32* KSres = new Torus32[(klvl1+1)*Nlvl1];

    clock_t tdebks = clock();
    for (int trial=0; trial<NBTRIALS; trial++) {
	int u=0;
	for (int i=0; i<(klvl1+1)*Nlvl1; i++) KSres[i]=0;
	KSres[u*Nlvl1]=samples[0][nlvl2]; //b
	for (int i=0; i<nlvl2; i++) {
	    //decompose a
	    uint64_t ai=samples[0][i];
	    for (int j=0; j<tlvl2; j++) {
		int8_t aij=(ai & 1ul<<(63-j))?1:0;
		for (int l=0; l<(klvl1+1)*Nlvl1; l++)
		    KSres[l]+=aij*KS[i][j][u][l];
	    }
	}
    }
    clock_t tendks = clock();

    cout << "KS:" << double(tendks-tdebks)/NBTRIALS << "mus " << endl;
#endif

}


/**
 * \ file FFT.cpp
 */

#include <cmath>

#include <ATK/Utility/FFT.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( FFT_test )
{
  std::vector<double> input(128);
  std::vector<double> ref(128);
  for(int i = 0; i < 100; ++i)
  {
    input[i] = ref[i] = i+1;
  }
  std::vector<std::complex<double> > frequency(128);

  ATK::FFT<double> processor;
  processor.set_size(128);

  processor.process_forward(input.data(), frequency.data(), 100);
  processor.process_backward(frequency.data(), input.data(), 128);

  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(input[i], ref[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( doubleFFT_test )
{
  std::vector<double> input(128);
  std::vector<double> double_input(128);
  std::vector<double> ref(128);
  for(int i = 0; i < 100; ++i)
  {
    input[i] = ref[i] = i+1;
    double_input[i] = 2 * (i+1);
  }
  std::vector<std::complex<double> > frequency(128);
  std::vector<std::complex<double> > double_frequency(128);

  ATK::FFT<double> processor;
  processor.set_size(128);

  processor.process_forward(input.data(), frequency.data(), 100);
  processor.process_forward(double_input.data(), double_frequency.data(), 100);
  processor.process_backward(frequency.data(), input.data(), 128);
  processor.process_backward(double_frequency.data(), double_input.data(), 128);

  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(input[i], ref[i], 0.0001);
    BOOST_REQUIRE_CLOSE(double_input[i], 2*ref[i], 0.0001);
  }
}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
#include <cmath>
#include <iostream>

#include "sparsetensor.h"
#include "bpmfutils.h"
#include "noisemodels.h"

using namespace Eigen;

////  AdaptiveGaussianNoise  ////
void AdaptiveGaussianNoise::init(MatrixData & matrixData) {
  double se = 0.0;
  double mean_value = matrixData.mean_value;

#pragma omp parallel for schedule(dynamic, 4) reduction(+:se)
  for (int k = 0; k < matrixData.Y.outerSize(); ++k) {
    for (SparseMatrix<double>::InnerIterator it(matrixData.Y, k); it; ++it) {
      se += square(it.value() - mean_value);
    }
  }

  var_total = se / matrixData.Y.nonZeros();
  if (var_total <= 0.0 || std::isnan(var_total)) {
    // if var cannot be computed using 1.0
    var_total = 1.0;
  }
  // Var(noise) = Var(total) / (SN + 1)
  alpha     = (sn_init + 1.0) / var_total;
  alpha_max = (sn_max + 1.0)  / var_total;
}

void AdaptiveGaussianNoise::init(TensorData & data) {
  double se = 0.0;
  double mean_value = data.mean_value;

  auto& sparseMode   = (*data.Y)[0];
  VectorXd & values  = sparseMode->values;

#pragma omp parallel for schedule(dynamic, 4) reduction(+:se)
  for (int i = 0; i < values.size(); i++) {
    se += square(values(i) - mean_value);
  }
  var_total = se / values.size();
  if (var_total <= 0.0 || std::isnan(var_total)) {
    var_total = 1.0;
  }
  // Var(noise) = Var(total) / (SN + 1)
  alpha     = (sn_init + 1.0) / var_total;
  alpha_max = (sn_max + 1.0)  / var_total;
}

void AdaptiveGaussianNoise::update(MatrixData & data, std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples)
{
  double sumsq = 0.0;
  MatrixXd & U = *samples[0];
  MatrixXd & V = *samples[1];

  Eigen::SparseMatrix<double> & train = data.Y;
  double mean_value = data.mean_value;

#pragma omp parallel for schedule(dynamic, 4) reduction(+:sumsq)
  for (int j = 0; j < train.outerSize(); j++) {
    auto Vj = V.col(j);
    for (SparseMatrix<double>::InnerIterator it(train, j); it; ++it) {
      double Yhat = Vj.dot( U.col(it.row()) ) + mean_value;
      sumsq += square(Yhat - it.value());
    }
  }
  // (a0, b0) correspond to a prior of 1 sample of noise with full variance
  double a0 = 0.5;
  double b0 = 0.5 * var_total;
  double aN = a0 + train.nonZeros() / 2.0;
  double bN = b0 + sumsq / 2.0;
  alpha = rgamma(aN, 1.0 / bN);
  if (alpha > alpha_max) {
    alpha = alpha_max;
  }
}

void AdaptiveGaussianNoise::update(TensorData & data, std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples)
{
  double sumsq = 0.0;
  double mean_value = data.mean_value;

  auto& sparseMode = (*data.Y)[0];
  auto& U = samples[0];

  const int nmodes = samples.size();
  const int num_latents = U->rows();

#pragma omp parallel for schedule(dynamic, 4) reduction(+:sumsq)
  for (int n = 0; n < data.dims(0); n++) {
    Eigen::VectorXd u = U->col(n);
    for (int j = sparseMode->row_ptr(n);
             j < sparseMode->row_ptr(n + 1);
             j++)
    {
      VectorXi idx = sparseMode->indices.row(j);
      // computing prediction from tensor
      double Yhat = mean_value;
      for (int d = 0; d < num_latents; d++) {
        double tmp = u(d);

        for (int m = 1; m < nmodes; m++) {
          tmp *= (*samples[m])(d, idx(m - 1));
        }
        Yhat += tmp;
      }
      sumsq += square(Yhat - sparseMode->values(j));
    }

  }
  double a0 = 0.5;
  double b0 = 0.5;
  double aN = a0 + sparseMode->values.size() / 2.0;
  double bN = b0 + sumsq / 2.0;
  alpha = rgamma(aN, 1.0 / bN);
  if (alpha > alpha_max) {
    alpha = alpha_max;
  }
}

inline double nCDF(double val) {return 0.5 * erfc(-val * M_SQRT1_2);}

/////  evalModel functions
void ProbitNoise::evalModel(MatrixData & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  const unsigned N = data.Ytest.nonZeros();
  Eigen::VectorXd pred(N);
  Eigen::VectorXd test(N);
  Eigen::MatrixXd & rows = *samples[0];
  Eigen::MatrixXd & cols = *samples[1];

// #pragma omp parallel for schedule(dynamic,8) reduction(+:se, se_avg) <- dark magic :)
  for (int k = 0; k < data.Ytest.outerSize(); ++k) {
    int idx = data.Ytest.outerIndexPtr()[k];
    for (Eigen::SparseMatrix<double>::InnerIterator it(data.Ytest,k); it; ++it) {
     pred[idx] = nCDF(cols.col(it.col()).dot(rows.col(it.row())));
     test[idx] = it.value();

      // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Online_algorithm
      double pred_avg;
      if (n == 0) {
        pred_avg = pred[idx];
      } else {
        double delta = pred[idx] - predictions[idx];
        pred_avg = (predictions[idx] + delta / (n + 1));
        predictions_var[idx] += delta * (pred[idx] - pred_avg);
      }
      predictions[idx++] = pred_avg;

   }
  }
  auc_test_onesample = auc(pred,test);
  auc_test = auc(predictions, test);
}

void FixedGaussianNoise::evalModel(MatrixData & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
   auto rmse = eval_rmse(data.Ytest, n, predictions, predictions_var, *samples[1], *samples[0], data.mean_value);
   rmse_test = rmse.second;
   rmse_test_onesample = rmse.first;
}

void AdaptiveGaussianNoise::evalModel(MatrixData & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
   auto rmse = eval_rmse(data.Ytest, n, predictions, predictions_var, *samples[1], *samples[0], data.mean_value);
   rmse_test = rmse.second;
   rmse_test_onesample = rmse.first;
}

// evalModel for TensorData
void ProbitNoise::evalModel(TensorData & data, const int Nepoch, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  // TODO
  throw std::runtime_error("ProbitNoise::evalModel unimplemented.");
}

void FixedGaussianNoise::evalModel(TensorData & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  auto rmse = eval_rmse_tensor(data.Ytest, n, predictions, predictions_var, samples, data.mean_value);
  rmse_test = rmse.second;
  rmse_test_onesample = rmse.first;
}

void AdaptiveGaussianNoise::evalModel(TensorData & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  auto rmse = eval_rmse_tensor(data.Ytest, n, predictions, predictions_var, samples, data.mean_value);
  rmse_test = rmse.second;
  rmse_test_onesample = rmse.first;
}

//evalModel for Censored data
void FixedGaussianNoise::evalModel(MatrixDataCensored & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  auto rmse = eval_rmse(data.Ytest, n, predictions, predictions_var, *samples[1], *samples[0], data.mean_value);
  rmse_test = rmse.second;
  rmse_test_onesample = rmse.first;
}

void AdaptiveGaussianNoise::evalModel(MatrixDataCensored & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
  //TODO
  throw std::runtime_error("evalModel not implemented for Censored Data and adaptive noise");
}

void ProbitNoise::evalModel(MatrixDataCensored & data, const int n, Eigen::VectorXd & predictions, Eigen::VectorXd & predictions_var,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples) {
//TODO
throw std::runtime_error("evalModel not implemented for Censored Data and probit noise");
}


#include <Eigen/SparseCore>

extern "C" {

size_t
prod_nnz(
        size_t          a_rows,
        size_t          a_cols,
        size_t          b_cols,
        const int64_t * a_indptr,
        const int64_t * a_indices,
        const double *  a_data,
        const int64_t * b_indptr,
        const int64_t * b_indices,
        const double *  b_data
    )
{
    typedef Eigen::SparseMatrix<double, Eigen::RowMajor, int64_t> SpMat;
    int64_t a_nnz = a_indptr[a_rows];
    int64_t b_nnz = b_indptr[a_cols];
    Eigen::Map<const SpMat> a(
        a_rows, a_cols, a_nnz, a_indptr, a_indices, a_data
    );
    Eigen::Map<const SpMat> b(
        a_cols, b_cols, b_nnz, b_indptr, b_indices, b_data
    );
    SpMat c = a * b;
    return c.nonZeros();
}

} // extern C


/**
 * \ file ButterworthFilter.cpp
 */

#include <ATK/EQ/ButterworthFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.03158665365618605));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964151592208));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.011129047318919743));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.3520674471793266));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152940196));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.9961319837713813));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8406150184993316));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.27288445508133696));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.27185528281637));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8408965558193944));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8406817187144716));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.9981831475045972));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.9985753881639808));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8408969221170415));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


#include <boost/geometry/arithmetic/determinant.hpp>


#ifndef HOPS_DEGENERATEMULTIVARIATEGAUSSIANMODEL_HPP
#define HOPS_DEGENERATEMULTIVARIATEGAUSSIANMODEL_HPP

#define _USE_MATH_DEFINES
#include <math.h>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <utility>

namespace hops {
    template<typename Matrix, typename Vector>
    class DegenerateMultivariateGaussianModel {
    public:
        using MatrixType = Matrix;
        using VectorType = Vector;

        DegenerateMultivariateGaussianModel(VectorType mean, MatrixType covariance, std::vector<long> inactive = std::vector<long>(0));

        typename MatrixType::Scalar computeNegativeLogLikelihood(const VectorType &x) const;

        MatrixType computeExpectedFisherInformation(const VectorType &) const;

        VectorType computeLogLikelihoodGradient(const VectorType &x) const;

    private:
        VectorType mean;
        MatrixType covariance;
        std::vector<long> inactive;
        MatrixType inverseCovariance;
        typename MatrixType::Scalar logNormalizationConstant;

        void removeRow(Eigen::MatrixXd& matrix, unsigned int rowToRemove) const {
            unsigned int numRows = matrix.rows()-1;
            unsigned int numCols = matrix.cols();

            if (rowToRemove < numRows) {
                matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.bottomRows(numRows-rowToRemove);
            }

            matrix.conservativeResize(numRows,numCols);
        }

        void removeColumn(Eigen::MatrixXd& matrix, unsigned int colToRemove) const {
            unsigned int numRows = matrix.rows();
            unsigned int numCols = matrix.cols()-1;

            if (colToRemove < numCols) {
                matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.rightCols(numCols-colToRemove);
            }

            matrix.conservativeResize(numRows,numCols);
        }

        void removeRow(Eigen::VectorXd& vector, unsigned int rowToRemove) const {
            unsigned int numRows = vector.rows()-1;

            if (rowToRemove < numRows) {
                vector.segment(rowToRemove,numRows-rowToRemove) = vector.tail(numRows-rowToRemove);
            }

            vector.conservativeResize(numRows);
        }

        void stripInactive (Eigen::MatrixXd& matrix) const {
            for (auto& i : inactive) {
                removeRow(matrix, i);
                removeColumn(matrix, i);
            }
        }

        void stripInactive (Eigen::VectorXd& vector) const {
            for (auto& i : inactive) {
                removeRow(vector, i);
            }
        }
    };

    template<typename MatrixType, typename VectorType>
    DegenerateMultivariateGaussianModel<MatrixType, VectorType>::DegenerateMultivariateGaussianModel(VectorType mean,
                                                                                MatrixType covariance,
                                                                                std::vector<long> inactive) :
        mean(mean),
        covariance(covariance),
        inactive(inactive)
    {
        stripInactive(this->mean);
        stripInactive(this->covariance);

        Eigen::LLT<MatrixType, Eigen::Upper> solver(this->covariance);
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixL = solver.matrixL();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixU = solver.matrixU();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> inverseMatrixL = matrixL.inverse();
        inverseCovariance = inverseMatrixL * inverseMatrixL.transpose();

        logNormalizationConstant = -static_cast<typename MatrixType::Scalar>(this->mean.rows()) / 2 *
                                   std::log(2 * M_PI)
                                   - matrixL.diagonal().array().log().sum();
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar
    DegenerateMultivariateGaussianModel<MatrixType, VectorType>::computeNegativeLogLikelihood(const VectorType &x) const {
        VectorType _x = x;
        stripInactive(_x);
        return -logNormalizationConstant +
               0.5 * static_cast<typename MatrixType::Scalar>((_x - mean).transpose() * inverseCovariance * (_x - mean));
    }

    template<typename MatrixType, typename VectorType>
    MatrixType
    DegenerateMultivariateGaussianModel<MatrixType, VectorType>::computeExpectedFisherInformation(const VectorType &) const {
        return inverseCovariance;
    }

    template<typename MatrixType, typename VectorType>
    VectorType
    DegenerateMultivariateGaussianModel<MatrixType, VectorType>::computeLogLikelihoodGradient(const VectorType &x) const {
        VectorType _x = x;
        stripInactive(_x);
        return -inverseCovariance * (_x - mean);
    }
}

#endif //HOPS_DEGENERATEMULTIVARIATEGAUSSIANMODEL_HPP


#ifndef STAN_MATH_PRIM_PROB_NEG_BINOMIAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_NEG_BINOMIAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a negative binomial random variate with the specified shape and
 * inverse scale parameters using the given random number generator.
 *
 * alpha and beta can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_shape type of shape parameter
 * @tparam T_inv type of inverse scale parameter
 * @tparam RNG type of random number generator
 * @param alpha (Sequence of) positive shape parameter(s)
 * @param beta (Sequence of) positive inverse scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) negative binomial random variate(s)
 * @throw std::domain_error if alpha or beta are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_shape, typename T_inv, class RNG>
inline typename VectorBuilder<true, int, T_shape, T_inv>::type neg_binomial_rng(
    const T_shape& alpha, const T_inv& beta, RNG& rng) {
  using boost::gamma_distribution;
  using boost::variate_generator;
  using boost::random::poisson_distribution;
  using T_alpha_ref = ref_type_t<T_shape>;
  using T_beta_ref = ref_type_t<T_inv>;
  static const char* function = "neg_binomial_rng";
  check_consistent_sizes(function, "Shape parameter", alpha,
                         "Inverse scale Parameter", beta);
  T_alpha_ref alpha_ref = alpha;
  T_beta_ref beta_ref = beta;
  check_positive_finite(function, "Shape parameter", alpha_ref);
  check_positive_finite(function, "Inverse scale parameter", beta_ref);

  scalar_seq_view<T_alpha_ref> alpha_vec(alpha_ref);
  scalar_seq_view<T_beta_ref> beta_vec(beta_ref);
  size_t N = max_size(alpha, beta);
  VectorBuilder<true, int, T_shape, T_inv> output(N);

  for (size_t n = 0; n < N; ++n) {
    double rng_from_gamma = variate_generator<RNG&, gamma_distribution<> >(
        rng, gamma_distribution<>(alpha_vec[n], 1.0 / beta_vec[n]))();

    // same as the constraints for poisson_rng
    check_less(function, "Random number that came from gamma distribution",
               rng_from_gamma, POISSON_MAX_RATE);
    check_not_nan(function, "Random number that came from gamma distribution",
                  rng_from_gamma);
    check_nonnegative(function,
                      "Random number that came from gamma distribution",
                      rng_from_gamma);

    output[n] = variate_generator<RNG&, poisson_distribution<> >(
        rng, poisson_distribution<>(rng_from_gamma))();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP

#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
#  include <boost/geometry/io/dsv/write.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Strategy functor for distance point to segment calculation
\ingroup strategies
\details Class which calculates the distance of a point to a segment, using latlong points
\see http://williams.best.vwh.net/avform.htm
\tparam Point point type
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-point distance strategy, defaults to haversine

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename Point,
    typename PointOfSegment = Point,
    typename CalculationType = void,
    typename Strategy = typename services::default_strategy<point_tag, Point>::type
>
class cross_track
{
public :
    typedef typename promote_floating_point
        <
            typename select_calculation_type
                <
                    Point,
                    PointOfSegment,
                    CalculationType
                >::type
        >::type return_type;

    inline cross_track()
    {
        m_strategy = Strategy();
        m_radius = m_strategy.radius();
    }

    inline cross_track(return_type const& r)
        : m_radius(r)
        , m_strategy(r)
    {}

    inline cross_track(Strategy const& s)
        : m_strategy(s)
    {
        m_radius = m_strategy.radius();
    }

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    inline return_type apply(Point const& p,
                PointOfSegment const& sp1, PointOfSegment const& sp2) const
    {
        // http://williams.best.vwh.net/avform.htm#XTE
        return_type d1 = m_strategy.apply(sp1, p);

        // Actually, calculation of d2 not necessary if we know that the projected point is on the great circle...
        return_type d2 = m_strategy.apply(sp2, p);

        return_type crs_AD = course(sp1, p);
        return_type crs_AB = course(sp1, sp2);
        return_type XTD = m_radius * geometry::math::abs(asin(sin(d1 / m_radius) * sin(crs_AD - crs_AB)));

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " " << crs_AD * geometry::math::r2d << std::endl;
std::cout << "Course " << dsv(sp1) << " to " << dsv(sp2) << " " << crs_AB * geometry::math::r2d << std::endl;
std::cout << "XTD: " << XTD << " d1: " <<  d1  << " d2: " <<  d2  << std::endl;
#endif

        // Return shortest distance, either to projected point on segment sp1-sp2, or to sp1, or to sp2
        return return_type((std::min)((std::min)(d1, d2), XTD));
    }

    inline return_type radius() const { return m_radius; }

private :
    BOOST_CONCEPT_ASSERT
        (
            (geometry::concept::PointDistanceStrategy<Strategy >)
        );

    return_type m_radius;

    // Point-point distances are calculated in radians, on the unit sphere
    Strategy m_strategy;

    /// Calculate course (bearing) between two points. Might be moved to a "course formula" ...
    inline return_type course(Point const& p1, Point const& p2) const
    {
        // http://williams.best.vwh.net/avform.htm#Crs
        return_type dlon = get_as_radian<0>(p2) - get_as_radian<0>(p1);
        return_type cos_p2lat = cos(get_as_radian<1>(p2));

        // "An alternative formula, not requiring the pre-computation of d"
        return atan2(sin(dlon) * cos_p2lat,
            cos(get_as_radian<1>(p1)) * sin(get_as_radian<1>(p2))
            - sin(get_as_radian<1>(p1)) * cos_p2lat * cos(dlon));
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Point, typename PointOfSegment, typename CalculationType, typename Strategy>
struct tag<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template <typename Point, typename PointOfSegment, typename CalculationType, typename Strategy>
struct return_type<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
    typedef typename cross_track<Point, PointOfSegment, CalculationType, Strategy>::return_type type;
};

template
<
    typename Point,
    typename PointOfSegment,
    typename CalculationType,
    typename Strategy,
    typename P,
    typename PS
>
struct similar_type<cross_track<Point, PointOfSegment, CalculationType, Strategy>, P, PS>
{
    typedef cross_track<Point, PointOfSegment, CalculationType, Strategy> type;
};

template
<
    typename Point,
    typename PointOfSegment,
    typename CalculationType,
    typename Strategy,
    typename P,
    typename PS
>
struct get_similar<cross_track<Point, PointOfSegment, CalculationType, Strategy>, P, PS>
{
    static inline typename similar_type
        <
            cross_track<Point, PointOfSegment, CalculationType, Strategy>, P, PS
        >::type apply(cross_track<Point, PointOfSegment, CalculationType, Strategy> const& strategy)
    {
        return cross_track<P, PS, CalculationType, Strategy>(strategy.radius());
    }
};

template <typename Point, typename PointOfSegment, typename CalculationType, typename Strategy>
struct comparable_type<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
    // Comparable type is here just the strategy
    typedef typename similar_type
        <
            cross_track
                <
                    Point, PointOfSegment, CalculationType, Strategy
                >, Point, PointOfSegment
        >::type type;
};

template
<
    typename Point, typename PointOfSegment,
    typename CalculationType,
    typename Strategy
>
struct get_comparable<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
    typedef typename comparable_type
        <
            cross_track<Point, PointOfSegment, CalculationType, Strategy>
        >::type comparable_type;
public :
    static inline comparable_type apply(cross_track<Point, PointOfSegment, CalculationType, Strategy> const& strategy)
    {
        return comparable_type(strategy.radius());
    }
};

template
<
    typename Point, typename PointOfSegment,
    typename CalculationType,
    typename Strategy
>
struct result_from_distance<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
private :
    typedef typename cross_track<Point, PointOfSegment, CalculationType, Strategy>::return_type return_type;
public :
    template <typename T>
    static inline return_type apply(cross_track<Point, PointOfSegment, CalculationType, Strategy> const& , T const& distance)
    {
        return distance;
    }
};

template
<
    typename Point, typename PointOfSegment,
    typename CalculationType,
    typename Strategy
>
struct strategy_point_point<cross_track<Point, PointOfSegment, CalculationType, Strategy> >
{
    typedef Strategy type;
};

/*

TODO:  spherical polar coordinate system requires "get_as_radian_equatorial<>"

template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        segment_tag, Point, PointOfSegment,
        spherical_polar_tag, spherical_polar_tag,
        Strategy
    >
{
    typedef cross_track
        <
            Point,
            PointOfSegment,
            void,
            typename boost::mpl::if_
                <
                    boost::is_void<Strategy>,
                    typename default_strategy
                        <
                            point_tag, Point, PointOfSegment,
                            spherical_polar_tag, spherical_polar_tag
                        >::type,
                    Strategy
                >::type
        > type;
};
*/

template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        segment_tag, Point, PointOfSegment,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef cross_track
        <
            Point,
            PointOfSegment,
            void,
            typename boost::mpl::if_
                <
                    boost::is_void<Strategy>,
                    typename default_strategy
                        <
                            point_tag, Point, PointOfSegment,
                            spherical_equatorial_tag, spherical_equatorial_tag
                        >::type,
                    Strategy
                >::type
        > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP


#include "io_utils.h"

#include <Eigen/Dense>
#include <fstream>

Eigen::MatrixXd bayesmix::read_eigen_matrix(const std::string &filename,
                                            const char delim /* = ','*/) {
  // Initialize objects
  unsigned int cols = 0, rows = 0;
  double buffer[MAXBUFSIZE];
  std::ifstream filestream(filename);
  if (!filestream.is_open()) {
    std::string err = "File " + filename + " does not exist";
    throw std::invalid_argument(err);
  }

  // Loop over file lines
  std::string line, entry;
  while (getline(filestream, line, '\n')) {
    unsigned int temp = 0;
    std::stringstream linestream(line);
    while (getline(linestream, entry, delim)) {
      // Place read values into the buffer array
      std::stringstream entrystream(entry);
      entrystream >> buffer[cols * rows + temp++];
    }
    if (temp == 0) {
      continue;
    }
    if (cols == 0) {
      cols = temp;
    }
    rows++;
  }

  filestream.close();

  // Fill an Eigen Matrix with values from the buffer array
  Eigen::MatrixXd mat(rows, cols);
  for (size_t i = 0; i < rows; i++) {
    for (size_t j = 0; j < cols; j++) {
      mat(i, j) = buffer[cols * i + j];
    }
  }
  return mat;
};

void bayesmix::write_matrix_to_file(const Eigen::MatrixXd &mat,
                                    const std::string &filename,
                                    const char delim /*= ','*/) {
  using namespace Eigen;
  std::string del;
  del = delim;
  const IOFormat CSVFormat(StreamPrecision, DontAlignCols, del, "\n");
  std::ofstream file(filename.c_str());
  file << mat.format(CSVFormat);
}


#include <cassert>

#include <iostream>
#include <fstream>

#include <boost/program_options.hpp>

#include <graphlab.hpp>

#include "factor_graph.hpp"

#include <graphlab/macros_def.hpp>

// Types from the factor_graph.hpp
typedef factor_graph::factor_type factor_type;
typedef factor_graph::domain_type domain_type;
typedef factor_type::assignment_type  assignment_type;
typedef factor_graph::variable_type   variable_type;

// Structs
// ===========================================================================>

struct vertex_data {
  factor_type potential;
  factor_type belief;
  void load(graphlab::iarchive& arc) {
    arc >> potential;
    arc >> belief;
  }
  void save(graphlab::oarchive& arc) const {
    arc << potential;
    arc << belief;
  }
}; // End of vertex data

struct edge_data {
  factor_type message;
  factor_type old_message;
  void load(graphlab::iarchive& arc) {
    arc >> message;
    arc >> old_message;
  }
  void save(graphlab::oarchive& arc) const {
    arc << message;
    arc << old_message;
  }
}; // End of edge data

// Define the graph types
typedef graphlab::graph<vertex_data, edge_data> graph_type;
typedef graphlab::types<graph_type> gl_types;

double bound = 1e-5;
double damping = 0.3;

// Update Functions
// ===========================================================================>
void bp_update(gl_types::iscope& scope,
               gl_types::icallback& scheduler) {
   // Grab the state from the scope
  // ------------------------------------------------------->
  // Get the vertex data
  vertex_data& vdata = scope.vertex_data();

  // Get the in and out edges by reference
  const gl_types::edge_list in_edges = scope.in_edge_ids();
  const gl_types::edge_list out_edges = scope.out_edge_ids();
  assert(in_edges.size() == out_edges.size());

  // Compute the belief
  // ------------------------------------------------------->
  // Initialize the belief as the value of the factor
  vdata.belief = vdata.potential;
  foreach(graphlab::edge_id_t ineid, in_edges) {
    // Get the in and out edge data
    edge_data& edata = scope.edge_data(ineid);
    edata.old_message = edata.message;
    vdata.belief *= edata.old_message;
  }
  vdata.belief.normalize();

  // Compute outbound messages
  // ------------------------------------------------>
  // Send outbound messages
  factor_type cavity, tmp_msg;
  for(size_t i = 0; i < in_edges.size(); ++i) {
    // Get the edge ids
    const graphlab::edge_id_t outeid = out_edges[i];
    const graphlab::edge_id_t ineid = in_edges[i];
    // CLEVER HACK: Here we are expoiting the sorting of the edge ids
    // to do fast O(1) time edge reversal
    assert(scope.target(outeid) == scope.source(ineid));
    // Get the in and out edge data
    const edge_data& in_edge = scope.edge_data(ineid);
    edge_data& out_edge = scope.edge_data(outeid);

    // Create stack allocated cavity factor
    cavity = vdata.belief;
    // Compute cavity
    cavity /= in_edge.old_message; // Make the cavity a cavity
    cavity.normalize();

    // Create stack allocated temporary message
    tmp_msg.set_args(out_edge.message.args());
    assert(tmp_msg.num_vars() == 1);
    tmp_msg.marginalize(cavity);
    tmp_msg.normalize();

    // Compute message residual
    double residual = tmp_msg.l1_diff(out_edge.old_message);
     // Damp the message
    tmp_msg.damp(out_edge.message, damping);
    // Assign the out message
    out_edge.message = tmp_msg;

    if(residual > bound) {
      gl_types::update_task task(scope.target(outeid), bp_update);
      scheduler.add_task(task, residual);
    }
  }
} // end of BP_update

/**
 * Construct a belief propagation graph from a factor graph
 */
void make_bp_graph(const factor_graph& fgraph,
                   gl_types::graph& graph) {
  assert(!fgraph.variables().empty());
  assert(!fgraph.factors().empty());
  assert(fgraph.variables().rbegin()->id()
         == fgraph.variables().size()-1);
  vertex_data vdata;
  // Add all the variables
  foreach(factor_graph::variable_type variable, fgraph.variables()) {
    factor_graph::domain_type domain(variable);
    vdata.potential.set_args(domain);
    vdata.potential.uniform();
    vdata.belief = vdata.potential;
    graphlab::vertex_id_t vid = graph.add_vertex(vdata);
    assert(vid == variable.id());
  }
  assert(graph.num_vertices() == fgraph.variables().size());
  // Add all the factors and all the edges
  size_t factor_index = graph.num_vertices();
  edge_data edata;
  foreach(const factor_graph::factor_type& factor, fgraph.factors()) {
    // Setup the vertex data for a factor
    vdata.potential = factor;
    vdata.belief.set_args(factor.args());
    vdata.belief.uniform();
    // Add the factor to the graph
    graphlab::vertex_id_t vid = graph.add_vertex(vdata);
    assert(vid == factor_index);
    // Attach all the edges
    for(size_t i = 0; i < factor.num_vars(); ++i) {
      factor_graph::variable_type variable = factor.args().var(i);
      factor_graph::domain_type domain(variable);
      edata.message.set_args(domain);
      edata.message.uniform();
      edata.old_message = edata.message;
      graph.add_edge(factor_index, variable.id(), edata);
      graph.add_edge(variable.id(), factor_index, edata);
    }
    ++factor_index;
  }
  graph.finalize();
} // end of make_bp_graph

/**
 * Save the final belief estimates to a text file.
 */
void save_beliefs(const std::string& filename,
                  factor_graph& fgraph,
                  gl_types::graph& graph,
                  size_t num_variables) {
  // Open the file to store the final belief vectors
  std::ofstream fout(filename.c_str());
  assert(fout.good());
  // Save all the beliefs
  for(size_t i = 0; i < num_variables; ++i) {
    vertex_data& vdata = graph.vertex_data(i);
    vdata.belief.normalize();
    fout << fgraph.var_name(i) << " // ";
    // Save the normalized belief estimates to the file
    for(size_t j = 0; j < vdata.belief.size(); ++j) {
      fout << std::exp(vdata.belief.logP(j));
      if((j + 1) < vdata.belief.size()) fout << ", ";
    }
    fout << "\n";
  }
  fout.close();
} // end of save beliefs

// MAIN
// ============================================================================>
int main(int argc, char** argv) {
  std::cout << "This program solves the sidechain prediction task."
            << std::endl;

  // Parse command line arguments --------------------------------------------->
  bound = 1E-5; // <-- Defined globally
  damping = 0.3; // <-- Defined globally
  std::string network_filename;
  std::string beliefs_filename = "beliefs.txt";

  graphlab::command_line_options clopts("Run Loopy BP on an Alchemy Network");
  clopts.attach_option("graph",
                       &network_filename,
                       "The Alchemy factor graph file.");
  clopts.add_positional("graph");
  clopts.attach_option("bound",
                       &bound, bound,
                       "Residual termination bound");
  clopts.attach_option("damping",
                       &damping, damping,
                       "The amount of message damping (higher = more damping)");
  clopts.attach_option("beliefs",
                       &beliefs_filename, beliefs_filename,
                       "The file to save the belief predictions");
  clopts.set_scheduler_type("splash(splash_size=100)");
  clopts.set_scope_type("edge");

  // set the global logger
  // global_logger().set_log_level(LOG_WARNING);
  // global_logger().set_log_to_console(true);

  bool success = clopts.parse(argc, argv);
  if(!success && !clopts.is_set("graph")) {
    return EXIT_FAILURE;
  }

  gl_types::core core;
  core.set_engine_options(clopts);
  core.sched_options().add_option("update_function", bp_update);

  // Load the factor graph from file ------------------------------------------>
  std::cout << "Loading Factor Graph in Alchemy Format" << std::endl;
  factor_graph fgraph;
  fgraph.load_alchemy(network_filename);
  const size_t num_variables = fgraph.variables().size();
  const size_t num_factors = fgraph.factors().size();
  std::cout << "Finished!" << std::endl;

  // Build the BP graph from the factor graph---------------------------------->
  std::cout << "Building BP graph from the factor graph" << std::endl;
  make_bp_graph( fgraph, core.graph() );
  size_t num_vertices = core.graph().num_vertices();
  assert(num_vertices == num_variables + num_factors);
  size_t num_edges = core.graph().num_edges();
  std::cout << "Loaded: " << num_vertices << " vertices "
            << "and " << num_edges << " edges." << std::endl;
  std::cout << "Finished!" << std::endl;

  // Tell the scheduler that the bp_update function should be applied
  // to all vertices with priority:
  double initial_priority = 100.0;
  core.add_task_to_all(bp_update, initial_priority);

  // Running the engine ------------------------------------------------------->
  std::cout << "Running the engine. " << std::endl;
  // Run the engine (this blocks until their are no tasks left).
  // Convergence is determined when there are no update tasks left.
  const double runtime = core.start();
  std::cout << "Done!" << std::endl;

  // Print some fun facts------------------------------------------------------>
  size_t update_count = core.last_update_count();
  std::cout << "Finished Running engine in " << runtime
            << " seconds." << std::endl
            << "Total updates: " << update_count << std::endl
            << "Efficiency: " << (double(update_count) / runtime)
            << " updates per second "
            << std::endl;

  // Save the beliefs --------------------------------------------------------->
  std::cout << "Saving the beliefs. " << std::endl;
  save_beliefs(beliefs_filename, fgraph, core.graph(), num_variables);
  std::cout << "Finished saving beliefs." << std::endl;

  // // Save statistics about the run -------------------------------------------->
  // std::cout << "Saving the statistics file. " << std::endl;
  // std::ofstream stats(opts.stats_filename.c_str(), std::ios::app);
  // assert(stats.good());
  // stats << opts.scheduler << ", "
  //       << opts.network_filename << ", "
  //       << opts.scope << ", "
  //       << opts.ncpus << ", "
  //       << runtime << ", "
  //       << update_count << ", "
  //       << num_vertices << ", "
  //       << num_edges << ", "
  //       << num_variables << ", "
  //       << num_factors << ", "
  //       << opts.splash_size << std::endl;
  // stats.close();
  // std::cout << "Finished saving statistics file." << std::endl;

  return EXIT_SUCCESS;
} // end of main


#ifndef STAN_MATH_PRIM_PROB_CATEGORICAL_LOGIT_GLM_LPMF_HPP
#define STAN_MATH_PRIM_PROB_CATEGORICAL_LOGIT_GLM_LPMF_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <Eigen/Core>
#include <cmath>

namespace stan {
namespace math {

/** \ingroup multivar_dists
 * Returns the log PMF of the Generalized Linear Model (GLM)
 * with categorical distribution and logit (softmax) link function.
 *
 * @tparam T_y type of classes. It can be either `std::vector<int>` or `int`.
 * @tparam T_x_scalar type of a scalar in the matrix of independent variables
 * (features)
 * @tparam T_x_rows compile-time number of rows of `x`. It can be either
 * `Eigen::Dynamic` or 1.
 * @tparam T_alpha_scalar type of scalar in the intercept vector
 * @tparam T_beta_scalar type of a scalar in the matrix of weights
 * @param y a scalar or vector of classes. If it is a scalar it will be
 * broadcast - used for all instances. Values should be between 1 and number of
 * classes, including endpoints.
 * @param x design matrix or row vector. If it is a row vector it will be
 * broadcast - used for all instances.
 * @param alpha intercept vector (in log odds)
 * @param beta weight matrix
 * @return log probability or log sum of probabilities
 * @throw std::domain_error x, beta or alpha is infinite or y is not within
 * bounds
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <bool propto, typename T_y, typename T_x_scalar, int T_x_rows,
          typename T_alpha_scalar, typename T_beta_scalar>
return_type_t<T_x_scalar, T_alpha_scalar, T_beta_scalar>
categorical_logit_glm_lpmf(
    const T_y& y, const Eigen::Matrix<T_x_scalar, T_x_rows, Eigen::Dynamic>& x,
    const Eigen::Matrix<T_alpha_scalar, Eigen::Dynamic, 1>& alpha,
    const Eigen::Matrix<T_beta_scalar, Eigen::Dynamic, Eigen::Dynamic>& beta) {
  using T_partials_return
      = partials_return_t<T_x_scalar, T_alpha_scalar, T_beta_scalar>;
  static const char* function = "categorical_logit_glm_lpmf";

  using Eigen::Array;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using std::exp;
  using std::log;

  const size_t N_instances = T_x_rows == 1 ? size(y) : x.rows();
  const size_t N_attributes = x.cols();
  const size_t N_classes = beta.cols();

  check_consistent_size(function, "Vector of dependent variables", y,
                        N_instances);
  check_consistent_size(function, "Intercept vector", alpha, N_classes);
  check_size_match(function, "x.cols()", N_attributes, "beta.rows()",
                   beta.rows());
  check_bounded(function, "categorical outcome out of support", y, 1,
                N_classes);

  if (size_zero(y) || N_classes == 1) {
    return 0;
  }

  if (!include_summand<propto, T_x_scalar, T_alpha_scalar,
                       T_beta_scalar>::value) {
    return 0;
  }

  const auto& x_val = value_of_rec(x);
  const auto& beta_val = value_of_rec(beta);
  const auto& alpha_val = value_of_rec(alpha);

  const auto& alpha_val_vec = as_column_vector_or_scalar(alpha_val).transpose();

  Array<T_partials_return, T_x_rows, Dynamic> lin
      = (x_val * beta_val).rowwise() + alpha_val_vec;
  Array<T_partials_return, T_x_rows, 1> lin_max
      = lin.rowwise().maxCoeff();  // This is used to prevent overflow when
                                   // calculating softmax/log_sum_exp and
                                   // similar expressions
  Array<T_partials_return, T_x_rows, Dynamic> exp_lin
      = exp(lin.colwise() - lin_max);
  Array<T_partials_return, T_x_rows, 1> inv_sum_exp_lin
      = 1 / exp_lin.rowwise().sum();

  T_partials_return logp = log(inv_sum_exp_lin).sum() - lin_max.sum();
  if (T_x_rows == 1) {
    logp *= N_instances;
  }
  scalar_seq_view<T_y> y_seq(y);
  for (int i = 0; i < N_instances; i++) {
    if (T_x_rows == 1) {
      logp += lin(0, y_seq[i] - 1);
    } else {
      logp += lin(i, y_seq[i] - 1);
    }
  }
  // TODO(Tadej) maybe we can replace previous block with the following line
  // when we have newer Eigen  T_partials_return logp =
  // lin(Eigen::all,y-1).sum() + log(inv_sum_exp_lin).sum() - lin_max.sum();

  if (!std::isfinite(logp)) {
    check_finite(function, "Weight vector", beta);
    check_finite(function, "Intercept", alpha);
    check_finite(function, "Matrix of independent variables", x);
  }

  // Compute the derivatives.
  operands_and_partials<Matrix<T_x_scalar, T_x_rows, Dynamic>,
                        Matrix<T_alpha_scalar, Dynamic, 1>,
                        Matrix<T_beta_scalar, Dynamic, Dynamic>>
      ops_partials(x, alpha, beta);

  if (!is_constant_all<T_x_scalar>::value) {
    if (T_x_rows == 1) {
      Array<double, 1, Dynamic> beta_y = beta_val.col(y_seq[0] - 1);
      for (int i = 1; i < N_instances; i++) {
        beta_y += beta_val.col(y_seq[i] - 1).array();
      }
      ops_partials.edge1_.partials_
          = beta_y
            - (exp_lin.matrix() * beta_val.transpose()).array().colwise()
                  * inv_sum_exp_lin * N_instances;
    } else {
      Array<double, Dynamic, Dynamic> beta_y(N_instances, N_attributes);
      for (int i = 0; i < N_instances; i++) {
        beta_y.row(i) = beta_val.col(y_seq[i] - 1);
      }
      ops_partials.edge1_.partials_
          = beta_y
            - (exp_lin.matrix() * beta_val.transpose()).array().colwise()
                  * inv_sum_exp_lin;
      // TODO(Tadej) maybe we can replace previous block with the following line
      // when we have newer Eigen  ops_partials.edge1_.partials_ = beta_val(y -
      // 1, all) - (exp_lin.matrix() * beta.transpose()).colwise() *
      // inv_sum_exp_lin;
    }
  }
  if (!is_constant_all<T_alpha_scalar, T_beta_scalar>::value) {
    Array<T_partials_return, T_x_rows, Dynamic> neg_softmax_lin
        = exp_lin.colwise() * -inv_sum_exp_lin;
    if (!is_constant_all<T_alpha_scalar>::value) {
      if (T_x_rows == 1) {
        ops_partials.edge2_.partials_
            = neg_softmax_lin.colwise().sum() * N_instances;
      } else {
        ops_partials.edge2_.partials_ = neg_softmax_lin.colwise().sum();
      }
      for (int i = 0; i < N_instances; i++) {
        ops_partials.edge2_.partials_[y_seq[i] - 1] += 1;
      }
    }
    if (!is_constant_all<T_beta_scalar>::value) {
      Matrix<T_partials_return, Dynamic, Dynamic> beta_derivative;
      if (T_x_rows == 1) {
        beta_derivative
            = x_val.transpose() * neg_softmax_lin.matrix() * N_instances;
      } else {
        beta_derivative = x_val.transpose() * neg_softmax_lin.matrix();
      }

      for (int i = 0; i < N_instances; i++) {
        if (T_x_rows == 1) {
          beta_derivative.col(y_seq[i] - 1) += x_val;
        } else {
          beta_derivative.col(y_seq[i] - 1) += x_val.row(i);
        }
      }
      // TODO(Tadej) maybe we can replace previous loop with the following line
      // when we have newer Eigen  ops_partials.edge3_.partials_(Eigen::all, y -
      // 1) += x_val.colwise.sum().transpose();

      ops_partials.edge3_.partials_ = std::move(beta_derivative);
    }
  }
  return ops_partials.build(logp);
}

template <typename T_y, typename T_x_scalar, int T_x_rows,
          typename T_alpha_scalar, typename T_beta_scalar>
return_type_t<T_x_scalar, T_alpha_scalar, T_beta_scalar>
categorical_logit_glm_lpmf(
    const T_y& y, const Eigen::Matrix<T_x_scalar, T_x_rows, Eigen::Dynamic>& x,
    const Eigen::Matrix<T_alpha_scalar, Eigen::Dynamic, 1>& alpha,
    const Eigen::Matrix<T_beta_scalar, Eigen::Dynamic, Eigen::Dynamic>& beta) {
  return categorical_logit_glm_lpmf<false>(y, x, alpha, beta);
}

}  // namespace math
}  // namespace stan

#endif


// Volumetric3D_bubble.cpp
// created by Kuangdai on 19-Oct-2016
// a bubble-shaped heterogeneity

#include "Volumetric3D_bubble.h"
#include "Parameters.h"
#include "Geodesy.h"
#include <boost/algorithm/string.hpp>
#include <sstream>

void Volumetric3D_bubble::initialize(const std::vector<std::string> &params) {
    // need at least 7 parameters to make a bubble
    if (params.size() < 7) {
        throw std::runtime_error("Volumetric3D_bubble::initialize || "
            "Not enough parameters for a bubble-shaped heterogeneity. Need 7 at least.");
    }

    const std::string source = "Volumetric3D_bubble::initialize";

    // property name
    bool found = false;
    for (int i = 0; i < Volumetric3D::MaterialPropertyString.size(); i++) {
        if (boost::iequals(params[0], Volumetric3D::MaterialPropertyString[i])) {
            mMaterialProp = Volumetric3D::MaterialProperty(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_bubble::initialize || "
            "Unknown material property, name = " + params[0]);
    }

    // reference type
    found = false;
    for (int i = 0; i < Volumetric3D::MaterialRefTypeString.size(); i++) {
        if (boost::iequals(params[1], Volumetric3D::MaterialRefTypeString[i]) ||
            boost::iequals(params[1], Volumetric3D::MaterialRefTypeStringShort[i])) {
            mReferenceType = Volumetric3D::MaterialRefType(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_bubble::initialize || "
            "Unknown material reference type, type = " + params[1]);
    }

    // value inside
    Parameters::castValue(mValueInside, params[2], source);

    // radius
    Parameters::castValue(mRadius, params[3], source); mRadius *= 1e3;

    // location
    Parameters::castValue(mDepth, params[4], source); mDepth *= 1e3;
    Parameters::castValue(mLat, params[5], source);
    Parameters::castValue(mLon, params[6], source);

    // optional
    try {
        int ipar = 7;
        Parameters::castValue(mSourceCentered, params.at(ipar++), source);
        Parameters::castValue(mFluid, params.at(ipar++), source);
        Parameters::castValue(mHWHM, params.at(ipar++), source); mHWHM *= 1e3;
    } catch (std::out_of_range) {
        // nothing
    }

    // compute xyz of endpoints and length
    RDCol3 rtpBubble;
    if (mSourceCentered) {
        RDCol3 rtpBubbleSrc;
        rtpBubbleSrc(0) = Geodesy::getROuter() - mDepth;
        rtpBubbleSrc(1) = mLat * degree;
        rtpBubbleSrc(2) = mLon * degree;
        rtpBubble = Geodesy::rotateSrc2Glob(rtpBubbleSrc, mSrcLat, mSrcLon, mSrcDep);
    } else {
        rtpBubble(0) = Geodesy::getROuter() - mDepth;
        rtpBubble(1) = Geodesy::lat2Theta_d(mLat, mDepth);
        rtpBubble(2) = Geodesy::lon2Phi(mLon);
    }
    mXyzBubble = Geodesy::toCartesian(rtpBubble);

    // use 20% of radius for HWHM if not specified
    if (mHWHM < 0.) {
        mHWHM = mRadius * .2;
    }

    // for Absolute models
    if (mReferenceType == Volumetric3D::MaterialRefType::Absolute) {
        // decay is not allowed
        mHWHM = 0.;
        // convert to SI
        mValueInside *= MaterialPropertyAbsSI[mMaterialProp];
    }
}

bool Volumetric3D_bubble::get3dProperties(double r, double theta, double phi, double rElemCenter,
    std::vector<MaterialProperty> &properties,
    std::vector<MaterialRefType> &refTypes,
    std::vector<double> &values) const {

    // header
    properties = std::vector<MaterialProperty>(1, mMaterialProp);
    refTypes = std::vector<MaterialRefType>(1, mReferenceType);
    values = std::vector<double>(1, 0.);

    // distance from point to axis
    RDCol3 rtpTarget;
    rtpTarget(0) = r;
    rtpTarget(1) = theta;
    rtpTarget(2) = phi;
    const RDCol3 &xyzTarget = Geodesy::toCartesian(rtpTarget);
    double distance = (mXyzBubble - xyzTarget).norm();

    // treat as center if inside bubble
    distance -= mRadius;
    if (distance < 0.) {
        distance = 0.;
    }

    // outside range
    if (distance > 4. * mHWHM) {
        return false;
    }

    // compute Gaussian
    double stddev = mHWHM / sqrt(2. * log(2.));
    double gaussian = mValueInside * exp(-distance * distance / (stddev * stddev * 2.));

    // set perturbations
    values[0] = gaussian;
    return true;
}

std::string Volumetric3D_bubble::verbose() const {
    std::stringstream ss;
    ss << "\n======================= 3D Volumetric ======================" << std::endl;
    ss << "  Model Name           =   bubble" << std::endl;
    ss << "  Material Property    =   " << MaterialPropertyString[mMaterialProp] << std::endl;
    ss << "  Reference Type       =   " << MaterialRefTypeString[mReferenceType] << std::endl;
    if (mReferenceType == Volumetric3D::MaterialRefType::Absolute) {
        ss << "  Value Inside         =   " << mValueInside / MaterialPropertyAbsSI[mMaterialProp] << std::endl;
    } else {
        ss << "  Value Inside         =   " << mValueInside << std::endl;
    }
    ss << "  Bubble Radius / km   =   " << mRadius / 1e3 << std::endl;
    ss << "  Depth / km           =   " << mDepth / 1e3 << std::endl;
    ss << "  Lat or Theta / deg   =   " << mLat << std::endl;
    ss << "  Lon or Phi / deg     =   " << mLon << std::endl;
    ss << "  Source-centered      =   " << (mSourceCentered ? "YES" : "NO") << std::endl;
    ss << "  HWHM / km            =   " << mHWHM / 1e3 << std::endl;
    ss << "======================= 3D Volumetric ======================\n" << std::endl;
    return ss.str();
}