// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "volume.h"
#include "cross.h"
#include <Eigen/Geometry>
template <
  typename DerivedV,
  typename DerivedT,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedT>& T,
  Eigen::PlainObjectBase<Derivedvol>& vol)
{
  using namespace Eigen;
  const int m = T.rows();
  vol.resize(m,1);
  for(int t = 0;t<m;t++)
  {
    const RowVector3d & a = V.row(T(t,0));
    const RowVector3d & b = V.row(T(t,1));
    const RowVector3d & c = V.row(T(t,2));
    const RowVector3d & d = V.row(T(t,3));
    vol(t) = -(a-d).dot((b-d).cross(c-d))/6.;
  }
}

template <
  typename DerivedA,
  typename DerivedB,
  typename DerivedC,
  typename DerivedD,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedA> & A,
  const Eigen::PlainObjectBase<DerivedB> & B,
  const Eigen::PlainObjectBase<DerivedC> & C,
  const Eigen::PlainObjectBase<DerivedD> & D,
  Eigen::PlainObjectBase<Derivedvol> & vol)
{
  const auto & AmD = A-D;
  const auto & BmD = B-D;
  const auto & CmD = C-D;
  Eigen::PlainObjectBase<DerivedA> BmDxCmD;
  cross(BmD.eval(),CmD.eval(),BmDxCmD);
  const auto & AmDdx = (AmD.array() * BmDxCmD.array()).rowwise().sum();
  vol = -AmDdx/6.;
}

template <
  typename VecA,
  typename VecB,
  typename VecC,
  typename VecD>
IGL_INLINE typename VecA::Scalar igl::volume_single(
  const VecA & a,
  const VecB & b,
  const VecC & c,
  const VecD & d)
{
  return -(a-d).dot((b-d).cross(c-d))/6.;
}

template <
  typename DerivedL,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedL>& L,
  Eigen::PlainObjectBase<Derivedvol>& vol)
{
  using namespace Eigen;
  const int m = L.rows();
  typedef typename Derivedvol::Scalar ScalarS;
  vol.resize(m,1);
  for(int t = 0;t<m;t++)
  {
    const ScalarS u = L(t,0);
    const ScalarS v = L(t,1);
    const ScalarS w = L(t,2);
    const ScalarS U = L(t,3);
    const ScalarS V = L(t,4);
    const ScalarS W = L(t,5);
    const ScalarS X = (w - U + v)*(U + v + w);
    const ScalarS x = (U - v + w)*(v - w + U);
    const ScalarS Y = (u - V + w)*(V + w + u);
    const ScalarS y = (V - w + u)*(w - u + V);
    const ScalarS Z = (v - W + u)*(W + u + v);
    const ScalarS z = (W - u + v)*(u - v + W);
    const ScalarS a = sqrt(x*Y*Z);
    const ScalarS b = sqrt(y*Z*X);
    const ScalarS c = sqrt(z*X*Y);
    const ScalarS d = sqrt(x*y*z);
    vol(t) = sqrt(
       (-a + b + c + d)*
       ( a - b + c + d)*
       ( a + b - c + d)*
       ( a + b + c - d))/
       (192.*u*v*w);
  }
}
#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
// generated by autoexplicit.sh
template void igl::volume<Eigen::Matrix<double, -1, 6, 0, -1, 6>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 6, 0, -1, 6> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template Eigen::Matrix<double, 1, 3, 1, 1, 3>::Scalar igl::volume_single<Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&);
template Eigen::Matrix<double, 3, 1, 0, 3, 1>::Scalar igl::volume_single<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&);
template void igl::volume<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template void igl::volume<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


/**
 * \file HadamardMixture.hxx
 */

#include <ATK/Delay/FeedbackDelayNetworkFilter.hxx>
#include <ATK/Delay/HadamardMixture.h>

#include <Eigen/Dense>

#include <ATK/Core/TypeTraits.h>
#include <ATK/Utility/fmath.h>

namespace ATK
{
  template<typename DataType_, unsigned int order>
  class HadamardMixture<DataType_, order>::MixtureImpl
  {
  public:
    using Vector = Eigen::Matrix<DataType, nb_channels, 1>;
    using Matrix = Eigen::Matrix<DataType, nb_channels, nb_channels>;

    Vector mix(const Vector& x) const
    {
      return transition * x;
    }

  protected:
    const Matrix transition = create();

    static Matrix create()
    {
      return (DataType_(1 / fmath::pow(2, order / 2.)) * recursive_create<order>()).template cast<DataType_>();
    }

    template<unsigned int recursive_order>
    static Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, (1U<<recursive_order), (1U<<recursive_order)> recursive_create()
    {
      if constexpr(recursive_order == 0)
      {
        return Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, 1U, 1U>::Constant(1);
      }
      else
      {
        constexpr auto big_size = (1U << recursive_order);
        constexpr auto small_size = (1U << (recursive_order - 1));
        Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, big_size, big_size> cur_transition;

        auto M_1 = recursive_create<recursive_order - 1>();
        cur_transition.block(0, 0, small_size, small_size) = M_1;
        cur_transition.block(0, small_size, small_size, small_size) = M_1;
        cur_transition.block(small_size, 0, small_size, small_size) = -M_1;
        cur_transition.block(small_size, small_size, small_size, small_size) = M_1;
        return cur_transition;
      }
    }
  };
}


#include "Core.h"
#include "random.h"
#include <boost/random.hpp>
#include <ctime>

float angle_clamp(const float rads) {
	const float P = float(2.0*M_PI);
	float clamped = rads;

	while (clamped < 0.0f) {
		clamped += P;
	}

	while (clamped > 2*P) {
		clamped -= P;
	}

	return clamped;
}

float getAngle( const vec2 &a, const vec2 &b ) {
	float dx = a.x - b.x;
	float dy = a.y - b.y;
	float rads = atan2f(dy, dx);
	float clamped = angle_clamp(rads);
	return clamped;
}

vec3 GetRandomVector(float length) {
	return vec3(FRAND_RANGE(-1.0f,+1.0f),
	            FRAND_RANGE(-1.0f,+1.0f),
	            FRAND_RANGE(-1.0f,+1.0f)).getNormal() * length;
}

string toString(const any &value) {
	string typeName = value.type().name();

	if (typeName == typeid(vec2).name()) {
		return vec2::toString(any_cast<vec2>(value));
	} else if (typeName == typeid(vec3).name()) {
		return vec3::toString(any_cast<vec3>(value));
	} else if (typeName == typeid(vec4).name()) {
		return vec4::toString(any_cast<vec4>(value));
	} else if (typeName == typeid(mat4).name()) {
		return mat4::toString(any_cast<mat4>(value));
	} else if (typeName == typeid(mat3).name()) {
		return mat3::toString(any_cast<mat3>(value));
	} else if (typeName == typeid(float).name()) {
		return ftos(any_cast<float>(value));
	} else if (typeName == typeid(int).name()) {
		return itos(any_cast<int>(value));
	} else if (typeName == typeid(size_t).name()) {
		return sizet_to_string(any_cast<size_t>(value));
	} else if (typeName == typeid(bool).name()) {
		return any_cast<bool>(value) ? "true" : "false";
	} else {
		return "???";
	}
}

vec3 calcTriNorm(const vec3 &a, const vec3 &b, const vec3 &c) {
	vec3 v1 = a - b;
	vec3 v2 = b - c;
	vec3 n = v1.cross(v2);
	return n.getNormal();
}

vec3 calcTriNorm(const Triangle &tri) {
	return calcTriNorm(tri.a, tri.b, tri.c);
}

float SampleLogNormal(float mean, float sigma) {
	// Create a Mersenne twister random number generator
	// that is seeded once with #seconds since 1970
	static mt19937 rng(static_cast<unsigned> (std::time(0)));

	// select gamma probability distribution
	lognormal_distribution<float> dist(mean, sigma);

	// bind random number generator to distribution, forming a function
	variate_generator<mt19937&, lognormal_distribution<float> >  sampler(rng, dist);

	// sample from the distribution
	return sampler();
}

