///////////////////////////////////////////////////////////////////////////////
// tail_mean.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under 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_ACCUMULATORS_STATISTICS_TAIL_MEAN_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TAIL_MEAN_HPP_DE_01_01_2006

#include <numeric>
#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{

    ///////////////////////////////////////////////////////////////////////////////
    // coherent_tail_mean_impl
    //
    /**
        @brief Estimation of the coherent tail mean based on order statistics (for both left and right tails)

        The coherent tail mean \f$\widehat{CTM}_{n,\alpha}(X)\f$ is equal to the non-coherent tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$
        plus a correction term that ensures coherence in case of non-continuous distributions.

        \f[
            \widehat{CTM}_{n,\alpha}^{\mathrm{right}}(X) = \widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X) +
            \frac{1}{\lceil n(1-\alpha)\rceil}\hat{q}_{n,\alpha}(X)\left(1 - \alpha - \frac{1}{n}\lceil n(1-\alpha)\rceil \right)
        \f]

        \f[
            \widehat{CTM}_{n,\alpha}^{\mathrm{left}}(X) = \widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X) +
            \frac{1}{\lceil n\alpha\rceil}\hat{q}_{n,\alpha}(X)\left(\alpha - \frac{1}{n}\lceil n\alpha\rceil \right)
        \f]
    */
    template<typename Sample, typename LeftRight>
    struct coherent_tail_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef float_type result_type;

        coherent_tail_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            extractor<tag::non_coherent_tail_mean<LeftRight> > const some_non_coherent_tail_mean = {};

            return some_non_coherent_tail_mean(args)
                 + numeric::fdiv(quantile(args), n)
                 * (
                     ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability]
                     - numeric::fdiv(n, count(args))
                   );
        }

        // serialization is done by accumulators it depends on
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version) {}
    };

    ///////////////////////////////////////////////////////////////////////////////
    // non_coherent_tail_mean_impl
    //
    /**
        @brief Estimation of the (non-coherent) tail mean based on order statistics (for both left and right tails)

        An estimation of the non-coherent tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$ is given by the mean of the
        \f$\lceil n\alpha\rceil\f$ smallest samples (left tail) or the mean of the  \f$\lceil n(1-\alpha)\rceil\f$
        largest samples (right tail), \f$n\f$ being the total number of samples and \f$\alpha\f$ the quantile level:

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X) = \frac{1}{\lceil n(1-\alpha)\rceil} \sum_{i=\lceil \alpha n \rceil}^n X_{i:n}
        \f]

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X) = \frac{1}{\lceil n\alpha\rceil} \sum_{i=1}^{\lceil \alpha n \rceil} X_{i:n}
        \f]

        It thus requires the caching of at least the \f$\lceil n\alpha\rceil\f$ smallest or the \f$\lceil n(1-\alpha)\rceil\f$
        largest samples.

        @param quantile_probability
    */
    template<typename Sample, typename LeftRight>
    struct non_coherent_tail_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef float_type result_type;

        non_coherent_tail_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            // If n is in a valid range, return result, otherwise return NaN or throw exception
            if (n <= static_cast<std::size_t>(tail(args).size()))
                return numeric::fdiv(
                    std::accumulate(
                        tail(args).begin()
                      , tail(args).begin() + n
                      , Sample(0)
                    )
                  , n
                );
            else
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }
            }
        }

        // serialization is done by accumulators it depends on
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version) {}
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::coherent_tail_mean<>
// tag::non_coherent_tail_mean<>
//
namespace tag
{
    template<typename LeftRight>
    struct coherent_tail_mean
      : depends_on<count, quantile, non_coherent_tail_mean<LeftRight> >
    {
        typedef accumulators::impl::coherent_tail_mean_impl<mpl::_1, LeftRight> impl;
    };

    template<typename LeftRight>
    struct non_coherent_tail_mean
      : depends_on<count, tail<LeftRight> >
    {
        typedef accumulators::impl::non_coherent_tail_mean_impl<mpl::_1, LeftRight> impl;
    };