float SampleNormal(float mean, float sigma) {
	// Create a Mersenne twister random number generator
	// that is seeded once with #seconds since 1970
	static mt19937 rng(static_cast<unsigned> (std::time(0)));

	// select Gaussian probability distribution
	normal_distribution<float> norm_dist(mean, sigma);

	// bind random number generator to distribution, forming a function
	variate_generator<mt19937&, normal_distribution<float> >  normal_sampler(rng, norm_dist);

	// sample from the distribution
	return normal_sampler();
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * UdU' Factorisation of Covariance Filter.
 *
 * For efficiency UD_scheme requires to know the maximum q_size of the predict_model
 * ISSUES:
 *  observe functions: returned rcond is the minimum of each sequential update, an overall conditioning would be better
 */
#include "UDFlt.hpp"
#include "matSup.hpp"
#include <boost/limits.hpp>

/* Filter namespace */
namespace Bayesian_filter
{
	using namespace Bayesian_filter_matrix;

UD_scheme::
UD_scheme (std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) :
		Kalman_state_filter(x_size),
		q_max(q_maxsize),
		UD(x_size,x_size+q_max),
		s(Empty), Sd(Empty),
		d(x_size+q_max), dv(x_size+q_max), v(x_size+q_max),
		a(x_size), b(x_size),
		h1(x_size), w(x_size),
		znorm(Empty),
		zpdecol(Empty),
		Gz(Empty),
		GIHx(Empty)
/* Initialise filter and set the size of things we know about
 */
{
	last_z_size = 0;	// Matrices conform to z_initialsize, they are left Empty if z_initialsize==0
	observe_size (z_initialsize);
}

UD_scheme&
 UD_scheme::operator= (const UD_scheme& a)
/* Optimise copy assignment to only copy filter state
 * Precond: matrix size conformance
 */
{
	Kalman_state_filter::operator=(a);
	q_max = a.q_max;
	UD = a.UD;
	return *this;
}

void
 UD_scheme::init ()
/* Initialise from a state and state coveriance
 * Computes UD factor from initial covaiance
 * Predcond:
 *  X
 * Postcond:
 *  X
 *  UD=X, d is PSD
 */
{
					// Factorise X into left partition of UD
	std::size_t x_size = UD.size1();
	UD.sub_matrix(0,x_size, 0,x_size) .assign (X);
	Float rcond = UdUfactor (UD, x_size);
	rclimit.check_PSD(rcond, "Initial X not PSD");
}

void
 UD_scheme::update ()
/* Defactor UD back into X
 * Precond:
 *  UD
 * Postcond:
 *  X=UD  PSD iff UD is PSD
 */
{
	UdUrecompose (X, UD);
}

UD_scheme::Float
 UD_scheme::predict (Linrz_predict_model& f)
/* Prediction using a diagonalised noise q, and its coupling G
 *  q can have order less then x and a matching G so GqG' has order of x
 * Precond:
 *	UD
 * Postcond:
 *  UD is PSD
 */
{
	x = f.f(x);			// Extended Kalman state predict is f(x) directly

						// Predict UD from model
	Float rcond = predictGq (f.Fx, f.G, f.q);
	rclimit.check_PSD(rcond, "X not PSD in predict");
	return rcond;
}

UD_scheme::Float
 UD_scheme::predictGq (const Matrix& Fx, const Matrix& G, const FM::Vec& q)
/* MWG-S prediction from Bierman  p.132
 *  q can have order less then x and a matching G so GqG' has order of x
 * Precond:
 *  UD
 * Postcond:
 *  UD
 *
 * Return:
 *		reciprocal condition number, -1 if negative, 0 if semi-definite (including zero)
 */
{
	std::size_t i,j,k;
	const std::size_t n = x.size();
	const std::size_t Nq = q.size();
	const std::size_t N = n+Nq;
	Float e;
					// Check preallocated space for q size
	if (Nq > q_max)
		error (Logic_exception("Predict model q larger than preallocated space"));

	if (n > 0)		// Simplify reverse loop termination
	{
						// Augment d with q, UD with G
		for (i = 0; i < Nq; ++i)		// 0..Nq-1
		{
			d[i+n] = q[i];
		}
		for (j = 0; j < n; ++j)		// 0..n-1
		{
			Matrix::Row UDj(UD,j);
			Matrix::const_Row  Gj(G,j);
			for (i = 0; i < Nq; ++i)		// 0..Nq-1
				UDj[i+n] = Gj[i];
		}

						// U=Fx*U and diagonals retrieved
		for (j = n-1; j > 0; --j)		// n-1..1
		{
						// Prepare d(0)..d(j) as temporary
			for (i = 0; i <= j; ++i)	// 0..j
				d[i] = Float(UD(i,j));	// ISSUE mixed type proxy assignment

						// Lower triangle of UD is implicitly empty
			for (i = 0; i < n; ++i) 	// 0..n-1
			{
				Matrix::Row UDi(UD,i);
				Matrix::const_Row Fxi(Fx,i);
				UDi[j] = Fxi[j];
				for (k = 0; k < j; ++k)	// 0..j-1
					UDi[j] += Fxi[k] * d[k];
			}
		}
		d[0] = Float(UD(0,0));	// ISSUE mixed type proxy assignment

						//  Complete U = Fx*U
		for (j = 0; j < n; ++j)			// 0..n-1
		{
			UD(j,0) = Fx(j,0);
		}

						// The MWG-S algorithm on UD transpose
		j = n-1;
		do {							// n-1..0
			Matrix::Row UDj(UD,j);
			e = 0;
			for (k = 0; k < N; ++k)		// 0..N-1
			{
				v[k] = Float(UDj[k]);	// ISSUE mixed type proxy assignment
				dv[k] = d[k] * v[k];
				e += v[k] * dv[k];
			}
			// Check diagonal element
			if (e > 0)
			{
				// Positive definite
				UDj[j] = e;

				Float diaginv = 1 / e;
				for (k = 0; k < j; ++k)	// 0..j-1
				{
					Matrix::Row UDk(UD,k);
					e = 0;
					for (i = 0; i < N; ++i)	// 0..N-1
						e += UDk[i] * dv[i];
					e *= diaginv;
					UDj[k] = e;

					for (i = 0; i < N; ++i)	// 0..N-1
						UDk[i] -= e * v[i];
				}
			}//PD
			else if (e == 0)
			{
				// Possibly semi-definite, check not negative
				UDj[j] = e;

				// 1 / e is infinite
				for (k = 0; k < j; ++k)	// 0..j-1
				{
					Matrix::Row UDk(UD,k);
					for (i = 0; i < N; ++i)	// 0..N-1
					{
						e = UDk[i] * dv[i];
						if (e != 0)
							goto Negative;
					}
					// UD(j,k) unaffected
				}
			}//PD
			else
			{
				// Negative
				goto Negative;
			}
		} while (j-- > 0); //MWG-S loop

						// Transpose and Zero lower triangle
		for (j = 1; j < n; ++j)			// 0..n-1
		{
			Matrix::Row UDj(UD,j);
			for (i = 0; i < j; ++i)
			{
				UD(i,j) = UDj[i];
				UDj[i] = 0;			// Zeroing unnecessary as lower only used as a scratch
			}
		}

	}

	// Estimate the reciprocal condition number from upper triangular part
	return UdUrcond(UD,n);

Negative:
	return -1;
}

void
 UD_scheme::observe_size (std::size_t z_size)
/* Optimised dynamic observation sizing
 */
{
	if (z_size != last_z_size) {
		last_z_size = z_size;

		s.resize(z_size, false);
		Sd.resize(z_size, false);
		znorm.resize(z_size, false);
	}
}

Bayes_base::Float
 UD_scheme::observe (Linrz_uncorrelated_observe_model& h, const Vec& z)
/* Standard linrz observe
 *  Uncorrelated observations are applied sequentially in the order they appear in z
 *  The sequential observation updates state x
 *  Therefore the model of each observation needs to be computed sequentially. Generally this
 *  is inefficient and observe (UD_sequential_observe_model&) should be used instead
 * Precond:
 *	 UD
 *	 Zv is PSD
 * Postcond:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

								// Dynamic sizing
	observe_size (z_size);
								// Apply observations sequentially as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (std::size_t o = 0; o < z_size; ++o)
	{
								// Observation model, extracted for a single z element
		const Vec& zp = h.h(x);
		h.normalise(znorm = z, zp);
		noalias(h1) = row(h.Hx, o);
								// Check Z precondition
		if (h.Zv[o] < 0)
			error (Numeric_exception("Zv not PSD in observe"));
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h1, h.Zv[o]);
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using normalised non-linear innovation
		s = znorm[o] - zp[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

Bayes_base::Float
 UD_scheme::observe (Linrz_correlated_observe_model& /*h*/, const Vec& /*z*/)
/* No solution for Correlated noise and Linearised model
 */
{
	error (Logic_exception("observe no Linrz_correlated_observe_model solution"));
	return 0;	// never reached
}

Bayes_base::Float
 UD_scheme::observe (Linear_correlated_observe_model& h, const Vec& z)
/* Special Linear Hx observe for correlated Z
 *  Z must be PD and will be decorrelated
 * Applies observations sequentially in the order they appear in z
 * Creates temporary Vec and Matrix to decorrelate z,Z
 * Precondition:
 *  UD
 *  Z is PSD
 * Postcondition:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	std::size_t i, j, k;
	const std::size_t x_size = x.size();
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

					// Dynamic sizing
	observe_size (z_size);
	if (z_size != zpdecol.size()) {
		zpdecol.resize(z_size, false);
		Gz.resize(z_size,z_size, false);
		GIHx.resize(z_size, x_size, false);
	}

					// Factorise process noise as GzG'
	{	Float rcond = FM::UdUfactor (Gz, h.Z);
		rclimit.check_PSD(rcond, "Z not PSD in observe");
	}

								// Observation prediction and normalised observation
	const Vec& zp = h.h(x);
	h.normalise(znorm = z, zp);

	if (z_size > 0)
	{							// Solve G* GIHx = Hx for GIHx in-place
		GIHx = h.Hx;
		for (j = 0; j < x_size; ++j)
		{
			i = z_size-1;
			do {
				for (k = i+1; k < z_size; ++k)
				{
					GIHx(i,j) -= Gz(i,k) * GIHx(k,j);
				}
			} while (i-- > 0);
		}

		zpdecol = zp;			// Solve G zp~ = z, G z~ = z  for zp~,z~ in-place
		i = z_size-1;
		do {
			for (k = i+1; k < z_size; ++k)
			{
				znorm[i] -= Gz(i,k) * znorm[k];
				zpdecol[i] -= Gz(i,k) * zpdecol[k];
			}
		} while (i-- > 0);
	}//if (z_size>0)

								// Apply observations sequential as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (std::size_t o = 0; o < z_size; ++o)
	{
		h1 = row(GIHx,o);
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h1, Gz(o,o));
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using linear innovation
		s = znorm[o]-zpdecol[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

Bayes_base::Float
 UD_scheme::observe (UD_sequential_observe_model& h, const Vec& z)
/* Special observe using observe_model_sequential for fast uncorrelated linrz operation
 * Uncorrelated observations are applied sequentially in the order they appear in z
 * The sequential observation updates state x. Therefore the model of
 * each observation needs to be computed sequentially
 * Precondition:
 *  UD
 *  Z is PSD
 * Postcondition:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	std::size_t o;
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

								// Dynamic sizing
	observe_size (z_size);
								// Apply observations sequentially as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (o = 0; o < z_size; ++o)
	{
								// Observation prediction and model
		const Vec& zp = h.ho(x, o);
		h.normalise(znorm = z, zp);
								// Check Z precondition
		if (h.Zv[o] < 0)
			error (Numeric_exception("Zv not PSD in observe"));
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h.Hx_o, h.Zv[o]);
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using non-linear innovation
		s = znorm[o]-zp[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

UD_scheme::Float
 UD_scheme::observeUD (FM::Vec& gain, Float & alpha, const Vec& h, const Float r)
/** Linear UD factorisation update
 *  Bierman UdU' factorisation update. Bierman p.100
 * Input
 *  h observation coefficients
 *  r observation variance
 * Output
 *  gain  observation Kalman gain
 *  alpha observation innovation variance
 * Variables with physical significance
 *  gamma becomes covariance of innovation
 * Precondition:
 *  UD
 *  r is PSD (not checked)
 * Postcondition:
 *  UD (see return value)
 * Return:
 *  reciprocal condition number of UD, -1 if alpha singular (negative or zero)
 */
{
	std::size_t i,j,k;
	const std::size_t n = UD.size1();
	Float gamma, alpha_jm1, lamda;
	// a(n) is U'a
	// b(n) is Unweighted Kalman gain

					// Compute b = DU'h, a = U'h
	a = h;
	for (j = n-1; j >= 1; --j)	// n-1..1
	{
		for (k = 0; k < j; ++k)	// 0..j-1
		{
			a[j] += UD(k,j) * a[k];
		}
		b[j] = UD(j,j) * a[j];
	}
	b[0] = UD(0,0) * a[0];

					// Update UD(0,0), d(0) modification
	alpha = r + b[0] * a[0];
	if (alpha <= 0) goto alphaNotPD;
	gamma = 1 / alpha;
	UD(0,0) *= r * gamma;
					// Update rest of UD and gain b
	for (j = 1; j < n; ++j)		// 1..n-1
	{
					// d modification
		alpha_jm1 = alpha;	// alpha at j-1
		alpha += b[j] * a[j];
		lamda = -a[j] * gamma;
		if (alpha <= 0) goto alphaNotPD;
		gamma = 1 / alpha;
		UD(j,j) *= alpha_jm1 * gamma;
					// U modification
		for (i = 0; i < j; ++i)		// 0..j-1
		{
			Float UD_jm1 = UD(i,j);
			UD(i,j) = UD_jm1 + lamda * b[i];
			b[i] += b[j] * UD_jm1;
		}
	}
					// Update gain from b
	noalias(gain) = b * gamma;
 	// Estimate the reciprocal condition number from upper triangular part
	return UdUrcond(UD,n);

alphaNotPD:
	return -1;
}

}//namespace


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <boost/math/special_functions/log1p.hpp>
#include <cmath>
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline GumbelBicop::GumbelBicop()
{
  family_ = BicopFamily::gumbel;
  parameters_ = Eigen::VectorXd(1);
  parameters_lower_bounds_ = Eigen::VectorXd(1);
  parameters_upper_bounds_ = Eigen::VectorXd(1);
  parameters_ << 1;
  parameters_lower_bounds_ << 1;
  parameters_upper_bounds_ << 50;
}