    struct abstract_non_coherent_tail_mean
      : depends_on<>
    {
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::non_coherent_tail_mean;
// extract::coherent_tail_mean;
//
namespace extract
{
    extractor<tag::abstract_non_coherent_tail_mean> const non_coherent_tail_mean = {};
    extractor<tag::tail_mean> const coherent_tail_mean = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(non_coherent_tail_mean)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(coherent_tail_mean)
}

using extract::non_coherent_tail_mean;
using extract::coherent_tail_mean;

// for the purposes of feature-based dependency resolution,
// coherent_tail_mean<LeftRight> provides the same feature as tail_mean
template<typename LeftRight>
struct feature_of<tag::coherent_tail_mean<LeftRight> >
  : feature_of<tag::tail_mean>
{
};

template<typename LeftRight>
struct feature_of<tag::non_coherent_tail_mean<LeftRight> >
  : feature_of<tag::abstract_non_coherent_tail_mean>
{
};

// So that non_coherent_tail_mean can be automatically substituted
// with weighted_non_coherent_tail_mean when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::non_coherent_tail_mean<LeftRight> >
{
    typedef tag::non_coherent_weighted_tail_mean<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::non_coherent_weighted_tail_mean<LeftRight> >
  : feature_of<tag::non_coherent_tail_mean<LeftRight> >
{};

// NOTE that non_coherent_tail_mean cannot be feature-grouped with tail_mean,
// which is the base feature for coherent tail means, since (at least for
// non-continuous distributions) non_coherent_tail_mean is a different measure!

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


/*
 * cg.cpp
 *
 *  Created on: Feb 22, 2013
 *      Author: Joao Cunha <joao.cunha@ua.pt>
 */

#include "cg.h"

#include <iostream>

#include <Eigen/Core>

using namespace std;

namespace libgp
{

CG::CG()
{
}

CG::~CG()
{
}

void CG::maximize(GaussianProcess* gp, size_t n, bool verbose)
{
	const double INT = 0.1; // don't reevaluate within 0.1 of the limit of the current bracket
	const double EXT = 3.0; // extrapolate maximum 3 times the current step-size
	const int MAX = 20;		// max 20 function evaluations per line search
	const double RATIO = 10;	// maximum allowed slope ratio
	const double SIG = 0.1, RHO = SIG/2;
	/* SIG and RHO are the constants controlling the Wolfe-
	   Powell conditions. SIG is the maximum allowed absolute ratio between
	   previous and new slopes (derivatives in the search direction), thus setting
	   SIG to low (positive) values forces higher precision in the line-searches.
	   RHO is the minimum allowed fraction of the expected (from the slope at the
	   initial point in the linesearch). Constants must satisfy 0 < RHO < SIG < 1.
	   Tuning of SIG (depending on the nature of the function to be optimized) may
	  speed up the minimization; it is probably not worth playing much with RHO.
	*/

	/* The code falls naturally into 3 parts, after the initial line search is
	   started in the direction of steepest descent. 1) we first enter a while loop
	   which uses point 1 (p1) and (p2) to compute an extrapolation (p3), until we
	   have extrapolated far enough (Wolfe-Powell conditions). 2) if necessary, we
	   enter the second loop which takes p2, p3 and p4 chooses the subinterval
	   containing a (local) minimum, and interpolates it, unil an acceptable point
	   is found (Wolfe-Powell conditions). Note, that points are always maintained
	   in order p0 <= p1 <= p2 < p3 < p4. 3) compute a new search direction using
	   conjugate gradients (Polack-Ribiere flavour), or revert to steepest if there
	   was a problem in the previous line-search. Return the best value so far, if
	   two consecutive line-searches fail, or whenever we run out of function
	   evaluations or line-searches. During extrapolation, the "f" function may fail
	   either with an error or returning Nan or Inf, and maxmize should handle this
	   gracefully.
	*/

	bool ls_failed = false;									//prev line-search failed
	double f0 = -gp->log_likelihood();						//initial negative marginal log likelihood
	Eigen::VectorXd df0 = -gp->log_likelihood_gradient();	//initial gradient
	Eigen::VectorXd X = gp->covf().get_loghyper();			//hyper parameters

	if(verbose) cout << f0 << endl;

	Eigen::VectorXd s = -df0;								//initial search direction
	double d0 = -s.dot(s);									//initial slope
	double x3 = 1/(1-d0);

	double f3 = 0;
	double d3 = 0;
	Eigen::VectorXd df3 = df0;

	double x2 = 0, x4 = 0;
	double f2 = 0, f4 = 0;
	double d2 = 0, d4 = 0;

	for (unsigned int i = 0; i < n; ++i)
	{
		//copy current values
		Eigen::VectorXd X0 = X;
		double F0 = f0;
		Eigen::VectorXd dF0 = df0;
		unsigned int M = min(MAX, (int)(n-i));

		while(1)											//keep extrapolating until necessary
		{
			x2 = 0;
			f2 = f0;
			d2 = d0;
			f3 = f0;
			df3 = df0;
			double success = false;

			while( !success && M>0)
			{
				M --;
				i++;
				gp->covf().set_loghyper(X+s*x3);
				f3 = -gp->log_likelihood();
				df3 = -gp->log_likelihood_gradient();

				if(verbose) cout << f3 << endl;

				bool nanFound = false;
				//test NaN and Inf's
				for (int j = 0; j < df3.rows(); ++j)
				{
					if(isnan(df3(j)))
					{
						nanFound = true;
						break;
					}
				}
				if(!isnan(f3) && !isinf(f3) && !nanFound)
					success = true;
				else
				{
					x3 = (x2+x3)/2; 						// if fail, bissect and try again
				}
			}
			//keep best values
			if(f3 < F0)
			{
				X0 = X+s*x3;
				F0 = f3;
				dF0 = df3;
			}

			d3 = df3.dot(s);								// new slope

			if( (d3 > SIG*d0) || (f3 >  f0+x3*RHO*d0) || M == 0) // are we done extrapolating?
			{
				break;
			}

			double x1 = x2; double f1 = f2; double d1 = d2;	// move point 2 to point 1
			x2 = x3; f2 = f3; d2 = d3;						// move point 3 to point 2
			double A = 6*(f1-f2) + 3*(d2+d1)*(x2-x1);				// make cubic extrapolation
			double B = 3*(f2-f1) - (2*d1+d2)*(x2-x1);
			x3 = x1-d1*(x2-x1)*(x2-x1)/(B+sqrt(B*B -A*d1*(x2-x1)));
			if(isnan(x3) || x3 < 0 || x3 > x2*EXT)			// num prob | wrong sign | beyond extrapolation limit
				x3 = EXT*x2;
			else if(x3 < x2+INT*(x2-x1))					// too close to previous point
				x3 = x2+INT*(x2-x1);
		}

		while( ( (abs(d3) > -SIG*d0) || (f3 > f0+x3*RHO*d0) ) && (M > 0))	// keep interpolating
		{
			if( (d3 > 0) || (f3 > f0+x3*RHO*d0) )			// choose subinterval
			{												// move point 3 to point 4
				x4 = x3;
				f4 = f3;
				d4 = d3;
			}
			else
			{
				x2 = x3;									//move point 3 to point 2
				f2 = f3;
				d2 = d3;
			}

			if(f4 > f0)
				x3 = x2 - (0.5*d2*(x4-x2)*(x4-x2))/(f4-f2-d2*(x4-x2));	// quadratic interpolation
			else
			{
				double A = 6*(f2-f4)/(x4-x2)+3*(d4+d2);
				double B = 3*(f4-f2)-(2*d2+d4)*(x4-x2);
				x3 = x2+sqrt(B*B-A*d2*(x4-x2)*(x4-x2) -B)/A;
			}

			if(isnan(x3) || isinf(x3))
				x3 = (x2+x4)/2;

			x3 = std::max(std::min(x3, x4-INT*(x4-x2)), x2+INT*(x4-x2));

			gp->covf().set_loghyper(X+s*x3);
			f3 = -gp->log_likelihood();
			df3 = -gp->log_likelihood_gradient();

			if(f3 < F0)												// keep best values
			{
				X0 = X+s*x3;
				F0 = f3;
				dF0 = df3;
			}

			if(verbose) cout << F0 << endl;

			M--;
			i++;
			d3 = df3.dot(s);										// new slope
		}

		if( (abs(d3) < -SIG*d0) && (f3 < f0+x3*RHO*d0))
		{
			X = X+s*x3;
			f0 = f3;
			s = (df3.dot(df3)-df0.dot(df3)) / (df0.dot(df0))*s - df3;	// Polack-Ribiere CG direction
			df0 = df3;													// swap derivatives
			d3 = d0; d0 = df0.dot(s);
			if(verbose) cout << f0 << endl;
			if(d0 > 0)													// new slope must be negative
			{															// otherwise use steepest direction
				s = -df0;
				d0 = -s.dot(s);
			}

			x3 = x3 * std::min(RATIO, d3/(d0-std::numeric_limits< double >::min()));	// slope ratio but max RATIO
			ls_failed = false;																// this line search did not fail
		}
		else
		{														// restore best point so far
			X = X0;
			f0 = F0;
			df0 = dF0;

			if(verbose) cout << f0 << endl;

			if(ls_failed || i >= n)								// line search failed twice in a row
				break;											// or we ran out of time, so we give up

			s = -df0;
			d0 = -s.dot(s);										// try steepest
			x3 = 1/(1-d0);
			ls_failed = true;									// this line search failed
		}

	}
	gp->covf().set_loghyper(X);
}

}


#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

}


#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");
}


#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


// 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


#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();
}