inline double
GumbelBicop::generator(const double& u)
{
  return std::pow(std::log(1 / u), this->parameters_(0));
}

inline double
GumbelBicop::generator_inv(const double& u)
{
  return std::exp(-std::pow(u, 1 / this->parameters_(0)));
}

inline double
GumbelBicop::generator_derivative(const double& u)
{
  double theta = double(this->parameters_(0));
  return std::pow(std::log(1 / u), theta - 1) * (-theta / u);
}

// inline double GumbelBicop::generator_derivative2(const double &u)
//{
//    double theta = double(this->parameters_(0));
//    return (theta - 1 - std::log(u)) * std::pow(std::log(1 / u), theta - 2) *
//           (theta / std::pow(u, 2));
//}

inline Eigen::VectorXd
GumbelBicop::pdf_raw(const Eigen::MatrixXd& u)
{
  double theta = static_cast<double>(parameters_(0));
  double thetha1 = 1.0 / theta;
  auto f = [theta, thetha1](const double& u1, const double& u2) {
    double t1 = std::pow(-std::log(u1), theta) + std::pow(-std::log(u2), theta);
    double temp = -std::pow(t1, thetha1) + (2 * thetha1 - 2.0) * std::log(t1) +
                  (theta - 1.0) * std::log(std::log(u1) * std::log(u2)) -
                  std::log(u1 * u2) +
                  boost::math::log1p((theta - 1.0) * std::pow(t1, -thetha1));
    return std::exp(temp);
  };
  return tools_eigen::binaryExpr_or_nan(u, f);
}

inline Eigen::VectorXd
GumbelBicop::hinv1_raw(const Eigen::MatrixXd& u)
{
  double theta = double(this->parameters_(0));
  double u1, u2;
  Eigen::VectorXd hinv = Eigen::VectorXd::Zero(u.rows());
  for (int j = 0; j < u.rows(); ++j) {
    u1 = u(j, 1);
    u2 = u(j, 0);
    if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
      hinv(j) = std::numeric_limits<double>::quiet_NaN();
    } else {
      hinv(j) = qcondgum(&u1, &u2, &theta);
    }
  }

  return hinv;
}

inline Eigen::MatrixXd
GumbelBicop::tau_to_parameters(const double& tau)
{
  auto par = Eigen::VectorXd::Constant(1, 1.0 / (1 - std::fabs(tau)));
  return par.cwiseMax(parameters_lower_bounds_)
    .cwiseMin(parameters_upper_bounds_);
}

inline double
GumbelBicop::parameters_to_tau(const Eigen::MatrixXd& parameters)
{
  return (parameters(0) - 1) / parameters(0);
}

inline Eigen::VectorXd
GumbelBicop::get_start_parameters(const double tau)
{
  Eigen::VectorXd par = tau_to_parameters(tau);
  par = par.cwiseMax(parameters_lower_bounds_);
  par = par.cwiseMin(parameters_upper_bounds_);
  return par;
}
}

// This is copy&paste from the VineCopula package
inline double
qcondgum(double* q, double* u, double* de)
{
  double a, p, z1, z2, con, de1, dif;
  double mxdif;
  int iter;

  p = 1 - *q;
  z1 = -log(*u);
  con = log(1. - p) - z1 + (1. - *de) * log(z1);
  de1 = *de - 1.;
  a = pow(2. * pow(z1, *de), 1. / (*de));
  mxdif = 1;
  iter = 0;
  dif = .1; // needed in case first step leads to NaN
  while ((mxdif > 1.e-6) && (iter < 20)) {
    double g = a + de1 * log(a) + con;
    double gp = 1. + de1 / a;
    if ((boost::math::isnan)(g) || (boost::math::isnan)(gp) ||
        (boost::math::isnan)(g / gp)) {
      // added for de>50
      dif /= -2.;
    } else {
      dif = g / gp;
    }
    a -= dif;
    iter++;
    int it = 0;
    while ((a <= z1) && (it < 20)) {
      dif /= 2.;
      a += dif;
      ++it;
    }
    mxdif = fabs(dif);
  }
  z2 = pow(pow(a, *de) - pow(z1, *de), 1. / (*de));
  return (exp(-z2));
}


///////////////////////////////////////////////////////////////////////////////
// kernel::example::scalar_nw.cpp                                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //needed?
#include <boost/math/tools/precision.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/container/map.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/map_fwd.hpp>

#include <boost/statistics/detail/fusion/functor/at_key.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/estimation/meta_nw_visitor_unary.hpp>
#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <libs/statistics/detail/kernel/example/scalar_nw.h>

void example_scalar_nw(std::ostream& out){

    out << "-> example_scalar_nw : ";
    using namespace boost;

    namespace kernel = boost::statistics::detail::kernel;

    // This example shows how to compute a Nadaraya-Watson estimate of E[y|x].
    // The type used for each data-unit, here, is a fusion map whose x and y
    // components are accessed using keys

    // Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vals_;
    typedef mpl::int_<0>                                    key_x_;
    typedef mpl::int_<1>                                    key_y_;
    typedef fusion::pair<key_x_,val_>                       x_;
    typedef fusion::pair<key_y_,val_>                       y_;
    typedef statistics::detail::fusion::functor::at_key<key_x_> at_key_x_;
    typedef statistics::detail::fusion::functor::at_key<key_y_> at_key_y_;
    typedef fusion::map<x_,y_>                              data_unit_;
    typedef std::vector<data_unit_>                          dataset_;
    // The rationale for data_range_ is it's cheap to copy
    typedef sub_range<dataset_>                             data_range_;

    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef kernel::scalar::gaussian_kernel<val_>                  gauss_k_;
    typedef kernel::meta_nw_visitor_unary<
        at_key_x_,
        at_key_y_
    > meta_nw_visitor_u_;
    typedef meta_nw_visitor_u_::apply<
        gauss_k_,
        val_
    >::type  nw_visitor_u_;
    typedef nw_visitor_u_::nw_visitor_type              nw_visitor_;
    typedef nw_visitor_u_::rp_visitor_type              rp_visitor_;

    // Constants
    const val_ bandwidth = 0.5;
    const val_ eps = math::tools::epsilon<val_>();
    const unsigned n = 10;

    // Initialization
    vals_ vec_rp; vec_rp.reserve(n);
    vals_ vec_nw; vec_nw.reserve(n);
    dataset_ dataset;
    dataset.reserve(n);
    {
        urng_ urng;
        norm_ norm;
        gen_ gen(urng,norm);
        val_ one = static_cast<val_>(1);
        for(unsigned i = 0; i<n; i++){
            dataset.push_back(
                data_unit_(
                    fusion::make_pair<key_x_>(gen()),
                    fusion::make_pair<key_y_>(one)
                )
            );
        }
    }

    // Computes nw = E[y|x] for each x in the dataset. The density (rp) is
    // obtained as a by-product. Here, y = 1, so we should have
    // rp = nw (un-normalized).
    BOOST_FOREACH(data_unit_& u,dataset){
        nw_visitor_ nw_visitor = std::for_each(
            boost::begin(dataset),
            boost::end(dataset),
            nw_visitor_u_(
                bandwidth,
                fusion::at_key<key_x_>(u)
            )
        );
        val_ u_nw = nw_visitor.unnormalized_estimate();
        vec_nw.push_back(u_nw);
        rp_visitor_ rp_visitor = nw_visitor.rp_visitor();
        val_ rp = rp_visitor.estimate();
        BOOST_ASSERT(fabs(rp-u_nw)<eps);
    }

    // Same as above using estimator

    typedef kernel::estimator<
        data_range_,
        meta_nw_visitor_u_::apply,
        gauss_k_
    > estimator_;
    estimator_ estimator(bandwidth);
    estimator.train(
        data_range_(dataset)
    ); // * step 1 *

    BOOST_FOREACH(data_unit_& u,dataset){
        // -> these steps are independent of step2, they're just a test
        val_ x = fusion::at_key<key_x_>(u);
        BOOST_AUTO( nw_v , estimator.visit(x) );
        val_ u_nw = nw_v.unnormalized_estimate();
        BOOST_AUTO( rp_v , nw_v.rp_visitor() );
        val_ rp = rp_v.estimate();
        BOOST_ASSERT(fabs(rp-u_nw)<eps);
        // <-

        estimator.predict(x); // * step 2 *

    }

    out << "<-" << std::endl;
}


#define ARMA_DONT_USE_WRAPPER

#include <armadillo>

#include "../include/calc_asymptotic_variance.hpp"

using namespace std;
using namespace arma;

//' Calculate the asymptotic variance for the predicted y values
//'
//' @param Sigma_ll Sigma_ll matrix for the whole genome
//' @param Sigma_ls Sigma_ls matrix for the whole genome
//' @param Sigma_ss Sigma_ss matrix for the whole genome
//' @param Sigma_ss_blockwise a one-dimensional field containing blockwise Sigma_ss matrices
//' @param sigma2_s estimated value of sigma^2_s
//' @param n sample size for observed data (not the reference panel)
//' @param Xl_test genotypes matrix for large effect SNPs for test subjects
//' @param Xs_test genotypes matrix for small effect SNPs for test subjects
//' @return variance of predicted y values

arma::mat calc_asymptotic_variance(const arma::mat& Sigma_ll,
                                   const arma::mat& Sigma_ls,
                                   const arma::mat& Sigma_ss,
                                   const arma::field <arma::mat >& Sigma_ss_blockwise,
                                   double sigma2_s,
                                   unsigned int n,
                                   const arma::mat& Xl_test,
                                   const arma::mat& Xs_test){
  arma::mat Ainv = calc_A_inverse(Sigma_ss_blockwise, sigma2_s, n);
  arma::mat var_bl = calc_var_betal(Sigma_ll,
                                    Sigma_ls,
                                    Sigma_ss,
                                    Ainv,
                                    n);
  arma::mat var_bs = calc_var_betas(Sigma_ss,
                                    Sigma_ls,
                                    Ainv,
                                    sigma2_s,
                                    n,
                                    var_bl);
  arma::mat result = Xl_test * var_bl * arma::trans(Xl_test) + Xs_test * var_bs * arma::trans(Xs_test);
  return(result);
}

//' Calculate A inverse matrix
//'
//' @details (sigma^{-2}n^{-1} I_ms + Sigma_ss) = A. Here, we use the block diagonal structure
//' @param field a one-dimensional armadillo field for the entire genome. Each entry corresponds to a single LD block
//' @param sigma2_s estimate of sigma^2_s
//' @param n sample size
//' @return A inverse matrix

arma::mat calc_A_inverse(const arma::field <arma::mat >& field,
                         double sigma2_s,
                         unsigned int n)  {
  unsigned int n_blocks = field.n_elem;
  arma::field <arma::mat> inv_field(n_blocks);
  for( unsigned int i = 0; i < n_blocks; i++) {
    //make the diagonal matrix
    unsigned int m_s = field(i).n_rows;
    inv_field(i) = arma::inv_sympd(arma::eye(m_s, m_s) / (n * sigma2_s) + field(i));
  }
  arma::mat result = BlockDiag(inv_field);
  return result;
}

//' Calculate variance of coefficient estimator for large effects
//'
//' @param Sigma_ll Sigma_ll constructed for one LD block
//' @param Sigma_ls Sigma_ls constructed for one LD block
//' @param Sigma_ss Sigma_ss constructed for one LD block
//' @param A_inverse inverse of (sigma^{-2}n^{-1} I_ms + Sigma_ss)
//' @param n sample size
//' @return covariance matrix

arma::mat calc_var_betal(const arma::mat& Sigma_ll,
                         const arma::mat& Sigma_ls,
                         const arma::mat& Sigma_ss,
                         const arma::mat& A_inverse,
                         unsigned int n){
  //calculate second matrix
  arma::mat big = Sigma_ll - Sigma_ls * A_inverse * arma::trans(Sigma_ls);
  //invert and divide by n
  arma::mat result = arma::inv_sympd(big) / n;
  return (result);
}

//' Calculate variance of coefficient estimator for small effects
//'
//' @param Sigma_ss Sigma_ss matrix
//' @param Sigma_ls Sigma_ls matrix
//' @param A_inverse A inverse matrix
//' @param sigma2_s estimated value of sigma^2_s
//' @param n sample size
//' @param var_bl variance of beta hat l
//' @return covariance matrix

  arma::mat calc_var_betas(const arma::mat& Sigma_ss,
                           const arma::mat& Sigma_ls,
                           const arma::mat& A_inverse,
                           double sigma2_s,
                           unsigned int n,
                           const arma::mat& var_bl){
  arma::mat small = arma::trans(Sigma_ls) - Sigma_ss * A_inverse * arma::trans(Sigma_ls);
  arma::mat term2 = small * var_bl * arma::trans(small);
  arma::mat term1 = Sigma_ss - Sigma_ss * A_inverse * Sigma_ss;
  arma::mat result = sigma2_s * sigma2_s * n * (term1 + term2);
  return (result);
}

//' Construct a block diagonal matrix from a collection of matrices
//'
//' @param x a field of matrices, possibly of different sizes. Some matrices may have no rows and no columns
//' @return a block diagonal matrix
//' @reference https://stackoverflow.com/questions/29198893/block-diagonal-matrix-armadillo

arma::mat BlockDiag( const arma::field<arma::mat>& x ) {

  unsigned int len = x.n_elem;
  int drow = 0;
  int dcol = 0;
  arma::ivec rvec(len);
  arma::ivec cvec(len);
  //get dimensions of each matrix in the field
  for(unsigned int i = 0; i < len; i++) {
    rvec(i) = x(i).n_rows ;
    cvec(i) = x(i).n_cols ;
    drow += rvec(i);
    dcol += cvec(i);
  }
  //initialize matrix to be returned
  arma::mat X(drow, dcol, fill::zeros);
  int idx_row = 0;
  int idx_col = 0;
  // place matrices at correct places
  for(unsigned int i=0; i < len; i++) {
    if (rvec(i) > 0 && cvec(i) > 0){
      X.submat(idx_row,
               idx_col,
               idx_row + rvec(i) - 1,
               idx_col + cvec(i) - 1) = x(i) ;
      idx_row = idx_row + rvec(i) ;
      idx_col = idx_col + cvec(i);
    }
  }
  return(X);
}

//' Construct a n by p matrix from field containing matrices with n rows, but possibly fewer columns.
//'
//' @param x a field of matrices, possibly of different sizes, but all with the same number of rows.
//' @return a matrix

arma::mat ConcatenateColumns( const arma::field<arma::mat>& x ) {

  unsigned int len = x.n_elem;
  //unsigned int nrow = x(1).n_rows;//problem!
  int dcol = 0;

  arma::ivec cvec(len);
  arma::ivec rvec(len);
  //get number of columns of each matrix in the field
  for(unsigned int i = 0; i < len; i++) {
    cvec(i) = x(i).n_cols ;
    rvec(i) = x(i).n_rows;
    dcol += cvec(i);
  }
  unsigned int nrow = max(rvec);
  //initialize matrix to be returned
  arma::mat X(nrow, dcol, fill::zeros);
  cout << "number of rows: " << nrow << endl;
  cout << "number of columns: " << dcol << endl;

  int idx_col = 0;
  // place matrices at correct places
  for(unsigned int i=0; i < len; i++) {
    if (cvec(i) > 0){
      X.submat(0,
               idx_col,
               nrow - 1,
               idx_col + cvec(i) - 1) = x(i) ;
      idx_col = idx_col + cvec(i);
    }
  }
  return(X);
}

//' Assemble one set of five matrices for one chromosome
//'
//' @details Input is a two-dimensional arma::field, say from one chromosome, where each cell contains an arma::mat
//'     Specifically, it is a k by 5 arma::field, where k is the number of blocks on the chromosome of interest.
//' @param field a two-dimensional arma::field. See details.
//' @return a one-dimensional field containing exactly five arma::mat matrices: Sigma_ss, Sigma_sl, Sigma_ll, geno_s, geno_l

arma::field <arma::mat> assembleMatrices(const arma::field < arma::mat>& field){
  arma::field <arma::mat> result(5);
  cout << "field has this number of rows: " << field.n_rows << endl;
  result(0) = BlockDiag(field.col(0));
  cout << "result(0) has this dimension: " << result(0).n_rows << " rows & " << result(0).n_cols << " columns" << endl;
  result(1) = BlockDiag(field.col(1));
  cout << "result(1) has this dimension: " << result(1).n_rows << " rows & " << result(1).n_cols << " columns" << endl;
  result(2) = BlockDiag(field.col(2));
  cout << "result(2) has this dimension: " << result(2).n_rows << " rows & " << result(2).n_cols << " columns" << endl;
  result(3) = ConcatenateColumns(field.col(3));
  cout << "result(3) has this dimension: " << result(3).n_rows << " rows & " << result(3).n_cols << " columns" << endl;
  result(4) = ConcatenateColumns(field.col(4));
  return result;
}


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

// Copyright Christopher Kormanyos 2015 - 2016.
// Copyright Paul A. Bristow 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing fixed-point text-based Mandelbrot calculation with high resolution.

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <algorithm>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <numeric>
#include <vector>

#include <boost/cstdint.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>

#define MANDELBROT_01_AQUA_FULL
//#define MANDELBROT_02_YELLOW_TOP
//#define MANDELBROT_03_FUSCHIA_SWIRL
//#define MANDELBROT_04_BLACK_WHITE_SEAHORSES