#include <alglib/ap.h>
#include <alglib/interpolation.h>

#include "spline-2d.hpp"

#define This Spline2d

namespace perceive
{
using std::cout;
using std::endl;

// ----------------------------------------------------------------------- Pimpl

class This::Pimpl
{
 public:
   Pimpl()
       : is_init(false)
       , n_control_points(0)
       , approx_length(dNAN)
   {}

   Vector2 range_hint{0.0, 1.0};
   bool is_init;
   uint n_control_points;
   double approx_length;
   alglib::spline1dinterpolant spline_x;
   alglib::spline1dinterpolant spline_y;
   alglib::spline1dfitreport rep_x;
   alglib::spline1dfitreport rep_y;
};

// ---------------------------------------------------------------- Construction

This::Spline2d()
    : pimpl(make_unique<Pimpl>())
{}

This::Spline2d(const Spline2d& rhs)
    : Spline2d()
{
   *this = rhs;
}
This::Spline2d(Spline2d&& rhs)
    : pimpl{nullptr}
{
   *this = std::move(rhs);
}

This::Spline2d(const std::vector<Vector2>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::vector<Vector2r>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::deque<Vector2>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::deque<Vector2r>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}

This::~Spline2d() = default;

Spline2d& This::operator=(const Spline2d& rhs)
{
   if(this != &rhs) *pimpl = *rhs.pimpl;
   return *this;
}

Spline2d& This::operator=(Spline2d&& rhs) = default;

// ------------------------------------------------------------------- Accessors

bool This::is_init() const { return pimpl->is_init; }
uint This::n_control_points() const { return pimpl->n_control_points; }
double This::approx_length() const { return pimpl->approx_length; }

double This::rms_error_x() const { return pimpl->rep_x.rmserror; }
double This::avg_error_x() const { return pimpl->rep_x.avgerror; }
double This::avg_rel_error_x() const { return pimpl->rep_x.avgrelerror; }
double This::max_error_x() const { return pimpl->rep_x.maxerror; }

double This::rms_error_y() const { return pimpl->rep_y.rmserror; }
double This::avg_error_y() const { return pimpl->rep_y.avgerror; }
double This::avg_rel_error_y() const { return pimpl->rep_y.avgrelerror; }
double This::max_error_y() const { return pimpl->rep_y.maxerror; }

Vector2& This::range_hint() { return pimpl->range_hint; }
const Vector2& This::range_hint() const { return pimpl->range_hint; }

// -------------------------------------------------------------------- p-spline

/*************************************************************************
Fitting by penalized cubic spline.

Equidistant grid with M nodes on [min(x,xc),max(x,xc)] is  used  to  build
basis functions. Basis functions are cubic splines with  natural  boundary
conditions. Problem is regularized by  adding non-linearity penalty to the
usual least squares penalty function:

    S(x) = arg min { LS + P }, where
    LS   = SUM { w[i]^2*(y[i] - S(x[i]))^2 } - least squares penalty
    P    = C*10^rho*integral{ S''(x)^2*dx } - non-linearity penalty
    rho  - tunable constant given by user
    C    - automatically determined scale parameter,
           makes penalty invariant with respect to scaling of X, Y, W.

INPUT PARAMETERS:
    X   -   points, array[0..N-1].
    Y   -   function values, array[0..N-1].
    N   -   number of points (optional):
            * N>0
            * if given, only first N elements of X/Y are processed
            * if not given, automatically determined from X/Y sizes
    M   -   number of basis functions ( = number_of_nodes), M>=4.
    Rho -   regularization  constant  passed   by   user.   It   penalizes
            nonlinearity in the regression spline. It  is  logarithmically
            scaled,  i.e.  actual  value  of  regularization  constant  is
            calculated as 10^Rho. It is automatically scaled so that:
            * Rho=2.0 corresponds to moderate amount of nonlinearity
            * generally, it should be somewhere in the [-8.0,+8.0]
            If you do not want to penalize nonlineary,
            pass small Rho. Values as low as -15 should work.

OUTPUT PARAMETERS:
    Info-   same format as in LSFitLinearWC() subroutine.
            * Info>0    task is solved
            * Info<=0   an error occured:
                        -4 means inconvergence of internal SVD or
                           Cholesky decomposition; problem may be
                           too ill-conditioned (very rare)
    S   -   spline interpolant.
    Rep -   Following fields are set:
            * RMSError      rms error on the (X,Y).
            * AvgError      average error on the (X,Y).
            * AvgRelError   average relative error on the non-zero Y
            * MaxError      maximum error
                            NON-WEIGHTED ERRORS ARE CALCULATED

IMPORTANT:
    this subroitine doesn't calculate task's condition number for K<>0.

NOTE 1: additional nodes are added to the spline outside  of  the  fitting
interval to force linearity when x<min(x,xc) or x>max(x,xc).  It  is  done
for consistency - we penalize non-linearity  at [min(x,xc),max(x,xc)],  so
it is natural to force linearity outside of this interval.

NOTE 2: function automatically sorts points,  so  caller may pass unsorted
array.

  -- ALGLIB PROJECT --
     Copyright 18.08.2009 by Bochkanov Sergey
*************************************************************************/

template<typename U>
bool init_pspline(Spline2d& curve,
                  const U& container,
                  double rho,
                  unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;
   curve.pimpl->range_hint       = Vector2(0.0, 1.0);

   const uint32_t len = uint32_t(container.size());
   if(len < 5) return false;

   alglib::real_1d_array T;
   alglib::real_1d_array X;
   alglib::real_1d_array Y;
   auto extra = 0u;
   T.setlength(len + extra);
   X.setlength(len + extra);
   Y.setlength(len + extra);

   uint counter = 0;
   double t     = 0.0;
   double dt    = 1.0 / double(len - 1);
   for(const auto& p : container) {
      T[counter] = t;
      X[counter] = p(0);
      Y[counter] = p(1);
      t += dt;
      counter++;
   }

   // Add the last point in five more times... to get boundary
   for(auto i = 0u; i < extra; ++i) {
      T[len + i] = 1.0;
      X[len + i] = container.back()(0);
      Y[len + i] = container.back()(1);
   }

   alglib::ae_int_t info_x, info_y;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       T, X, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);
   alglib::spline1dfitpenalized(
       T, Y, M, rho, info_y, curve.pimpl->spline_y, curve.pimpl->rep_y);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0 || info_y > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      double dt                  = 0.1 / double(M - 1);
      Vector2 p                  = curve.evaluate(0.0);
      for(double t = dt; t <= 1.0; t += dt) {
         Vector2 q = curve.evaluate(t);
         curve.pimpl->approx_length += (p - q).norm();
         p = q;
      }
   }

   return curve.pimpl->is_init;
}