// Declare a base class for the Mandelbrot configuration.
class mandelbrot_configuration_base
{
public:
  virtual ~mandelbrot_configuration_base() { }

  long double x_lo() const { return my_x_lo; }
  long double x_hi() const { return my_x_hi; }
  long double y_lo() const { return my_y_lo; }
  long double y_hi() const { return my_y_hi; }

  virtual boost::uint_fast16_t max_iterations() const = 0;

  virtual int mandelbrot_fractional_resolution() const = 0;

  virtual long double step() const = 0;

  virtual boost::uint_fast32_t width () const = 0;
  virtual boost::uint_fast32_t height() const = 0;

  virtual boost::uint_fast32_t red_hue  () const = 0;
  virtual boost::uint_fast32_t green_hue() const = 0;
  virtual boost::uint_fast32_t blue_hue () const = 0;

protected:
  const long double my_x_lo;
  const long double my_x_hi;
  const long double my_y_lo;
  const long double my_y_hi;

  mandelbrot_configuration_base(const long double xl, const long double xh,
                                const long double yl, const long double yh) : my_x_lo(xl), my_x_hi(xh),
                                                                              my_y_lo(yl), my_y_hi(yh) { }
private:
  mandelbrot_configuration_base() : my_x_lo(0.0L), my_x_hi(0.0L),
                                    my_y_lo(0.0L), my_y_hi(0.0L) { }
};

// Make a template class that represents the Mandelbrot configuration.
// This class automatically creates sensible parameters based on
// the resolution of the fixed-point type supplied in the template
// parameter. If a custom pixel count is required, the step()
// method can be modified accordingly.
template<const int TotalDigits,
         const boost::uint_fast16_t MaxIterations,
         const int MandelbrotFractionalResolution,
         const boost::uint_fast32_t RedHue,
         const boost::uint_fast32_t GreenHue,
         const boost::uint_fast32_t BlueHue>
class mandelbrot_configuration : public mandelbrot_configuration_base
{
public:
  static_assert(    (RedHue   < UINT32_C(256))
                 && (GreenHue < UINT32_C(256))
                 && (BlueHue  < UINT32_C(256)), "The color hue parameters must be 255 or smaller.");

  typedef boost::fixed_point::negatable<16, 16 + 1 - TotalDigits> fixed_point_type;

  mandelbrot_configuration(const long double xl, const long double xh,
                           const long double yl, const long double yh)
    : mandelbrot_configuration_base(xl, xh, yl, yh) { }

  virtual ~mandelbrot_configuration() { }

private:
  virtual boost::uint_fast16_t max_iterations() const { return MaxIterations; }

  virtual int mandelbrot_fractional_resolution() const { return MandelbrotFractionalResolution; }

  virtual long double step() const { return 1.0L / (UINT64_C(1) << -mandelbrot_fractional_resolution()); }

  virtual boost::uint_fast32_t width () const { return static_cast<boost::uint_fast32_t>((x_hi() - x_lo()) / step()); }
  virtual boost::uint_fast32_t height() const { return static_cast<boost::uint_fast32_t>((y_hi() - y_lo()) / step()); }

  virtual boost::uint_fast32_t red_hue  () const { return RedHue; }
  virtual boost::uint_fast32_t green_hue() const { return GreenHue; }
  virtual boost::uint_fast32_t blue_hue () const { return BlueHue; }
};

// This class generated the rows of the mandelbrot iteration.
// The coordinates are set up according to the Mandelbrot configuration.
template<typename NumericType>
class mandelbrot_generator
{
public:
  mandelbrot_generator(const mandelbrot_configuration_base& config)
    : mandelbrot_configuration_object(config),
      mandelbrot_image               (config.width(), config.height()),
      mandelbrot_view                (boost::gil::rgb8_view_t()),
      mandelbrot_iteration_matrix    (mandelbrot_configuration_object.width(),
                                      std::vector<boost::uint_fast16_t>(mandelbrot_configuration_object.height())),
      mandelbrot_color_histogram     (static_cast<std::size_t>(config.max_iterations() + 1U), UINT32_C(0))
  {
    mandelbrot_view = boost::gil::view(mandelbrot_image);
  }

  ~mandelbrot_generator() { }

  void generate_mandelbrot_image()
  {
    // Setup the x-axis coordinates.
    std::vector<NumericType> x_values(mandelbrot_configuration_object.width());

    // Initialize the x-axis coordinates (one time only).
    {
      NumericType x_step(mandelbrot_configuration_object.x_lo());

      for(NumericType& x : x_values)
      {
        x = x_step;

        x_step += mandelbrot_configuration_object.step();
      }
    }

    // Initialize the y-axis coordinate.
    NumericType y(mandelbrot_configuration_object.y_hi());

    // TBD: The iteration through rows can be distributed in multithreading.

    // Loop through all the rows of pixels on the vertical
    // y-axis in the direction of decreasing y-value.
    for(boost::uint_fast32_t row = UINT32_C(0); row < mandelbrot_configuration_object.height(); ++row, y -= mandelbrot_configuration_object.step())
    {
      // Loop through this column of pixels on the horizontal
      // x-axis in the direction of increasing x-value.
      boost::uint_fast32_t col = UINT32_C(0);

      std::for_each(x_values.cbegin(),
                    x_values.cend(),
      [&y, &col, &row, this](const NumericType& x)
      {
        const NumericType cr(x);
        const NumericType ci(y);

        NumericType zr(0);
        NumericType zi(0);

        NumericType zr_sqr(0);
        NumericType zi_sqr(0);

        // Use an optimized complex-numbered multiplication scheme.
        // Thereby reduce the main work of the Mandelbrot iteration to
        // three real-valued multiplications and several real-valued
        // addition/subtraction operations.

        boost::uint_fast16_t i = UINT16_C(0);

        // Perform the iteration sequence for generating the Mandelbrot set.
        // Herein lies the work of the program.
        // TBD: This can easily be distributed to parallel processes.

        while(   (i < mandelbrot_configuration_object.max_iterations())
              && ((zr_sqr + zi_sqr) < 4))
        {
          zi *= zr;
          zi  = (zi + zi) + ci;

          zr = (zr_sqr - zi_sqr) + cr;

          zr_sqr = zr * zr;
          zi_sqr = zi * zi;

          ++i;
        }

        mandelbrot_iteration_matrix[col][row] = i;

        ++mandelbrot_color_histogram[i];

        ++col;
      });

      std::cout << "Calculating Mandelbrot image at row "
                << std::setw(6)
                << (row + 1U)
                << " of "
                << std::setw(6)
                << mandelbrot_configuration_object.height()
                << " total. Have patience."
                << "\r";
    }

    const boost::uint_fast32_t total = boost::uint_fast32_t(mandelbrot_configuration_object.width()) * mandelbrot_configuration_object.height();

    // Perform color-stretching using the histogram approach.
    // Convert the histogram entries such that a given entry contains
    // the sum of its own entries plus all previous entries. This provides
    // a set of scale factors for the color. The histogram approach
    // automatically scales to the distribution of pixels in the image.

    std::accumulate(mandelbrot_color_histogram.begin(),
                    mandelbrot_color_histogram.end(),
                    boost::uint_fast32_t(0U),
    [&total](boost::uint_fast32_t& sum, boost::uint_fast32_t& histogram_entry) -> boost::uint_fast32_t
    {
      sum += histogram_entry;

      histogram_entry = UINT32_C(0xFF) - static_cast<boost::uint_fast32_t>((boost::uint64_t(sum) * 0xFFU) / total);

      return sum;
    });

    for(boost::uint_fast32_t row = UINT32_C(0); row < mandelbrot_configuration_object.height(); ++row)
    {
      for(boost::uint_fast32_t col = UINT32_C(0); col < mandelbrot_configuration_object.width(); ++col)
      {
        const boost::uint_fast32_t color = mandelbrot_color_histogram[mandelbrot_iteration_matrix[col][row]];

        // Mix the color supplied in the template hue parameters.
        const boost::uint8_t rh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.red_hue  () * color) / UINT32_C(255));
        const boost::uint8_t gh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.green_hue() * color) / UINT32_C(255));
        const boost::uint8_t bh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.blue_hue () * color) / UINT32_C(255));

        const boost::gil::rgb8_pixel_t the_color  = boost::gil::rgb8_pixel_t(rh, gh, bh);

        mandelbrot_view(col, row) = boost::gil::rgb8_pixel_t(the_color);
      }
    }

    boost::gil::jpeg_write_view("mandelbrot.jpg", mandelbrot_view);

    std::cout << std::endl
              << "The ouptput file mandelbrot.jpg has been written"
              << std::endl;
  }

private:
  const mandelbrot_configuration_base&           mandelbrot_configuration_object;
  boost::gil::rgb8_image_t                       mandelbrot_image;
  boost::gil::rgb8_view_t                        mandelbrot_view;
  std::vector<std::vector<boost::uint_fast16_t>> mandelbrot_iteration_matrix;
  std::vector<boost::uint_fast32_t>              mandelbrot_color_histogram;
};

int main()
{
  #if defined MANDELBROT_01_AQUA_FULL

    // This is the classic full immage rendered in aqua tones (and black).
    typedef mandelbrot_configuration<128, UINT16_C(2000), -11,
                                     UINT32_C(80),
                                     UINT32_C(255),
                                     UINT32_C(255)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-2.000L, +0.500L,
                                                                        -1.000L, +1.000L);

  #elif defined MANDELBROT_02_YELLOW_TOP

    // This is an upper part of the image rendered in yellow and black tones.
    typedef mandelbrot_configuration<128, UINT16_C(2000), -13,
                                     UINT32_C(255),
                                     UINT32_C(255),
                                     UINT32_C(0)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.1208L - 0.1616L, -0.1208L + 0.1616L,
                                                                        +0.7607L - 0.1616L, +0.7607L + 0.1616L);

  #elif defined MANDELBROT_03_FUSCHIA_SWIRL

    // This is a fanning image rendered in fuschia tones (and black).
    typedef mandelbrot_configuration<128, UINT16_C(10000), -22,
                                     UINT32_C(255),
                                     UINT32_C(0),
                                     UINT32_C(210)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.749730L - 0.0002315L, -0.749730L + 0.0002315L,
                                                                        -0.046608L - 0.0002315L, -0.046608L + 0.0002315L);

  #elif defined MANDELBROT_04_BLACK_WHITE_SEAHORSES

    // This is a swirly seahorse image rendered in black-and-white tones.
    typedef mandelbrot_configuration<128, UINT16_C(10000), -46,
                                     UINT32_C(255),
                                     UINT32_C(255),
                                     UINT32_C(255)> mandelbrot_configuration_type;
    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.745398360667L - 1.25E-11L, -0.745398360667L + 1.25E-11L,
                                                                      +0.112504634996L - 1.25E-11L, +0.112504634996L + 1.25E-11L);

  #else

    #error: Mandelbrot imag type is not defined!

  #endif

  typedef mandelbrot_configuration_type::fixed_point_type mandelbrot_numeric_type;

  typedef mandelbrot_generator<mandelbrot_numeric_type> mandelbrot_generator_type;

  const std::clock_t start = std::clock();

  mandelbrot_generator_type* the_mandelbrot_generator = new mandelbrot_generator_type(mandelbrot_configuration_object);

  the_mandelbrot_generator->generate_mandelbrot_image();

  const float elapsed = (float(std::clock()) - float(start)) / CLOCKS_PER_SEC;

  std::cout << "Time for calculation: "
            << elapsed
            << "s"
            << std::endl;

  delete the_mandelbrot_generator;
}


#ifndef TRIUMF_BNMR_SLR_CBRT_EXP_HPP
#define TRIUMF_BNMR_SLR_CBRT_EXP_HPP

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed cube root exponential integral (from 0 to time_p <= time)
template <typename T = double>
T pulsed_cbrt_exp_integral(T time, T time_p, T nuclear_lifetime, T slr_rate) {
  // make sure that
  assert(time >= time_p);
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    return std::exp(-(time - t_p) / nuclear_lifetime) *
           std::exp(-std::cbrt(slr_rate * (time - t_p)));
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed cube root exponential
template <typename T = double>
T pulsed_cbrt_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                  T slr_rate) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_cbrt_exp_integral(time, time, nuclear_lifetime, slr_rate) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_cbrt_exp_integral(time, pulse_length, nuclear_lifetime,
                                     slr_rate) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed cube root exponential (ROOT)
template <typename T = double> T pulsed_cbrt_exp(const T *x, const T *par) {
  return pulsed_cbrt_exp<T>(*x, par[0], par[1], par[2], par[3]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_CBRT_EXP_HPP


#ifndef CT_ICP_TYPES_HPP
#define CT_ICP_TYPES_HPP

#include <map>
#include <unordered_map>
#include <list>

#include <tsl/robin_map.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <glog/logging.h>

#include "utils.hpp"

#define _USE_MATH_DEFINES

#include <math.h>

namespace ct_icp {

    // A Point3D
    struct Point3D {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        Eigen::Vector3d raw_pt; // Raw point read from the sensor
        Eigen::Vector3d pt; // Corrected point taking into account the motion of the sensor during frame acquisition
        double alpha_timestamp = 0.0; // Relative timestamp in the frame in [0.0, 1.0]
        double timestamp = 0.0; // The absolute timestamp (if applicable)
        int index_frame = -1; // The frame index

        Point3D() = default;
    };

    inline double AngularDistance(const Eigen::Matrix3d &rota,
                                  const Eigen::Matrix3d &rotb) {
        double norm = ((rota * rotb.transpose()).trace() - 1) / 2;
        norm = std::acos(norm) * 180 / M_PI;
        return norm;
    }

    // A Trajectory Frame
    struct TrajectoryFrame {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        bool success = true;
        double begin_timestamp = 0.0;
        double end_timestamp = 1.0;
        Eigen::Matrix3d begin_R;
        Eigen::Vector3d begin_t;
        Eigen::Matrix3d end_R;
        Eigen::Vector3d end_t;

        inline double EgoAngularDistance() const {
            return AngularDistance(begin_R, end_R);
        }

        double TranslationDistance(const TrajectoryFrame &other) {
            return (begin_t - other.begin_t).norm() + (end_t - other.end_t).norm();
        }

        double RotationDistance(const TrajectoryFrame &other) {
            return (begin_R * other.begin_R.inverse() - Eigen::Matrix3d::Identity()).norm() +
                   (end_R * other.end_R.inverse() - Eigen::Matrix3d::Identity()).norm();
        }

        TrajectoryFrame() = default;

        [[nodiscard]] inline Eigen::Matrix4d MidPose() const {
            Eigen::Matrix4d mid_pose = Eigen::Matrix4d::Identity();
            auto q_begin = Eigen::Quaterniond(begin_R);
            auto q_end = Eigen::Quaterniond(end_R);
            Eigen::Vector3d t_begin = begin_t;
            Eigen::Vector3d t_end = end_t;
            Eigen::Quaterniond q = q_begin.slerp(0.5, q_end);
            q.normalize();
            mid_pose.block<3, 3>(0, 0) = q.toRotationMatrix();
            mid_pose.block<3, 1>(0, 3) = 0.5 * t_begin + 0.5 * t_end;
            return mid_pose;
        }
    };

    // Voxel
    // Note: Coordinates range is in [-32 768, 32 767]
    struct Voxel {

        Voxel() = default;

        Voxel(short x, short y, short z) : x(x), y(y), z(z) {}

        bool operator==(const Voxel &vox) const { return x == vox.x && y == vox.y && z == vox.z; }

        inline bool operator<(const Voxel &vox) const {
            return x < vox.x || (x == vox.x && y < vox.y) || (x == vox.x && y == vox.y && z < vox.z);
        }

        inline static Voxel Coordinates(const Eigen::Vector3d &point, double voxel_size) {
            return {short(point.x() / voxel_size),
                    short(point.y() / voxel_size),
                    short(point.z() / voxel_size)};
        }

        short x;
        short y;
        short z;
    };

    typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> ArrayVector3d;
    typedef std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> ArrayMatrix4d;
    typedef ArrayMatrix4d ArrayPoses;

    struct VoxelBlock {

        explicit VoxelBlock(int num_points = 20) : num_points_(num_points) { points.reserve(num_points); }

        ArrayVector3d points;

        bool IsFull() const { return num_points_ == points.size(); }

        void AddPoint(const Eigen::Vector3d &point) {
            CHECK(num_points_ >= points.size()) << "Voxel Is Full";
            points.push_back(point);
        }

        inline int NumPoints() const { return points.size(); }

        inline int Capacity() { return num_points_; }

    private:
        int num_points_;
    };

    typedef tsl::robin_map<Voxel, VoxelBlock> VoxelHashMap;

} // namespace Elastic_ICP

// Specialization of std::hash for our custom type Voxel
namespace std {

    template<>
    struct hash<ct_icp::Voxel> {
        std::size_t operator()(const ct_icp::Voxel &vox) const {
#ifdef CT_ICP_IS_WINDOWS
            const std::hash<int32_t> hasher;
            return ((hasher(vox.x) ^ (hasher(vox.y) << 1)) >> 1) ^ (hasher(vox.z) << 1) >> 1;
#else
            const size_t kP1 = 73856093;
            const size_t kP2 = 19349669;
            const size_t kP3 = 83492791;
            return vox.x * kP1 + vox.y * kP2 + vox.z * kP3;
#endif
        }
    };
}

#endif //CT_ICP_TYPES_HPP


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main(int argc, char const *argv[])
{
  std::filesystem::path p("config.init");
  if ( argc > 1 ) {
    p = argv[1];
  }
  auto c = config(p);
  std::cout << " " << c.output_dir << std::endl;
  c.name = "";

  c.create_output_directory();
  std::ofstream ofconfig( c.output_dir / "config.init" );
  ofconfig << c << "\n";
  ofconfig.close();

/* --------------------------------------------------------------- */
  field<double,1> f(boost::extents[c.Nv][c.Nx]);
  double Kx = 0.5;

  f.range.v_min = -8.; f.range.v_max = 8.;
  //f.range.x_min =  0.; f.range.x_max = 2./Kx*math::pi<double>();
  f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
  f.compute_steps();

  ublas::vector<double> v (c.Nv,0.);
  for ( std::size_t k=0 ; k<c.Nv ; ++k ) { v[k] = Vk(k); }

  ublas::vector<double> kx(c.Nx);
  {
    double l = f.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  //double alpha = 0.2 , ui = 2.0;
  auto landau_M = maxwellian( 1.        ,  0.   , 1.   );
  auto db_M1    = maxwellian( 0.5       , -c.ui , 1.   ) , db_M2  = maxwellian( 0.5       ,  c.ui , 1.  );
  //auto bot_M1   = maxwellian( 1.-alpha  ,  0. , 1.   ) , bot_M2 = maxwellian( alpha     ,  ui , 0.25);
  auto bot_M1   = maxwellian(0.9,0.,1.) , bot_M2 = maxwellian(0.2,4.5,0.25);
  auto tb_MC    = maxwellian( 1.-c.alpha  ,  0. , c.Tc ) ,
       tb_M1    = maxwellian( 0.5*c.alpha ,  c.ui , 1.   ) , tb_M2  = maxwellian( 0.5*c.alpha , -c.ui , 1.  );
  auto v10_MC   = maxwellian( 1.-c.alpha  ,  0. , c.Tc ) , v10_Mh = maxwellian( c.alpha     ,  0. , 1.  );

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      //f[k][i] = ((1.-alpha)*M0c(Xi(i),Vk(k)) + 0.5*alpha*( Mpu(Xi(i),Vk(k)) + Mmu(Xi(i),Vk(k)) ))*(1.+0.01*std::cos(0.5*Xi(i)));

      //// landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// strong landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.6*std::cos(Kx*Xi(i)));
      //// double beam Kx=0.2, ui=2.4 ou Kx=0.2, ui=4.5
      //f[k][i] = (db_M1(Xi(i),Vk(k))+db_M2(Xi(i),Vk(k)))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// bot Kx=0.5 , alpha=0.2 , ui=4.5
      //f[k][i] = (bot_M1(Xi(i),Vk(k)) + bot_M2(Xi(i),Vk(k)))*(1.+0.04*std::cos(0.3*Xi(i)));
      //// tb Kx=0.5 , ui=4. , alpha=0.2 , Tc=0.01
      f[k][i] = tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //// v10 Kx=0.5 , alpha=0.2
      //f[k][i] = v10_MC(Xi(i),Vk(k)) + ( std::pow(Vk(k),10)*v10_Mh(Xi(i),Vk(k))/945. )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write( c.output_dir / "init.dat" );

  unsigned int i_t = 0;
  double current_time = 0.;
  double dt =  0.1*f.step.dv/0.6;
  //double dt = 1.433*f.step.dv;

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> Ec;   Ec.reserve(int(std::ceil(c.Tf/dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/dt))+1);

  // lambda to save data, any time you want, where you want
  auto save_data = [&] ( std::string && suffix ) -> void {
    suffix = c.name + suffix;

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    f.write( c.output_dir / ("vp_"+suffix+".dat") );
  };

  // to stop simulation at any time (and save data)
  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[38;5;202msave data...\033[0m\n";
    save_data("_SIGINT");
  });

  // space scheme
  auto wenol = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return wenolin::trp_v(f,E); };
  auto weno  = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return weno::trp_v(f,E); };
  auto cd2   = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return o2::trp_v(f,E); };

  // time scheme init
  //expRK::HochbruckOstermann<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2);
  //expRK::Krogstad<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2);
  //lawson::RK33<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,weno);
  lawson::RK44<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,weno);

  rk.E = rk.poisson_solver(f.density());
  {
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );
    Ec.push_back( kinetic_energy(f) );
    times.push_back( 0. );
  }

  while (  current_time < c.Tf ) {
    std::cout<<" ["<<std::setw(5)<<i_t<<"] "<< current_time <<"\r"<<std::flush;

    f = rk(f,dt);

    // end of time loop

    // MONITORING
    rk.E = rk.poisson_solver(f.density());
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );
    Ec.push_back( kinetic_energy(f) );

    //dt = std::min( 0.1 , SIGMA*f.step.dv/Emax[i_t] );

    // increment time
    ++i_t;
    current_time += dt;
    times.push_back( current_time );
  } // while (  i_t*dt < Tf )
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt <<std::endl;

  //f.write( c.output_dir / "vp.dat" );

  save_data("");