// ------------------------------------------------------------------------ Init

bool This::init(const std::vector<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<Vector2r>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2r>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

// -------------------------------------------------------------------- Evaluate

Vector2 This::evaluate(double t) const
{
   // assert(t >= 0.0 && t <= 1.0);

   return is_init() ? Vector2(alglib::spline1dcalc(pimpl->spline_x, t),
                              alglib::spline1dcalc(pimpl->spline_y, t))
                    : Vector2::nan();
}

This::GradientResult This::gradient(double t) const
{
   GradientResult x;

   if(is_init()) {
      double s, ds, ds2;
      double r, dr, dr2;
      alglib::spline1ddiff(pimpl->spline_x, t, s, ds, ds2);
      alglib::spline1ddiff(pimpl->spline_y, t, r, dr, dr2);
      x.x(0) = s;
      x.x(1) = r;
      x.g(0) = -dr;
      x.g(1) = ds;
      x.g.normalise();
   } else {
      x.x = x.g = Vector2::nan();
   }

   return x;
}

// ---------------------------------------------------------------------- Unpack

std::vector<array<double, 12>> This::unpack() const
{
   alglib::ae_int_t nx{0};
   alglib::ae_int_t ny{0};
   alglib::real_2d_array tbl_x, tbl_y;
   spline1dunpack(pimpl->spline_x, nx, tbl_x);
   spline1dunpack(pimpl->spline_y, ny, tbl_y);

   if(nx != ny)
      FATAL(format("How did this happen? nx = {} != {} = ny", nx, ny));

   auto N = tbl_x.rows();

   // INFO(format("nx = {}, len = ({}, {})", nx, tbl_x.rows(),tbl_x.cols()));

   std::vector<array<double, 12>> out;
   out.resize(size_t(N));
   for(alglib::ae_int_t ind = 0; ind < N; ++ind) {
      auto& coeff = out[size_t(ind)];
      auto ptr    = &coeff[0];
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_x(ind, i);
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_y(ind, i);
   }

   return out;
}

void This::init(const std::vector<array<double, 12>>& coefficients) const
{
   using alglib::spline1dinterpolant;

   auto make_interpolant = [&](bool is_x, spline1dinterpolant& interpolant) {
      unsigned offset = (is_x) ? 0 : 6;

      const unsigned dilate   = 1; // number of point per cooef set
      const double dilate_inv = 1.0 / double(dilate);
      const unsigned n        = unsigned(coefficients.size() * dilate + 1);
      alglib::real_1d_array x, y, d; // (x, f(x), f'(x))
      x.setlength(n);
      y.setlength(n);
      d.setlength(n);
      unsigned pos = 0; // write position

      auto write = [&](double t,
                       double X0,
                       double X1,
                       double C0,
                       double C1,
                       double C2,
                       double C3) {
         x(pos) = X0 + t * (X1 - X0);
         y(pos) = C0 + C1 * t + C2 * t * t + C3 * t * t * t;
         d(pos) = C1 + 2.0 * C2 * t + 3.0 * C3 * t * t;
         pos++;
      };

      for(unsigned ind = 0; ind < coefficients.size(); ++ind) {
         const auto& CC = coefficients[ind];
         auto X0        = CC[0 + offset];
         auto X1        = CC[1 + offset];
         auto C0        = CC[2 + offset];
         auto C1        = CC[3 + offset];
         auto C2        = CC[4 + offset];
         auto C3        = CC[5 + offset];
         for(unsigned i = 0; i < dilate; ++i)
            write(dilate_inv * double(i), X0, X1, C0, C1, C2, C3);

         // Write that final coefficient
         if(ind == coefficients.size() - 1) write(1.0, X0, X1, C0, C1, C2, C3);
      }

      assert(pos == n);

      alglib::spline1dbuildhermite(x, y, d, interpolant);
   };

   spline1dinterpolant spline_x, spline_y;
   make_interpolant(true, spline_x);
   make_interpolant(false, spline_y);

   pimpl->spline_x         = spline_x;
   pimpl->spline_y         = spline_y;
   pimpl->is_init          = true;
   pimpl->n_control_points = unsigned(coefficients.size());
   pimpl->rep_x.rmserror = pimpl->rep_y.rmserror = 0.0;
   pimpl->rep_x.avgerror = pimpl->rep_y.avgerror = 0.0;
   pimpl->rep_x.maxerror = pimpl->rep_y.maxerror = 0.0;
   pimpl->rep_x.avgrelerror = pimpl->rep_y.avgrelerror = 0.0;
}

// ----------------------------------------------------------------- Interpolate

void This::interpolate(unsigned n,
                       std::vector<Vector2>& out,
                       double min_t,
                       double max_t)
{
   assert(min_t >= 0.0 && min_t <= 1.0);
   assert(max_t >= 0.0 && max_t <= 1.0);
   assert(min_t <= max_t);

   out.reserve(n);

   if(n == 0)
      return;
   else if(n == 1)
      out.push_back(evaluate(max_t));
   else {
      double t  = min_t;
      double dt = (max_t - min_t) / double(n - 1);
      for(unsigned i = 0; i < n - 1; ++i) {
         out.push_back(evaluate(clamp(t, min_t, max_t)));
         t += dt;
      }
      out.push_back(evaluate(max_t));
   }

   assert(out.size() == n);
}

// ---------------------------------------------------------------------- Smooth

bool This::smooth(std::vector<Vector2>& path, double rho, int M_factor)
{
   if(path.size() < 5) return false;
   Spline2d spline;
   if(!spline.init(path, rho, M_factor)) return false;
   std::vector<Vector2> t;
   spline.interpolate(unsigned(path.size()), t);
   if(!(t.size() == path.size()))
      FATAL(format("Container size mismatch: expected {}, but got {}",
                   path.size(),
                   t.size()));
   auto src = t.begin();
   auto dst = path.begin();
   while(src != t.end()) *src++ = *dst++;
   return true;
}

// ---------------------------------------------------------------------- Find-t

double find_t(const Spline2d& spline,
              double y_value,
              double start_t,
              bool forward,
              double epsilon)
{
   if(!spline.is_init()) return double(NAN);

   uint n_cps = spline.n_control_points();
   assert(n_cps > 0);
   assert(start_t >= 0 && start_t <= 1.0);
   double step = (forward ? 1.0 : -1.0) * 0.5 / double(n_cps);

   double last_t  = start_t;
   Vector2 last_v = spline.evaluate(start_t);
   double next_t  = last_t + step;
   if(next_t <= 0.0) next_t = 0.0;
   if(next_t >= 1.0) next_t = 1.0;
   Vector2 next_v = spline.evaluate(next_t);

   auto is_between = [&]() {
      return (last_v.y < next_v.y)
                 ? inclusive_between<double>(last_v.y, y_value, next_v.y)
                 : inclusive_between<double>(next_v.y, y_value, last_v.y);
   };

   while(inclusive_between(0.0, next_t, 1.0) && !is_between()) {
      last_v = next_v;
      last_t = next_t;
      next_t += step;
      if(inclusive_between(0.0, next_t, 1.0)) next_v = spline.evaluate(next_t);
   }

   if(!is_between()) return dNAN;

   // Okay now we have to do a binary search to find 't'
   struct S
   {
      S()
          : y(dNAN)
          , t(dNAN)
      {}
      S(double y_, double t_)
          : y(y_)
          , t(t_)
      {}
      double y;
      double t;
   };
   S upper(last_v.y, last_t);
   S lower(next_v.y, next_t);
   if(upper.y < lower.y) { // swap
      S tmp(upper);
      upper = lower;
      lower = tmp;
   }

   double error = 1.0;
   uint counter = 0;
   S mid;
   while(error > epsilon) {
      if(counter++ > 100) FATAL(format("Failed to find 't' value!"));

      mid.t = 0.5 * (upper.t + lower.t);
      mid.y = spline.evaluate(mid.t).y;
      error = fabs(mid.y - y_value);

      bool go_low = inclusive_between(lower.y, y_value, mid.y)
                    || inclusive_between(mid.y, y_value, lower.y);
      bool go_high = inclusive_between(upper.y, y_value, mid.y)
                     || inclusive_between(mid.y, y_value, upper.y);
      if((go_low && go_high) || (!go_low && !go_high))
         LOG_ERR(format("t escaped"));

      if(go_low) upper = mid;
      if(go_high) lower = mid;
   }
   // INFO(format("counter = {}") % counter);
   return mid.t;
}

// --------------------------------------------------------------- Intersections

Vector2 spline_spline_intersection(const Spline2d& a,
                                   const Spline2d& b,
                                   const real threshold) // pixels
{
   // Have to find two 't' values (ta, tb) such that
   // a.evaluate(ta) == b.evaluate(tb)

   array<double, 3> tas, tbs;
   array<Vector2, 3> vas, vbs;

   // This _could_ be more efficient: two*golden-section at worst
   // However, 'intersections' is not the bottle-neck for this
   // cost-function. (Refitting the splines is.)
   auto update = [&](double ta, double tb, double range) {
      tas[0] = clamp(ta - 0.5 * range, 0.0, 1.0);
      tas[1] = ta;
      tas[2] = clamp(ta + 0.5 * range, 0.0, 1.0);
      tbs[0] = clamp(tb - 0.5 * range, 0.0, 1.0);
      tbs[1] = tb;
      tbs[2] = clamp(tb + 0.5 * range, 0.0, 1.0);
      for(unsigned i = 0; i < tas.size(); ++i) {
         vas[i] = a.evaluate(tas[i]);
         vbs[i] = b.evaluate(tbs[i]);
      }

      // which are the closest?
      unsigned best_i{0}, best_j{0};
      real best_dist = std::numeric_limits<real>::max();
      for(unsigned i = 0; i < tas.size(); ++i) {
         for(unsigned j = 0; j < tbs.size(); ++j) {
            auto dist = vas[i].quadrance(vbs[j]);
            if(dist < best_dist) {
               best_dist = dist;
               best_i    = i;
               best_j    = j;
            }
         }
      }

      auto ret = Vector3(tas[best_i], tbs[best_j], best_dist);

      if(false) {
         INFO("REPORT");
         cout << format("tas = [{}]", implode(tas.begin(), tas.end(), ", "))
              << endl;
         cout << format("tbs = [{}]", implode(tbs.begin(), tbs.end(), ", "))
              << endl;
         cout << format("vas = [{}]", implode(vas.begin(), vas.end(), ", "))
              << endl;
         cout << format("vbs = [{}]", implode(vbs.begin(), vbs.end(), ", "))
              << endl;
         cout << format("ret = {}", str(ret)) << endl;

         fgetc(stdin);
      }

      return ret;
   };

   double range = 1.0;
   double ta{0.0}, tb{0.0};
   Vector3 val(0.5, 0.5, std::numeric_limits<real>::max());
   unsigned counter = 0;
   while(val.z > (threshold * threshold)) {
      if(counter++ > 100) { break; }
      val = update(val.x, val.y, range);
      range *= 0.5;
   }

   return 0.5 * (a.evaluate(val.x) + b.evaluate(val.y));
}

} // namespace perceive