/*
  of.open( c.output_dir / "ee.dat" );
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Emax.dat" );
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "H.dat" );
  std::transform( H.begin() , H.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Ec.dat" );
  std::transform( Ec.begin() , Ec.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();
*/
  std::ofstream of;
  auto dx_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<< f.step.dx*(count++) <<" "<<y; return ss.str(); };

  of.open( c.output_dir / "E.dat" );
  std::transform( rk.E.begin() , rk.E.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  auto rho = f.density();
  of.open( c.output_dir / "rho.dat" );
  std::transform( rho.begin() , rho.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  auto J = f.courant();
  of.open( c.output_dir / "J.dat" );
  std::transform( J.begin() , J.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  of.open( c.output_dir / "energy.dat" );
  for ( auto i=0 ; i<times.size() ; ++i ) {
    of << times[i] << " " << ee[i] << " " << Ec[i] << " " << H[i] << " " << ee[i] + Ec[i] << "\n";
  }
  of.close();

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] -= tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write( c.output_dir / "diff.dat" );

  return 0;
}


/*!
* \file GrapheneFloquet.cpp
*
*
* \author Author: D. Gagnon <denisg6@hotmail.com>
*/

// Include some headers
#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <complex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

#include "GrapheneFloquet.hpp"
#include "Utils.hpp"

/// Main function
int main(int argc, char *argv[])
{

    // Parse parameter file
    boost::property_tree::ptree pt;
    boost::property_tree::ini_parser::read_ini("GrapheneFloquet.ini", pt);

    // Problem parameters
    double freq  =          std::stof(pt.get<std::string>("Parameters.frequency"));  // Angular frequency in Hz
    double E0 =             std::stof(pt.get<std::string>("Parameters.E0"));         // Peak electric field in V/m
    size_t nblocks =  std::stoi(pt.get<std::string>("Parameters.nblocks"));    // Envelope frequency, in units of freq

    // Parameter sweep
    double xmin =   std::stof(pt.get<std::string>("Sweep.xmin")); // Minimum frequency in sweep
    double xmax =   std::stof(pt.get<std::string>("Sweep.xmax")); // Minimum frequency in sweep
    size_t x_elem =    std::stoi(pt.get<std::string>("Sweep.xnum"));

    // Parameter sweep
    double ymin =   std::stof(pt.get<std::string>("Sweep.ymin")); // Minimum frequency in sweep
    double ymax =   std::stof(pt.get<std::string>("Sweep.ymax")); // Minimum frequency in sweep
    size_t y_elem =    std::stoi(pt.get<std::string>("Sweep.ynum"));

    // Process input variables (angular frequency units)
    double omega = 2.0*M_PI*freq;     // Angular frequency

    // Meshgrid (vectors of parameters)
    auto xvec = arma::linspace(xmin, xmax, x_elem);
    auto yvec = arma::linspace(ymin, ymax, y_elem);

    // Prepare output file
    // std::ofstream outfile;
    // outfile.open("quasienergies.dat");

    // Prepare output file for transition probabilities
    std::ofstream outfile2;
    // outfile2.open("kmap.dat");

    // Variables to store probability values
    auto probvec = arma::mat(y_elem,x_elem);
    auto probvec_sigma = arma::mat(y_elem,x_elem);

    // Variables to store quasienergy values
    auto quasi0 = arma::mat(y_elem,x_elem);
    auto quasi1 = arma::mat(y_elem,x_elem);
    auto quasi2 = arma::mat(y_elem,x_elem);

    // Variables for loops
    size_t id, id2;
    double prob = 0.0;
    double prob_sigma = 0.0; // Trans. prob. between sigma_z eigenstates

    // Loop for each K value and compute probability
    # pragma omp parallel for default(shared) private (id, id2, prob, prob_sigma)
    for (id=0; id < y_elem; id++)
    {
        // Initialize probabilities (will be passed by reference)
        for (id2=0; id2 < x_elem; id2++)
        {
            // Compute energies and transition probability
            auto Energies = QuasiEnergies(xvec[id2],yvec[id],omega,E0,nblocks, prob, prob_sigma);

            // Store in arrays
            probvec(id,id2) = prob;
            probvec_sigma(id,id2) = prob_sigma;

            quasi0(id,id2) = Energies[Energies.n_elem/2 - 1]; // Quasi-energ.
            quasi1(id,id2) = Energies[Energies.n_elem/2];
            quasi2(id,id2) = Energies[Energies.n_elem/2 + 1];
        }

    }

    // Save Floquet Data
    probvec.save("probability.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    quasi0.save("quasi0.dat", arma::raw_ascii);
    quasi1.save("quasi1.dat", arma::raw_ascii);
    quasi2.save("quasi2.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    return 0;

    /*
    auto Gamma = GammaValues(2.30,0.0,316227766016837.94,1e10,1);

    for(auto it = Gamma.begin(); it != Gamma.end(); it++)
    {
        std::cout << *it << std::endl;
    }

    double prob = 0.0;
    double prob_sigma = 0.0;
    auto Quasi = QuasiEnergies(2.30,0.0,316227766016837.94,1e10,1, prob, prob_sigma);
    */
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_math_nrand_hpp__
#define __multicontact_api_math_nrand_hpp__

#include <cmath>
#include <cstdlib>
#include <boost/math/constants/constants.hpp>

namespace multicontact_api
{
  namespace math
  {

    template<typename Scalar>
    inline Scalar nrand()
    {
      const Scalar two_pi = 2 * boost::math::constants::pi<Scalar>();
      const Scalar eps = std::numeric_limits<Scalar>::min();
      using std::rand;
      using std::cos;
      using std::sqrt;
      using std::log;

      Scalar u1, u2;
      const Scalar RAND_MAX_INV = (1.0 / RAND_MAX);
      do
      {
        u1 = (Scalar)rand() * RAND_MAX_INV;
        u2 = (Scalar)rand() * RAND_MAX_INV;
      }
      while(u1 <= eps);

      Scalar z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);

      return z0;

    }
  }
}

#endif // ifndef __multicontact_api_math_nrand_hpp__


//
// Created by dominik on 25.06.21.
//

#ifndef SQSGENERATOR_STRUCTURE_UTILS_HPP
#define SQSGENERATOR_STRUCTURE_UTILS_HPP

#include "types.hpp"
#include "utils.hpp"
#include <vector>
#include <limits>
#include <algorithm>
#include <boost/multi_array.hpp>
#include <boost/log/trivial.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using namespace boost;
using namespace boost::numeric::ublas;

namespace sqsgenerator::utils {

        template<typename T>
        multi_array<T, 3> pbc_shortest_vectors(const matrix<T> &lattice, const matrix<T> &coords, bool frac_coords = false){
            const matrix<T> cart_coords(frac_coords ? prod(coords, lattice): coords);
            auto num_atoms {static_cast<index_t>(coords.size1())};
            auto a {row(lattice, 0)};
            auto b {row(lattice, 1)};
            auto c {row(lattice, 2)};

            std::vector<int> axis {-1, 0, 1};
            multi_array<T, 3> vecs(boost::extents[num_atoms][num_atoms][3]);
            // pi1 = position_index_1
            // pi2 = position_index_2
            for (index_t pi1 = 0; pi1 < num_atoms; pi1++) {
                auto p1 {row(cart_coords, pi1)};
                for (index_t pi2 = pi1+1; pi2 < num_atoms; pi2++) {
                    auto p2 {row(cart_coords, pi2)};
                    T norm {std::numeric_limits<T>::max()};
                    for (auto &i : axis) {
                        for (auto &j: axis) {
                            for (auto &k: axis) {
                                auto t = i * a + j * b + k * c;
                                auto diff = p1 - (t + p2);
                                T image_norm {norm_2(diff)};
                                if (image_norm < norm) {
                                    norm = image_norm;
                                    for (index_t dim = 0; dim < 3; dim++) {
                                        vecs[pi1][pi2][dim] = diff(dim);
                                        vecs[pi2][pi1][dim] = -diff(dim);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            return vecs;
        }

        template<typename T>
        multi_array<T, 3> pbc_shortest_vectors(const std::vector<T> &lattice, const std::vector<T> &coords,  bool frac_coords = false){
            assert(lattice.size() == 9);
            assert(coords.size() % 3 == 0);
            return pbc_shortest_vectors(matrix_from_vector(3, 3, lattice),
                                        matrix_from_vector(coords.size() / 3, 3, coords), frac_coords);
        }

        template<typename MultiArray>
        multi_array<typename MultiArray::element, 2> distance_matrix(const MultiArray &vecs){
            typedef typename MultiArray::index index_t;
            typedef typename MultiArray::element T;
            auto shape(shape_from_multi_array(vecs));
            auto num_atoms {static_cast<index_t>(shape[0])};
            multi_array<T, 2> d2(boost::extents[num_atoms][num_atoms]);
            for (index_t i = 0; i < num_atoms; i++) {
                for (index_t j = i; j < num_atoms; j++) {
                    T norm = std::sqrt(
                            vecs[i][j][0]*vecs[i][j][0] +
                            vecs[i][j][1]*vecs[i][j][1] +
                            vecs[i][j][2]*vecs[i][j][2]);
                    d2[i][j] = norm;
                    d2[j][i] = norm;
                }
            }
            return d2;
        }

        template<typename MultiArray>
        pair_shell_matrix_t shell_matrix(const MultiArray &distance_matrix, const std::vector<typename MultiArray::element> &distances, typename MultiArray::element atol = 1.0e-5, typename MultiArray::element rtol=1.0e-8) {

            typedef typename MultiArray::index index_t;
            typedef typename MultiArray::element T;
            auto shape(shape_from_multi_array(distance_matrix));
            auto num_atoms {static_cast<index_t>(shape[0])};
            pair_shell_matrix_t shells(boost::extents[num_atoms][num_atoms]);
            auto is_close_tol = [&atol, &rtol] (T a, T b) {
                return is_close(a, b, atol, rtol);
            };

            auto find_shell = [&distances, &is_close_tol] (T distance) {
                if (distance < 0 ) return -1;
                if (is_close_tol(distance, 0.0)) return 0;
                else {
                    for (size_t i = 0; i < distances.size() - 1; i++) {
                        T lower_bound {distances[i]}, upper_bound {distances[i+1]};
                        if ((is_close_tol(distance, lower_bound) or distance > lower_bound) and (is_close_tol(distance, upper_bound) or upper_bound > distance)) {
                            return static_cast<int>(i+1);
                        }
                    }
                }
                return static_cast<int>(distances.size());
            };

            for (index_t i = 0; i < num_atoms; i++) {
                for (index_t j = i + 1; j < num_atoms; j++) {
                    int shell {find_shell(distance_matrix[i][j])};
                    if (shell < 0) throw std::runtime_error("A shell was detected which I am not aware of");
                    else if (shell == 0 and i != j) {
                        BOOST_LOG_TRIVIAL(warning) << "Atoms " + std::to_string(i) + " and " + std::to_string(j) + " are overlapping! (distance = " + std::to_string(distance_matrix[i][j])<< ", shell = " << shell <<")!";
                    }
                    shells[i][j] = shell;
                    shells[j][i] = shell;
                }
            }
            return shells;
        }

    template<typename MultiArray>
    std::vector<typename MultiArray::element> default_shell_distances(const MultiArray &distance_matrix, typename MultiArray::element atol = 1.0e-5, typename MultiArray::element rtol=1.0e-8) {
        typedef typename MultiArray::index index_t;
        typedef typename MultiArray::element T;
        auto shape(shape_from_multi_array(distance_matrix));
        auto num_atoms {static_cast<index_t>(shape[0])};
        std::vector<T> all_distances(distance_matrix.data(), distance_matrix.data() + distance_matrix.num_elements());
        std::sort(all_distances.begin(), all_distances.end());
        std::vector<T> shell_dists;

        auto is_close_tol = [=] (T a, T b) {
            return is_close(a, b, atol, rtol);
        };

        std::function<int(T)> get_shell_index  = [&](T distance){
            for (auto i = 0; i < shell_dists.size(); i++)  if (is_close_tol(distance, shell_dists[i])) return i;
            return -1;
        };

        for (const auto& distance : all_distances) {
            auto shell_dist_index {get_shell_index(distance)};
            // We average the distances
            if (shell_dist_index >= 0) shell_dists[shell_dist_index] = 0.5 * (shell_dists[shell_dist_index] + distance);
            else shell_dists.push_back(distance);
            std::sort(shell_dists.begin(), shell_dists.end());
        }

        // make a sanity check -> we check here that each of the computed coordination shell occurs at least once
        pair_shell_matrix_t current_shell_matrix(shell_matrix(distance_matrix, shell_dists, atol, rtol));
        std::set<shell_t> unique_shells(current_shell_matrix.data(), current_shell_matrix.data() + current_shell_matrix.num_elements());
        if (unique_shells.size() < shell_dists.size()) {
            auto message = "The number of of computed (default) shells does not match the occuring shells in the shell matrix (computed: " + std::to_string(shell_dists.size()) + " !=" + " shell_matrix: " + std::to_string(unique_shells.size()) + ")";
            BOOST_LOG_TRIVIAL(warning) << message;
            throw std::runtime_error(message);
        }
        return shell_dists;
    }

    std::map<shell_t, index_t> shell_index_map(const pair_shell_weights_t &weights);
    std::vector<AtomPair> create_pair_list(const pair_shell_matrix_t &shell_matrix, const std::map<shell_t, double> &weights);
    std::tuple<std::vector<shell_t>, std::vector<double>> compute_shell_indices_and_weights(const pair_shell_weights_t &shell_weights);
    array_3d_t compute_prefactors(const_pair_shell_matrix_ref_t shell_matrix, const pair_shell_weights_t &shell_weights, const configuration_t &configuration);
}

#endif //SQSGENERATOR_STRUCTURE_UTILS_HPP


#include <iostream>
#include <cinttypes>
#include <vector>
#include <numeric>
#include <iomanip>

using namespace std;

#include <boost/math/distributions/normal.hpp>

double epsilon = 1e-7;
double epsilon2 = 1e-24;

double solve(double a, int64_t till) {
    double answer(0.5);
    vector<pair<double, double>> vals{make_pair(0.0, 0.5)};
    double remaining((M_PI * M_PI) / 6 - 1);
#if 0
    double prev_answer = 0;
#endif
    for (int64_t i = 2; i <= till; ++i) {
        double component = ((double)1.0) / ((double)i) / i;
        vector<pair<double, double>> new_vals;
        for (auto v: vals) {
            if (v.first + remaining < a) {
                continue;
            }

            if (v.first + component > a) {
                answer += v.second / 2.0;
                new_vals.push_back(make_pair(v.first, v.second / 2.0));
            } else {
                new_vals.push_back(make_pair(v.first, v.second / 2.0));
                new_vals.push_back(make_pair(v.first + component, v.second / 2.0));
            }
        }
        remaining -= component;

        sort(new_vals.begin(), new_vals.end(), greater<pair<double, double>>());
        vals.clear();
        for (auto v: new_vals) {
            if (v.second < epsilon2) {
                continue;
            }
            if (vals.empty() || (vals.back().first - v.first) > epsilon) {
                vals.push_back(v);
            } else {
                vals.back().first = vals.back().first * vals.back().second + v.first * v.second;
                vals.back().second += v.second;
                vals.back().first /= vals.back().second;
            }
        }

        if (vals.empty()) {
            break;
        }
#if 0
        if (answer - prev_answer > 1e-8) {
            cout << i << ": " << answer << ", " << answer - prev_answer << " (" << vals.size() << ")" << endl;
            prev_answer = answer;
        }
#endif
    }

    // Approximate by normal distribution.
    boost::math::normal_distribution<double> nd(remaining / 2.0, remaining / boost::math::constants::pi<double>() / 2);
    if (!vals.empty()) {
        for (auto v: vals) {
            answer += v.second * (1.0 - boost::math::cdf(nd, a - v.first));
        }
    }

    return answer;
}

int main() {
    cout << setprecision(15);

    //double answer = solve(0.5, 10000000);
    double answer = solve(0.5, 3000);
    cout << setprecision(8);
    cout << answer << endl;
}


#include <simulation.hpp>

#include <boost/numeric/odeint.hpp>

#include "eigenIntegration.hpp"

namespace scpp
{

class ODE
{
public:
    ODE(Model::ptr_t model, double dt, const Model::input_vector_t &u0, const Model::input_vector_t &u1);
    void operator()(const Model::state_vector_t &f, Model::state_vector_t &dfdt, const double t);

private:
    Model::ptr_t model;
    Model::input_vector_t u0, u1;
    double dt;
};

ODE::ODE(Model::ptr_t model, double dt, const Model::input_vector_t &u0, const Model::input_vector_t &u1)
    : model(model), u0(u0), u1(u1), dt(dt) {}

void ODE::operator()(const Model::state_vector_t &f, Model::state_vector_t &dfdt, const double t)
{
    Model::input_vector_t u = u0 + t / dt * (u1 - u0);
    model->computef(f, u, dfdt);
}

void simulate(Model::ptr_t model, double dt,
              const Model::input_vector_t &u0,
              const Model::input_vector_t &u1,
              Model::state_vector_t &x)
{
    using namespace boost::numeric::odeint;
    runge_kutta_fehlberg78<Model::state_vector_t, double, Model::state_vector_t, double, vector_space_algebra> stepper;

    ODE ode(model, dt, u0, u1);

    integrate_adaptive(stepper, ode, x, 0., dt, dt / 20.);
}

} // namespace scpp