#define BOOST_SPIRIT_USE_PHOENIX_V3
#include <boost/variant/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_no_case.hpp>
#include <boost/spirit/include/phoenix.hpp>

#include <random>
#include <string>

#define WIN32_LEAN_AND_MEAN
#define NOMINMAX
#include <sdkddkver.h>
#include <Windows.h>

#include "weather.h"
#include "perlin.h"
#include "misc.h"

using namespace boost::spirit;

template <typename Iterator>
struct grammar : public qi::grammar<Iterator, double(), ascii::space_type>
{
	qi::rule<Iterator, double(), ascii::space_type> expr, term, fctr;
	double time;
	grammar(double t) : grammar::base_type(expr), time(t)
	{
		expr = term[_val = _1] >> *(('+' >> term[_val += _1]) | ('-' >> term[_val -= _1]));
		term = fctr[_val = _1] >> *(('*' >> fctr[_val *= _1]) | ('/' >> fctr[_val /= _1]));
		fctr = double_[_val = _1]
			| '(' >> expr[_val = _1] >> ')'
			| qi::no_case[qi::lit('t')][_val = time]
			| qi::no_case["cloud("] >> expr[_val = boost::phoenix::bind(cloud_, _1)] >> ")"
			| qi::no_case["rain("] >> expr[_val = boost::phoenix::bind(rain_, _1)] >> ")"
			| qi::no_case["wind_speed("] >> expr[_val = boost::phoenix::bind(wind_speed_, _1)] >> ")"
			| qi::no_case["air_temp("] >> expr[_val = boost::phoenix::bind(air_temp_, _1)] >> ")"
			;
	}
};

double rain_chance_;
double min_air_temp_;
double max_air_temp_;

perlin::noise cloud_;
perlin::noise rain_;
perlin::noise wind_speed_;
perlin::noise air_temp_;

std::string cloud_expr_;
std::string rain_expr_;
std::string wind_speed_expr_;
std::string air_temp_expr_;

std::random_device rd_;
std::mt19937 mt_;
bool rainfall_ = false;
bool stoprain_ = false;

void weather_init(unsigned int seed, weather_config *config)
{
	if (seed == 0)
		seed = rd_();
	mt_.seed(seed);

	if (config != nullptr)
	{
		rain_chance_ = config->rain_chance;
		min_air_temp_ = config->min_air_temp;
		max_air_temp_ = config->max_air_temp;

		cloud_.set_seed(mt_());
		cloud_.set_zero(config->cloud.zero);
		cloud_.set_amplitude(config->cloud.amplitude);
		cloud_.set_min_value(config->cloud.min);
		cloud_.set_max_value(config->cloud.max);
		cloud_.set_frequency(1.0 / config->cloud.period);
		cloud_.set_octave(config->cloud.octave);
		cloud_expr_ = config->cloud.expression != nullptr
			? config->cloud.expression : "cloud(T)";

		rain_.set_seed(mt_());
		rain_.set_zero(config->rain.zero);
		rain_.set_amplitude(config->rain.amplitude);
		rain_.set_min_value(config->rain.min);
		rain_.set_max_value(config->rain.max);
		rain_.set_frequency(1.0 / config->rain.period);
		rain_.set_octave(config->rain.octave);
		rain_expr_ = config->rain.expression != nullptr
			? config->rain.expression : "rain(T)";

		wind_speed_.set_seed(mt_());
		wind_speed_.set_zero(config->wind_speed.zero);
		wind_speed_.set_amplitude(config->wind_speed.amplitude);
		wind_speed_.set_min_value(config->wind_speed.min);
		wind_speed_.set_max_value(config->wind_speed.max);
		wind_speed_.set_frequency(1.0 / config->wind_speed.period);
		wind_speed_.set_octave(config->wind_speed.octave);
		wind_speed_expr_ = config->wind_speed.expression != nullptr
			? config->wind_speed.expression : "wind_speed(T)";

		air_temp_.set_seed(mt_());
		air_temp_.set_zero(config->air_temp.zero);
		air_temp_.set_amplitude(config->air_temp.amplitude);
		air_temp_.set_min_value(config->air_temp.min);
		air_temp_.set_max_value(config->air_temp.max);
		air_temp_.set_frequency(1.0 / config->air_temp.period);
		air_temp_.set_octave(config->air_temp.octave);
		air_temp_expr_ = config->air_temp.expression != nullptr
			? config->air_temp.expression : "air_temp(T)";
	}
}

void weather_simulate(double time, weather_info *info)
{
	double c = 0, r = 0, w = 0, a = 0;
	grammar<std::string::iterator> g(time);
	qi::phrase_parse(cloud_expr_.begin(), cloud_expr_.end(), g, ascii::space, c);
	qi::phrase_parse(rain_expr_.begin(), rain_expr_.end(), g, ascii::space, r);
	qi::phrase_parse(wind_speed_expr_.begin(), wind_speed_expr_.end(), g, ascii::space, w);
	qi::phrase_parse(air_temp_expr_.begin(), air_temp_expr_.end(), g, ascii::space, a);

	r = clamp<double>(r, rain_.min_value(), c);
	a *= (max_air_temp_ - min_air_temp_) / 2;
	a += min_air_temp_ + (max_air_temp_ - min_air_temp_) / 2;

	if (!rainfall_)
	{
		if (0 < r)
		{
			if (!stoprain_)
			{
				std::uniform_real_distribution<double> chance(0, 1);
				if (rain_chance_ < chance(mt_))
				{
					stoprain_ = true;
				}
			}
		}
		else
		{
			stoprain_ = false;
		}
	}

	r = stoprain_ ? 0 : r;
	rainfall_ = 0 < r;

	info->cloud = c;
	info->rain = r;
	info->air_temp = a;
	info->wind_speed = w;
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Geomlib_Rhodo.h"

#include <Eigen/Core>

#include <Urho3D/Core/Variant.h>
#include <Urho3D/Math/Vector3.h>

Geomlib::Rhodo::Rhodo(Rhodo& originalRhodo, int jumpIndex)
{
	Eigen::RowVector3i unitlessCoordinates = originalRhodo.ComputeUnitlessCoordinates()
		+ RhodoEigen::jumpsToNeighbors[jumpIndex];
	family_ = ComputeFamilyFromUnitlessCoordinates(unitlessCoordinates);
	xDiamJumps_ = (unitlessCoordinates(0) - RhodoEigen::iGeneratingRhodos[family_](0)) / 4;
	yDiamJumps_ = (unitlessCoordinates(1) - RhodoEigen::iGeneratingRhodos[family_](1)) / 4;
	zDiamJumps_ = (unitlessCoordinates(2) - RhodoEigen::iGeneratingRhodos[family_](2)) / 4;
}

Geomlib::Rhodo::Rhodo(int xUnitless, int yUnitless, int zUnitless)
{
	Eigen::RowVector3i unitlessCoords(xUnitless, yUnitless, zUnitless);
	family_ = ComputeFamilyFromUnitlessCoordinates(unitlessCoords);
	Eigen::RowVector3i generatingCenter = RhodoEigen::iGeneratingRhodos[family_];
	xDiamJumps_ = (unitlessCoords[0] - generatingCenter[0]) / 4;
	yDiamJumps_ = (unitlessCoords[1] - generatingCenter[1]) / 4;
	zDiamJumps_ = (unitlessCoords[2] - generatingCenter[2]) / 4;
}

Eigen::RowVector3f Geomlib::Rhodo::ComputeCoordinates(float s) const
{
	return s * (RhodoEigen::fGeneratingRhodos[family_] +
		(4 * (float)xDiamJumps_) * Eigen::RowVector3f(1.0f, 0.0f, 0.0f) +
		(4 * (float)yDiamJumps_) * Eigen::RowVector3f(0.0f, 1.0f, 0.0f) +
		(4 * (float)zDiamJumps_) * Eigen::RowVector3f(0.0f, 0.0f, 1.0f));
}

Eigen::RowVector3i Geomlib::Rhodo::ComputeUnitlessCoordinates() const
{
	return RhodoEigen::iGeneratingRhodos[family_] +
		(4 * xDiamJumps_) * Eigen::RowVector3i(1, 0, 0) +
		(4 * yDiamJumps_) * Eigen::RowVector3i(0, 1, 0) +
		(4 * zDiamJumps_) * Eigen::RowVector3i(0, 0, 1);
}

Urho3D::VariantVector Geomlib::Rhodo::ComputeVertices(float scale) const
{
	Urho3D::VariantVector tmp;

	Eigen::RowVector3i uCenter = ComputeUnitlessCoordinates();

	for (unsigned i = 0; i < 14; ++i) {
		Eigen::RowVector3i vert = uCenter + RhodoEigen::rhodoVertexCoords[i];
		Urho3D::Vector3 baseCoords(scale * vert(0), scale * vert(1), scale * vert(2));
		tmp.Push(baseCoords);
	}

	return tmp;
}

Urho3D::VariantVector Geomlib::Rhodo::ComputeRhombicPolylines(float scale) const
{
	Urho3D::VariantVector tmp;

	Eigen::RowVector3i uCenter = ComputeUnitlessCoordinates();
	Eigen::RowVector3f fCenter = scale * Eigen::RowVector3f((float)uCenter(0), (float)uCenter(1), (float)uCenter(2));

	// Loop across the 12 rhombic faces of the rhombic dodecahedron
	for (unsigned i = 0; i < 12; ++i) {
		// Get rhombic face i
		const Eigen::RowVector3f* rhombus = RhodoEigen::neighborRhombi[i];
		Urho3D::VariantMap rhombusPolyLine;
		Urho3D::VariantVector verts;
		// Loop over the 4 vertices of this rhombus
		for (unsigned j = 0; j < 4; ++j) {
			Eigen::RowVector3f v = scale * rhombus[j] + fCenter;

			Urho3D::Vector3 vert(v(0), v(1), v(2));
			verts.Push(vert);
		}
		// Close the rhombus polyline by adding the initial vertex again at the end
		Eigen::RowVector3f v = scale * rhombus[0] + fCenter;
		Urho3D::Vector3 vert(v(0), v(1), v(2));
		verts.Push(vert);

		rhombusPolyLine["vertices"] = verts;
		tmp.Push(rhombusPolyLine);
	}

	return tmp;
}

// hackish, depends on definition of generatingCenterDisplacements
int Geomlib::ComputeFamilyFromUnitlessCoordinates(const Eigen::RowVector3i& unitlessCoordinates)
{
	int a = unitlessCoordinates[0] % 4;
	int b = unitlessCoordinates[1] % 4;
	if (a == 0) {
		if (b == 0) {
			return 0;
		}
		else {
			return 1;
		}
	}
	else {
		if (b == 0) {
			return 2;
		}
		else {
			return 3;
		}
	}
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Particle_.h"

#include "ParsParticle.h"
#include "ParticleInitialCondition.h"

#include "LazyDensityOperatorFFT.h"
#include "TridiagonalHamiltonian.h"

#include <boost/math/special_functions/hermite.hpp>

#include "TMP_Tools.h"

#include <boost/assign/list_of.hpp>
#include <boost/bind.hpp>
#include <utility>

using namespace std;
using namespace mathutils;
using namespace cpputils;
using namespace fft;

////////
//
// Exact
//
////////

namespace {

const particle::Exact::Diagonal fExpFill(const particle::Spatial& space, double omrec)
{
  return particle::Exact::Diagonal(-DCOMP_I*omrec*blitz::sqr(space.getK()));
}

}

particle::Exact::Exact(const Spatial& space, double omrec)
  : FreeExact(space.getDimension()), details::Storage(space,omrec), factorExponents_(fExpFill(space,omrec))
{
}

void particle::Exact::updateU(structure::OneTime t) const
{
  getDiagonal()=exp(factorExponents_*t);
}

//////////////
//
// Hamiltonian
//
//////////////

namespace {

const particle::Tridiagonal expINKX(size_t dim, ptrdiff_t nK)
{
  using particle::Tridiagonal;

  typedef Tridiagonal::Diagonal Diag;

  ptrdiff_t D=dim-(nK>0 ? nK : -nK);
  if (D<0) return Tridiagonal();

  Diag temp(D); temp=1;
  if (nK>0) return Tridiagonal(Diag(),nK,temp);
  else if (nK==0) return Tridiagonal(temp);
  return Tridiagonal(Diag(),-nK,Diag(),temp);
}

const particle::Tridiagonal cosNKX(size_t dim, ptrdiff_t nK)
{
  return (expINKX(dim,nK)+expINKX(dim,-nK))/2.;
}

const particle::Tridiagonal hOverI(size_t dim, double vClass, const ModeFunction& mf)
{
  return (vClass && !isComplex(get<0>(mf))) ? vClass*(get<0>(mf)==MFT_SIN ? -1 : 1)*cosNKX(dim,get<1>(mf)<<1)/(2.*DCOMP_I) : particle::Tridiagonal();
}

const particle::Tridiagonal::Diagonal mainDiagonal(const particle::Spatial& space, double omrec)
{
  return particle::Tridiagonal::Diagonal(DCOMP_I*omrec*blitz::sqr(space.getK()));
}

}

namespace particle { // open it for the partial specializations

template<>
Hamiltonian<true >::Hamiltonian(const Spatial& space, double omrec, double vClass, const ModeFunction& mf)
  : Base(furnishWithFreqs(hOverI(space.getDimension(),vClass,mf),mainDiagonal(space,omrec))), Exact(space,omrec)
{
}

template<>
Hamiltonian<false>::Hamiltonian(const Spatial& space, double omrec, double vClass, const ModeFunction& mf)
  : Base(
         Tridiagonal(mainDiagonal(space,-omrec))
         +
         hOverI(space.getDimension(),vClass,mf)
         )
{
}

template<> template<>
Hamiltonian<false>::Hamiltonian(const Spatial& space, double omrec)
  : Base(
         Tridiagonal(mainDiagonal(space,-omrec))
         )
{
}

} // particle

///////////
//
// Averaged
//
///////////

particle::Averaged::Averaged(const Spatial& space)
  : Base("Particle",{"<P>","VAR(P)","<X>","DEV(X)"}),
    space_(space)
{
}

auto particle::Averaged::average_v(NoTime, const LazyDensityOperator& matrix) const -> const Averages
{
  int dim=space_.getDimension();

  auto averages(initializedAverages());

  const LazyDensityOperator::Ptr matrixX(quantumdata::ffTransform<tmptools::Vector<0> >(matrix,DIR_KX));

  for (int i=0; i<dim; i++) {

    double diag=matrix(i);
    averages(0)+=    space_.k(i) *diag;
    averages(1)+=sqr(space_.k(i))*diag;
  }

  for (int i=0; i<dim; i++) {
    double diag=matrixX->operator()(i);
    averages(2)+=    space_.x(i) *diag;
    averages(3)+=sqr(space_.x(i))*diag;
  }

  return averages;
}

void particle::Averaged::process_v(Averages& averages) const
{
  averages(1)-=sqr(averages(0));
  averages(3)=sqrt(averages(3)-sqr(averages(2)));
}

////////////////
//
// Highest level
//
////////////////

ParticleBase::ParticleBase(size_t fin,
                           const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : Free(1<<fin,realFreqs,complexFreqs), Averaged(particle::Spatial(fin))
{
  getParsStream()<<"Particle\n";
  getSpace().header(getParsStream());
}

PumpedParticleBase::PumpedParticleBase(size_t fin, double vClass, const ModeFunction& mf,
                                       const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : ParticleBase(fin,
                 boost::assign::list_of(*realFreqs.begin()).range(next(realFreqs.begin()),realFreqs.end())({"vClass",vClass,1.}),
                 complexFreqs),
    vClass_(vClass), mf_(mf)
{
  getParsStream()<<"Pump "<<mf<<endl;
}

Particle::Particle(const particle::Pars& p)
  : ParticleBase(p.fin,{RF{"omrec",p.omrec,1<<p.fin}}),
    Exact(getSpace(),p.omrec)
{
}

ParticleSch::ParticleSch(const particle::Pars& p)
  : ParticleBase(p.fin,{RF{"omrec",p.omrec,sqr(1<<p.fin)}}),
    Hamiltonian<false>(getSpace(),p.omrec)
{
  getParsStream()<<"Schroedinger picture.\n";
}

PumpedParticle::PumpedParticle(const particle::ParsPumped& p)
  : PumpedParticleBase(p.fin,p.vClass,ModeFunction(p.modePart,p.kPart),{RF{"omrec",p.omrec,1<<p.fin}}),
    Hamiltonian<true>(getSpace(),p.omrec,p.vClass,getMF())
{
}

PumpedParticleSch::PumpedParticleSch(const particle::ParsPumped& p)
  : PumpedParticleBase(p.fin,p.vClass,ModeFunction(p.modePart,p.kPart),{RF{"omrec" ,p.omrec,sqr(1<<p.fin)}}),
    Hamiltonian<false>(getSpace(),p.omrec,p.vClass,getMF())
{
  getParsStream()<<"Schroedinger picture.\n";
}

//////////
//
// Helpers
//
//////////

namespace {

using particle::Spatial;

const Spatial::Array fill(size_t fin, double d, double m)
{
  Spatial::Array res(1<<fin);
  return res=d*blitz::tensor::i-m;
}

}

particle::Spatial::Spatial(size_t fin, double deltaK)
  : fin_(fin),
    xMax_(PI/deltaK), deltaX_(2*xMax_/(1<<fin)), kMax_(PI/deltaX_), deltaK_(deltaK),
    x_(fill(fin,deltaX_,xMax_)), k_(fill(fin,deltaK_,kMax_))
{
}

void particle::Spatial::header(std::ostream& os) const
{
  os<<"Spatial Degree of Freedom finesse="<<fin_<<" xMax="<<xMax_<<" deltaX="<<deltaX_<<" kMax="<<kMax_<<" deltaK="<<deltaK_<<std::endl;
}

auto particle::expINKX(particle::Ptr particle, ptrdiff_t nK) -> const Tridiagonal
{
  size_t dim=particle->getDimension();
  Tridiagonal res(::expINKX(dim,nK));
  if (const auto exact=dynamic_cast<const particle::Exact*>(particle.get())) res.furnishWithFreqs(mainDiagonal(get<0>(*exact),get<1>(*exact)));
  return res;
}

auto particle::mfNKX(particle::Ptr particle, const ModeFunction& modeFunction) -> const Tridiagonal
{
  ModeFunctionType mf(get<0>(modeFunction));
  ptrdiff_t        nK(get<1>(modeFunction));
  switch (mf) {
  case MFT_SIN  : return sinNKX (particle,nK);
  case MFT_COS  : return cosNKX (particle,nK);
  case MFT_PLUS : return expINKX(particle,nK);
  case MFT_MINUS:                            ;
  }
  return expINKX(particle,-nK);
}

auto particle::mfComposition(particle::Ptr particle, const ModeFunction& modeFunction1, const ModeFunction& modeFunction2) -> const Tridiagonal
{
  ModeFunctionType mf1(get<0>(modeFunction1));
  ModeFunctionType mf2(get<0>(modeFunction2));
  ptrdiff_t        nK1(get<1>(modeFunction1));
  ptrdiff_t        nK2(get<1>(modeFunction2));
  ptrdiff_t        nK(abs(nK1));
  ptrdiff_t        sign1 = mf1==MFT_SIN && nK1<0 ? -1 : 1;
  ptrdiff_t        sign2 = mf2==MFT_SIN && nK2<0 ? -1 : 1;
  ptrdiff_t        sign  = sign1*sign2;

  if (abs(nK1) != abs(nK2)) throw std::runtime_error("Not a Tridiagonal");

  if (mf1==MFT_PLUS  && nK1<0) { mf1=MFT_MINUS; nK1*=-1; }
  if (mf1==MFT_MINUS && nK1<0) { mf1=MFT_PLUS;  nK1*=-1; }
  if (mf2==MFT_PLUS  && nK2<0) { mf2=MFT_MINUS; nK2*=-1; }
  if (mf2==MFT_MINUS && nK2<0) { mf2=MFT_PLUS;  nK2*=-1; }

  Tridiagonal id(quantumoperator::identity(particle->getSpace().getDimension()));

  typedef pair<ModeFunctionType,ModeFunctionType> MFPair;

  MFPair mfs(mf1,mf2);

  if (mfs == MFPair(MFT_SIN,MFT_SIN)){
    return sign * (id-cosNKX(particle,2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_COS,MFT_COS)){
    return (id+cosNKX(particle,2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_SIN,MFT_COS) || mfs == MFPair(MFT_COS,MFT_SIN)){
    return sign * sinNKX(particle,2*nK) / 2.;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_PLUS) || mfs == MFPair(MFT_MINUS,MFT_MINUS)){
    return id;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_MINUS)){
    return expINKX(particle,-2*nK);
  }
  if (mfs == MFPair(MFT_MINUS,MFT_PLUS)){
    return expINKX(particle,2*nK);
  }
  if (mfs == MFPair(MFT_PLUS,MFT_SIN) || mfs == MFPair(MFT_SIN,MFT_MINUS)) {
    return sign * (expINKX(particle, -2*nK)-id) * DCOMP_I/2.;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_COS) || mfs == MFPair(MFT_COS,MFT_MINUS)) {
    return (id + expINKX(particle,-2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_MINUS,MFT_COS) || mfs == MFPair(MFT_COS,MFT_PLUS)) {
    return (id + expINKX(particle, 2*nK)) / 2.;
  }
  if (!(mfs == MFPair(MFT_MINUS,MFT_SIN) || mfs == MFPair(MFT_SIN,MFT_PLUS))) {
    // this should never be reached
    throw std::logic_error("In mfComposition");
  }
  return -sign * (expINKX(particle,2*nK)-id) * DCOMP_I/2.;
}

/*
const Tridiagonal mfNKX_AbsSqr(ModeFunctionType mf, size_t dim, ptrdiff_t K)
{
  return (mfcomplex(mf) ? Identity(dim) : (mf==MFT_Sin ? -1 : 1)*cosNKX(dim,2*K))/2.;
  // The other 1/2 should be taken into account separately!
}
*/

auto particle::wavePacket(const InitialCondition& init, const Spatial& space, bool kFlag) -> StateVector
{
  double
    offset1=PI*init.getX0(),
    offset2=   init.getK0();

  if (init.isInK()) {swap(offset1,offset2); offset2=-offset2;}

  const Spatial::Array array(init.isInK() ? space.getK() : space.getX());

  StateVectorLow psiLow(exp(-blitz::sqr(array-offset1)/(4*sqr(init.getSig()))+DCOMP_I*array*offset2));

  if      ( kFlag && !init.isInK()) quantumdata::ffTransform(psiLow,DIR_XK);
  else if (!kFlag &&  init.isInK()) quantumdata::ffTransform(psiLow,DIR_KX);

  StateVector res(psiLow,quantumdata::byReference); res.renorm();

  return res;

}

auto particle::wavePacket(const Pars& p, bool kFlag) -> StateVector
{
  return wavePacket(p.init,Spatial(p.fin),kFlag);
}

namespace {

const particle::InitialCondition coherent(const particle::ParsPumped& p)
{
  return particle::InitialCondition(p.init.getX0(),p.init.getK0(),pow(p.omrec/fabs(p.vClass),.25)/sqrt(2),false);
}

}

auto particle::wavePacket(const ParsPumped& p, bool kFlag) -> StateVector
{
  if (p.init.getSig()) return wavePacket(static_cast<const Pars&>(p),kFlag);
  else                 return wavePacket(coherent(p),
                                         Spatial(p.fin),
                                         kFlag);
}

auto particle::hoState(int n, const InitialCondition& init, const Spatial& space, bool kFlag) -> StateVector
{
  if (n<0) n=0;

  double kx0(PI*init.getX0());

  size_t dim=space.getDimension();

  StateVectorLow psiLow(dim);

  for (size_t j=0; j<dim; j++) {
    double temp=(space.getX()(j)-kx0)/init.getSig();
    psiLow(j)=exp(-sqr(temp)/2.)*boost::math::hermite(n,temp);
  }

  if (kFlag) quantumdata::ffTransform(psiLow,DIR_XK);

  StateVector res(psiLow,quantumdata::byReference); res.renorm();

  return res;

}

auto particle::hoState(const Pars      & p, bool kFlag) -> StateVector
{
  return hoState(p.hoInitn,p.init,Spatial(p.fin),kFlag);
}

auto particle::hoState(const ParsPumped& p, bool kFlag) -> StateVector
{
  if (p.init.getSig()) return hoState(static_cast<const Pars&>(p),kFlag);
  else                 return hoState(p.hoInitn,
                                      coherent(p),
                                      Spatial(p.fin),
                                      kFlag);
}

auto particle::init(const Pars& p) -> StateVector
{
  if (const auto pp=dynamic_cast<const ParsPumped*>(&p))
    return p.hoInitn<0 ? wavePacket(*pp) : hoState(*pp);
  else
    return p.hoInitn<0 ? wavePacket( p ) : hoState( p );
}

particle::Ptr particle::make(const Pars& p, QM_Picture qmp)
{
  return qmp==QMP_SCH ? Ptr(std::make_shared<ParticleSch>(p)) : Ptr(std::make_shared<Particle>(p));
}

particle::PtrPumped particle::makePumped(const ParsPumped& p, QM_Picture qmp)
{
  return qmp==QMP_SCH ? PtrPumped(std::make_shared<PumpedParticleSch>(p)) : PtrPumped(std::make_shared<PumpedParticle>(p));
}


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

#include <sstream>
#include <NTL/RR.h>
#include <json/json.h>
#include "HE2NEncrypter.h"
#include "Random.h"

HE2NEncrypter::~HE2NEncrypter()
{
	delete kappamod;
};

HE2NEncrypter::HE2NEncrypter(int lambda, int eta, int nu){
	kappa = NTL::RandomPrime_ZZ(nu,20);
	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p ktmp = NTL::to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(ktmp);
}

HE2NEncrypter::HE2NEncrypter(int n, int d, int rho, int rhoprime)
{
    /* Compute lower bound for kappa */
	NTL::RR rtwo = NTL::RR(2);
	NTL::RR exp1 = NTL::RR(rho*d);
	NTL::RR nplusone = NTL::RR(n+1);
	NTL::RR exp2 = NTL::RR(d);
	NTL::RR exp3 = NTL::RR(rho);
	NTL::RR kloBound = pow(rtwo,exp1)*pow(nplusone,exp2);
	NTL::ZZ kappaLowerBound = to_ZZ(kloBound);
	/* Generate kappa as prime > lower bound */
	long nu = NumBits(kappaLowerBound)+1;
	kappa = NTL::RandomPrime_ZZ(nu,20);

	/*compute lower bound for p */
	NTL::RR kappareal = to_RR(kappa);
	NTL::RR ploBound = pow(pow(rtwo,exp3)+pow(kappareal,rtwo),exp2)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound = to_ZZ(ploBound);

	/* Generate p and q */
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);

	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p ktmp = NTL::to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(ktmp);
};

NTL::vec_ZZ_p HE2NEncrypter::encrypt(NTL::ZZ& plaintext)
{
    NTL::ZZ_p r = NTL::to_ZZ_p(rng->nextBigInteger(q));
    NTL::ZZ_p s = NTL::to_ZZ_p(rng->nextBigInteger(kappa));
    NTL::ZZ_p t = NTL::to_ZZ_p(rng->nextBigInteger(modulus));
    NTL::ZZ_p ptext = NTL::to_ZZ_p(plaintext);
    NTL::ZZ_p c = ptext + r*(*pmod) +s*(*kappamod);
    return ONE_VECTOR*c + (*a)*t;
};

std::string HE2NEncrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::ostringstream pStr,modStr,kappaStr,gammaStr;
	pStr << p;
	root["p"] = pStr.str();
	modStr << modulus;
	root["modulus"] = modStr.str();
	kappaStr << kappa;
	root["kappa"] = kappaStr.str();
	gammaStr << (*gamma);
    root["gamma"]=gammaStr.str();
	Json::FastWriter writer;
	return writer.write(root);
};


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/instruments/vanillaoption.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/fdamericanengine.hpp>
#include <ql/pricingengines/vanilla/fdbermudanengine.hpp>
#include <ql/exercise.hpp>
#include <boost/scoped_ptr.hpp>

namespace QuantLib {

    VanillaOption::VanillaOption(
        const boost::shared_ptr<StrikedTypePayoff>& payoff,
        const boost::shared_ptr<Exercise>& exercise)
    : OneAssetOption(payoff, exercise) {}

    Volatility VanillaOption::impliedVolatility(
             Real targetValue,
             const boost::shared_ptr<GeneralizedBlackScholesProcess>& process,
             Real accuracy,
             Size maxEvaluations,
             Volatility minVol,
             Volatility maxVol) const {

        QL_REQUIRE(!isExpired(), "option expired");

        boost::shared_ptr<SimpleQuote> volQuote(new SimpleQuote);

        boost::shared_ptr<GeneralizedBlackScholesProcess> newProcess =
            detail::ImpliedVolatilityHelper::clone(process, volQuote);

        // engines are built-in for the time being
        boost::scoped_ptr<PricingEngine> engine;
        switch (exercise_->type()) {
          case Exercise::European:
            engine.reset(new AnalyticEuropeanEngine(newProcess));
            break;
          case Exercise::American:
            engine.reset(new FDAmericanEngine<CrankNicolson>(newProcess));
            break;
          case Exercise::Bermudan:
            engine.reset(new FDBermudanEngine<CrankNicolson>(newProcess));
            break;
          default:
            QL_FAIL("unknown exercise type");
        }

        return detail::ImpliedVolatilityHelper::calculate(*this,
                                                          *engine,
                                                          *volQuote,
                                                          targetValue,
                                                          accuracy,
                                                          maxEvaluations,
                                                          minVol, maxVol);
    }

}


/**
 * Copyright 2021 Huawei Technologies Co., Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "backend/kernel_compiler/cpu/eigen/eigh_cpu_kernel.h"
#include <Eigen/Eigenvalues>
#include <type_traits>
#include "utils/ms_utils.h"

namespace mindspore {
namespace kernel {

namespace {
constexpr size_t kInputsNum = 2;
constexpr size_t kOutputsNum = 2;
constexpr size_t kDefaultShape = 1;
constexpr auto kAMatrixDimNum = 2;

}  // namespace
using Eigen::Dynamic;
using Eigen::EigenSolver;
using Eigen::Lower;
using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::RowMajor;
using Eigen::Upper;

template <typename T>
using MatrixSquare = Eigen::Matrix<T, Dynamic, Dynamic, RowMajor>;

template <typename T>
using ComplexMatrixSquare = Eigen::Matrix<std::complex<T>, Dynamic, Dynamic, RowMajor>;

template <typename T>
void EighCPUKernel<T>::InitKernel(const CNodePtr &kernel_node) {
  dtype_ = AnfAlgo::GetInputDeviceDataType(kernel_node, 0);

  compute_eigen_vectors = AnfAlgo::GetNodeAttr<bool>(kernel_node, C_EIEH_VECTOR);

  auto A_shape = AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 0);
  CHECK_KERNEL_INPUTS_NUM(A_shape.size(), kAMatrixDimNum, AnfAlgo::GetCNodeName(kernel_node));

  if (A_shape.size() != kShape2dDims || A_shape[0] != A_shape[1]) {
    MS_LOG(EXCEPTION) << "wrong array shape, A should be a matrix, but got [" << A_shape[0] << " X " << A_shape[1]
                      << "]";
  }
  m_ = A_shape[0];
}

template <typename T>
void EighCPUKernel<T>::InitInputOutputSize(const CNodePtr &kernel_node) {
  CPUKernel::InitInputOutputSize(kernel_node);
  (void)workspace_size_list_.template emplace_back(m_ * m_ * sizeof(T));
}

template <typename T>
bool SolveSelfAdjointMatrix(const Map<MatrixSquare<T>> &A, Map<MatrixSquare<T>> *output, Map<MatrixSquare<T>> *outputv,
                            bool compute_eigen_vectors) {
  Eigen::SelfAdjointEigenSolver<MatrixSquare<T>> solver(A);
  output->noalias() = solver.eigenvalues();
  if (compute_eigen_vectors) {
    outputv->noalias() = solver.eigenvectors();
  }
  return true;
}

template <typename T>
bool SolveComplexMatrix(const Map<MatrixSquare<T>> &A, Map<MatrixSquare<T>> *output, Map<MatrixSquare<T>> *outputv,
                        bool compute_eigen_vectors) {
  Eigen::ComplexEigenSolver<MatrixSquare<T>> solver(A);
  output->noalias() = solver.eigenvalues();
  if (compute_eigen_vectors) {
    outputv->noalias() = solver.eigenvectors();
  }
  return true;
}

template <typename T>
bool EighCPUKernel<T>::Launch(const std::vector<AddressPtr> &inputs, const std::vector<AddressPtr> &workspace,
                              const std::vector<AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kOutputsNum, kernel_name_);

  auto A_addr = reinterpret_cast<T *>(inputs[0]->addr);
  // is the Matrix a symmetric matrix(0, all, general matxi, -1 lower triangle, 1 upper triangle)
  auto symmetric_type = reinterpret_cast<bool *>(inputs[1]->addr);
  auto output_addr = reinterpret_cast<T *>(outputs[0]->addr);
  auto output_v_addr = reinterpret_cast<T *>(outputs[1]->addr);
  Map<MatrixSquare<T>> A(A_addr, m_, m_);
  Map<MatrixSquare<T>> A_(A_addr, m_, m_);
  Map<MatrixSquare<T>> output(output_addr, m_, 1);
  Map<MatrixSquare<T>> outputv(output_v_addr, m_, m_);
  // selfadjoint matrix
  if (*symmetric_type) {
    A_ = A.template selfadjointView<Lower>();
  } else {
    A_ = A.template selfadjointView<Upper>();
  }
  // Real scalar eigen solver
  if constexpr (std::is_same_v<T, float>) {
    SolveSelfAdjointMatrix(A_, &output, &outputv, compute_eigen_vectors);
  } else if constexpr (std::is_same_v<T, double>) {
    SolveSelfAdjointMatrix(A_, &output, &outputv, compute_eigen_vectors);
  } else {
    // complex eigen solver
    SolveComplexMatrix(A_, &output, &outputv, compute_eigen_vectors);
  }
  return true;
}
}  // namespace kernel
}  // namespace mindspore


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/poses/CPointPDFSOG.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <Eigen/Dense>

using namespace mrpt::poses;
using namespace mrpt::math;

using namespace mrpt::bayes;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CPointPDFSOG, CPosePDF, mrpt::poses)

/*---------------------------------------------------------------
	Constructor
  ---------------------------------------------------------------*/
CPointPDFSOG::CPointPDFSOG(size_t nModes) : m_modes(nModes) {}
/*---------------------------------------------------------------
			clear
  ---------------------------------------------------------------*/
void CPointPDFSOG::clear() { m_modes.clear(); }
/*---------------------------------------------------------------
	Resize
  ---------------------------------------------------------------*/
void CPointPDFSOG::resize(const size_t N) { m_modes.resize(N); }
/*---------------------------------------------------------------
						getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMean(CPoint3D& p) const
{
	size_t N = m_modes.size();
	double X = 0, Y = 0, Z = 0;

	if (N)
	{
		CListGaussianModes::const_iterator it;
		double sumW = 0;

		for (it = m_modes.begin(); it != m_modes.end(); ++it)
		{
			double w;
			sumW += w = exp(it->log_w);
			X += it->val.mean.x() * w;
			Y += it->val.mean.y() * w;
			Z += it->val.mean.z() * w;
		}
		if (sumW > 0)
		{
			X /= sumW;
			Y /= sumW;
			Z /= sumW;
		}
	}

	p.x(X);
	p.y(Y);
	p.z(Z);
}

std::tuple<CMatrixDouble33, CPoint3D> CPointPDFSOG::getCovarianceAndMean() const
{
	size_t N = m_modes.size();

	CMatrixDouble33 estCov;
	CPoint3D p;
	getMean(p);
	estCov.setZero();

	if (N)
	{
		// 1) Get the mean:
		double sumW = 0;
		auto estMean = CMatrixDouble31(p);

		CListGaussianModes::const_iterator it;

		for (it = m_modes.begin(); it != m_modes.end(); ++it)
		{
			double w;
			sumW += w = exp(it->log_w);

			auto estMean_i = CMatrixDouble31(it->val.mean);
			estMean_i -= estMean;

			auto partCov =
				CMatrixDouble33(estMean_i.asEigen() * estMean_i.transpose());
			partCov += it->val.cov;
			partCov *= w;
			estCov += partCov;
		}

		if (sumW != 0) estCov *= (1.0 / sumW);
	}

	return {estCov, p};
}

uint8_t CPointPDFSOG::serializeGetVersion() const { return 1; }
void CPointPDFSOG::serializeTo(mrpt::serialization::CArchive& out) const
{
	uint32_t N = m_modes.size();
	out << N;
	for (const auto& m : m_modes)
	{
		out << m.log_w;
		out << m.val.mean;
		mrpt::math::serializeSymmetricMatrixTo(m.val.cov, out);
	}
}
void CPointPDFSOG::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			uint32_t N;
			in >> N;
			this->resize(N);
			for (auto& m : m_modes)
			{
				in >> m.log_w;

				// In version 0, weights were linear!!
				if (version == 0) m.log_w = log(max(1e-300, m.log_w));

				in >> m.val.mean;
				mrpt::math::deserializeSymmetricMatrixFrom(m.val.cov, in);
			}
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CPointPDFSOG::copyFrom(const CPointPDF& o)
{
	MRPT_START

	if (this == &o) return;  // It may be used sometimes

	if (o.GetRuntimeClass() == CLASS_ID(CPointPDFSOG))
	{
		m_modes = dynamic_cast<const CPointPDFSOG*>(&o)->m_modes;
	}
	else
	{
		// Approximate as a mono-modal gaussian pdf:
		this->resize(1);
		m_modes[0].log_w = 0;
		o.getCovarianceAndMean(m_modes[0].val.cov, m_modes[0].val.mean);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						saveToTextFile
  ---------------------------------------------------------------*/
bool CPointPDFSOG::saveToTextFile(const std::string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (const auto& m_mode : m_modes)
		os::fprintf(
			f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m_mode.log_w),
			m_mode.val.mean.x(), m_mode.val.mean.y(), m_mode.val.mean.z(),
			m_mode.val.cov(0, 0), m_mode.val.cov(1, 1), m_mode.val.cov(2, 2),
			m_mode.val.cov(0, 1), m_mode.val.cov(0, 2), m_mode.val.cov(1, 2));
	os::fclose(f);
	return true;
}

/*---------------------------------------------------------------
						changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFSOG::changeCoordinatesReference(const CPose3D& newReferenceBase)
{
	for (auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
}

/*---------------------------------------------------------------
					drawSingleSample
 ---------------------------------------------------------------*/
void CPointPDFSOG::drawSingleSample(CPoint3D& outSample) const
{
	MRPT_START

	ASSERT_(m_modes.size() > 0);

	// 1st: Select a mode with a probability proportional to its weight:
	vector<double> logWeights(m_modes.size());
	vector<size_t> outIdxs;
	vector<double>::iterator itW;
	CListGaussianModes::const_iterator it;
	for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
		 ++it, ++itW)
		*itW = it->log_w;

	CParticleFilterCapable::computeResampling(
		CParticleFilter::prMultinomial,  // Resampling algorithm
		logWeights,  // input: log weights
		outIdxs  // output: indexes
	);

	// we need just one: take the first (arbitrary)
	size_t selectedIdx = outIdxs[0];
	ASSERT_(selectedIdx < m_modes.size());
	const CPointPDFGaussian* selMode = &m_modes[selectedIdx].val;

	// 2nd: Draw a position from the selected Gaussian:
	CVectorDouble vec;
	getRandomGenerator().drawGaussianMultivariate(vec, selMode->cov);

	ASSERT_(vec.size() == 3);
	outSample.x(selMode->mean.x() + vec[0]);
	outSample.y(selMode->mean.y() + vec[1]);
	outSample.z(selMode->mean.z() + vec[2]);

	MRPT_END
}

/*---------------------------------------------------------------
					bayesianFusion
 ---------------------------------------------------------------*/
void CPointPDFSOG::bayesianFusion(
	const CPointPDF& p1_, const CPointPDF& p2_,
	const double minMahalanobisDistToDrop)
{
	MRPT_START

	// p1: CPointPDFSOG, p2: CPosePDFGaussian:

	ASSERT_(p1_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));
	ASSERT_(p2_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));

	const auto* p1 = dynamic_cast<const CPointPDFSOG*>(&p1_);
	const auto* p2 = dynamic_cast<const CPointPDFSOG*>(&p2_);

	// Compute the new kernel means, covariances, and weights after multiplying
	// to the Gaussian "p2":
	CPointPDFGaussian auxGaussianProduct, auxSOG_Kernel_i;

	float minMahalanobisDistToDrop2 = square(minMahalanobisDistToDrop);

	this->m_modes.clear();
	bool is2D =
		false;  // to detect & avoid errors in 3x3 matrix inversions of range=2.

	for (const auto& m : p1->m_modes)
	{
		CMatrixDouble33 c = m.val.cov;

		// Is a 2D covariance??
		if (c(2, 2) == 0)
		{
			is2D = true;
			c(2, 2) = 1;
		}

		ASSERT_(c(0, 0) != 0 && c(0, 0) != 0);

		const CMatrixDouble33 covInv = c.inverse_LLt();

		Eigen::Vector3d eta = covInv * CMatrixDouble31(m.val.mean);

		// Normal distribution canonical form constant:
		// See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
		double a = -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
						   (eta.transpose() * c.asEigen() * eta)(0, 0));

		for (const auto& m2 : p2->m_modes)
		{
			auxSOG_Kernel_i = m2.val;
			if (auxSOG_Kernel_i.cov(2, 2) == 0)
			{
				auxSOG_Kernel_i.cov(2, 2) = 1;
				is2D = true;
			}
			ASSERT_(
				auxSOG_Kernel_i.cov(0, 0) > 0 && auxSOG_Kernel_i.cov(1, 1) > 0);

			// Should we drop this product term??
			bool reallyComputeThisOne = true;
			if (minMahalanobisDistToDrop > 0)
			{
				// Approximate (fast) mahalanobis distance (square):
				float mahaDist2;

				float stdX2 = max(auxSOG_Kernel_i.cov(0, 0), m.val.cov(0, 0));
				mahaDist2 =
					square(auxSOG_Kernel_i.mean.x() - m.val.mean.x()) / stdX2;

				float stdY2 = max(auxSOG_Kernel_i.cov(1, 1), m.val.cov(1, 1));
				mahaDist2 +=
					square(auxSOG_Kernel_i.mean.y() - m.val.mean.y()) / stdY2;

				if (!is2D)
				{
					float stdZ2 =
						max(auxSOG_Kernel_i.cov(2, 2), m.val.cov(2, 2));
					mahaDist2 +=
						square(auxSOG_Kernel_i.mean.z() - m.val.mean.z()) /
						stdZ2;
				}

				reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
			}

			if (reallyComputeThisOne)
			{
				auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, m.val);

				// ----------------------------------------------------------------------
				// The new weight is given by:
				//
				//   w'_i = w_i * exp( a + a_i - a' )
				//
				//      a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1))
				//      + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
				//
				// ----------------------------------------------------------------------
				TGaussianMode newKernel;

				newKernel.val = auxGaussianProduct;  // Copy mean & cov

				CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
				Eigen::Vector3d eta_i =
					CMatrixDouble31(auxSOG_Kernel_i.mean).asEigen();
				eta_i = covInv_i.asEigen() * eta_i;

				CMatrixDouble33 new_covInv_i = newKernel.val.cov.inverse_LLt();
				Eigen::Vector3d new_eta_i =
					CMatrixDouble31(newKernel.val.mean).asEigen();
				new_eta_i = new_covInv_i.asEigen() * new_eta_i;

				double a_i =
					-0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
							(eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
							 eta_i)(0, 0));
				double new_a_i =
					-0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
							(new_eta_i.transpose() *
							 newKernel.val.cov.asEigen() * new_eta_i)(0, 0));

				newKernel.log_w = m.log_w + m2.log_w + a + a_i - new_a_i;

				// Fix 2D case:
				if (is2D) newKernel.val.cov(2, 2) = 0;

				// Add to the results (in "this") the new kernel:
				this->m_modes.push_back(newKernel);
			}  // end if reallyComputeThisOne
		}  // end for it2

	}  // end for it1

	normalizeWeights();

	MRPT_END
}

/*---------------------------------------------------------------
						enforceCovSymmetry
 ---------------------------------------------------------------*/
void CPointPDFSOG::enforceCovSymmetry()
{
	MRPT_START
	// Differences, when they exist, appear in the ~15'th significant
	//  digit, so... just take one of them arbitrarily!
	for (auto& m_mode : m_modes)
	{
		m_mode.val.cov(0, 1) = m_mode.val.cov(1, 0);
		m_mode.val.cov(0, 2) = m_mode.val.cov(2, 0);
		m_mode.val.cov(1, 2) = m_mode.val.cov(2, 1);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						normalizeWeights
 ---------------------------------------------------------------*/
void CPointPDFSOG::normalizeWeights()
{
	MRPT_START

	if (!m_modes.size()) return;

	CListGaussianModes::iterator it;
	double maxW = m_modes[0].log_w;
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		maxW = max(maxW, it->log_w);

	for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;

	MRPT_END
}

/*---------------------------------------------------------------
						ESS
 ---------------------------------------------------------------*/
double CPointPDFSOG::ESS() const
{
	MRPT_START
	CListGaussianModes::const_iterator it;
	double cum = 0;

	/* Sum of weights: */
	double sumLinearWeights = 0;
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		sumLinearWeights += exp(it->log_w);

	/* Compute ESS: */
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		cum += square(exp(it->log_w) / sumLinearWeights);

	if (cum == 0)
		return 0;
	else
		return 1.0 / (m_modes.size() * cum);
	MRPT_END
}

/*---------------------------------------------------------------
						evaluatePDFInArea
 ---------------------------------------------------------------*/
void CPointPDFSOG::evaluatePDFInArea(
	float x_min, float x_max, float y_min, float y_max, float resolutionXY,
	float z, CMatrixD& outMatrix, bool sumOverAllZs)
{
	MRPT_START

	ASSERT_(x_max > x_min);
	ASSERT_(y_max > y_min);
	ASSERT_(resolutionXY > 0);

	const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
	const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
	outMatrix.setSize(Ny, Nx);

	for (size_t i = 0; i < Ny; i++)
	{
		const float y = y_min + i * resolutionXY;
		for (size_t j = 0; j < Nx; j++)
		{
			float x = x_min + j * resolutionXY;
			outMatrix(i, j) = evaluatePDF(CPoint3D(x, y, z), sumOverAllZs);
		}
	}

	MRPT_END
}

/*---------------------------------------------------------------
						evaluatePDF
 ---------------------------------------------------------------*/
double CPointPDFSOG::evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const
{
	if (!sumOverAllZs)
	{
		// Normal evaluation:
		auto X = CMatrixDouble31(x);
		double ret = 0;

		CMatrixDouble31 MU;

		for (const auto& m_mode : m_modes)
		{
			MU = CMatrixDouble31(m_mode.val.mean);
			ret += exp(m_mode.log_w) * math::normalPDF(X, MU, m_mode.val.cov);
		}

		return ret;
	}
	else
	{
		// Only X,Y:
		CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
		double ret = 0;

		X(0, 0) = x.x();
		X(1, 0) = x.y();

		for (const auto& m_mode : m_modes)
		{
			MU(0, 0) = m_mode.val.mean.x();
			MU(1, 0) = m_mode.val.mean.y();

			COV(0, 0) = m_mode.val.cov(0, 0);
			COV(1, 1) = m_mode.val.cov(1, 1);
			COV(0, 1) = COV(1, 0) = m_mode.val.cov(0, 1);

			ret += exp(m_mode.log_w) * math::normalPDF(X, MU, COV);
		}

		return ret;
	}
}

/*---------------------------------------------------------------
						getMostLikelyMode
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMostLikelyMode(CPointPDFGaussian& outVal) const
{
	if (this->empty())
	{
		outVal = CPointPDFGaussian();
	}
	else
	{
		auto it_best = m_modes.end();
		for (auto it = m_modes.begin(); it != m_modes.end(); ++it)
			if (it_best == m_modes.end() || it->log_w > it_best->log_w)
				it_best = it;

		outVal = it_best->val;
	}
}

/*---------------------------------------------------------------
						getAs3DObject
 ---------------------------------------------------------------*/
// void  CPointPDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjects::Ptr	&outObj
// )
// const
//{
//	// For each gaussian node
//	for (CListGaussianModes::const_iterator it = m_modes.begin(); it!=
// m_modes.end();++it)
//	{
//		opengl::CEllipsoid::Ptr obj =
// std::make_shared<opengl::CEllipsoid>();
//
//		obj->setPose( it->val.mean);
//		obj->setCovMatrix(it->val.cov,  it->val.cov(2,2)==0  ?  2:3);
//
//		obj->setQuantiles(3);
//		obj->enableDrawSolid3D(false);
//		obj->setColor(1,0,0, 0.5);
//
//		outObj->insert( obj );
//	} // end for each gaussian node
//}


#pragma once

#include <deal.II/base/numbers.h>
// // system includes --------------------------------------------------------
#include <cmath>

#include "base/numbers.hpp"
#include "enum/enum.hpp"
#include "moment_base.hpp"
#include "quadrature/qmidpoint.hpp"
#include "quadrature/quad_handler1d.hpp"
#include "quadrature/quadrature_handler.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

namespace boltzmann {

class Energy : public MomentBase
{
 public:
  Energy() {}

  // ------------------------------------------------------------
  template <typename SPECTRAL_BASIS>
  Energy(const SPECTRAL_BASIS& spectral_basis)
  {
    init(spectral_basis);
  }

  // ------------------------------------------------------------
  template <typename SPECTRAL_BASIS>
  void init(const SPECTRAL_BASIS& spectral_basis);

  /**
   * @brief compute energy
   *
   * @param dst
   * @param src
   * @param count #physical dofs
   */
  void compute(double* dst, const double* src, int count) const;

  // ------------------------------------------------------------
  double compute(const double* src) const;

  // ------------------------------------------------------------
  template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
  void compute(VEC_OUT& dst,
               const VEC_IN& src,
               const dealii::IndexSet& relevant_phys_dofs,
               const INDEXER& indexer) const
  {
    // some g++ cannot see this function inherited from MomentBase
    MomentBase::compute(dst, src, relevant_phys_dofs, indexer);
  }

 private:
  using MomentBase::entry_t;
  using MomentBase::n_velo_dofs;
  using MomentBase::contributions;
};

// ------------------------------------------------------------
template <typename SPECTRAL_BASIS>
void
Energy::init(const SPECTRAL_BASIS& spectral_basis)
{
  n_velo_dofs = spectral_basis.n_dofs();

  typedef typename std::tuple_element<1, typename SPECTRAL_BASIS::elem_t::container_t>::type
      radial_elem_t;

  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;

  auto& QR_handler = QuadHandler<MaxwellQuadrature>::GetInstance();
  typedef QuadAdaptor<MaxwellQuadrature> qr_adapt;

  unsigned int nqR = spectral::get_max_k(spectral_basis) + 2;
  Eigen::VectorXd pts_qR(nqR);
  Eigen::VectorXd wts_qR(nqR);

  qr_adapt::apply(pts_qR.data(), wts_qR.data(), QR_handler.get(nqR), 0.5);

  typedef typename SPECTRAL_BASIS::elem_t elem_t;
  for (auto it = spectral_basis.begin(); it != spectral_basis.end(); ++it) {
    typename elem_t::Acc::template get<radial_elem_t> getter;
    typename elem_t::Acc::template get<angular_elem_t> geta;

    if (geta(*it).get_id().l != 0) continue;

    const auto& rr = getter(*it);
    const double w = rr.w();

    if (std::abs(w - 0.5) > 1e-10)
      throw std::runtime_error("Error: wrong element weight in Energy::init");

    double sum = 0;
    for (unsigned int q = 0; q < pts_qR.size(); ++q) {
      const double r = pts_qR[q];
      sum += rr.evaluate(r) * r * r * wts_qR[q];
    }
    const unsigned int i = it - spectral_basis.begin();
    contributions.push_back(std::make_pair(i, 2 * numbers::PI * sum));
  }
}

// ------------------------------------------------------------
void
Energy::compute(double* dst, const double* src, int count) const
{
#pragma omp parallel for
  for (int i = 0; i < count; ++i) {
    const double* local_src = src + i * n_velo_dofs;
    double sum = 0;
    for (unsigned int j = 0; j < contributions.size(); ++j) {
      sum += contributions[j].second * local_src[contributions[j].first];
    }
    dst[i] = sum;
  }
}

// ------------------------------------------------------------
double
Energy::compute(const double* src) const
{
  double sum = 0.0;
  for (unsigned int j = 0; j < contributions.size(); ++j) {
    sum += contributions[j].second * src[contributions[j].first];
  }
  return sum;
}

}  // end namespace boltzmann


#pragma once
/**

   @file FishSchool.hpp
   @brief Fish school simulator with deterministic motion equations.

*/

#include <atomic>
#include <chrono>
#include <cinttypes>
#include <memory>

#include <boost/statechart/detail/memory.hpp>
#include <boost/statechart/fifo_scheduler.hpp>
#include <boost/statechart/event_base.hpp>

#include <yaml-cpp/yaml.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <dds/pub/Publisher.hpp>
#include <dds/sub/Subscriber.hpp>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include <mimir/IAlgorithm.hpp>

namespace mimir
{
  namespace algorithm
  {
    /**
       @brief Kinematic fish school simulation model.

       \rst

       This class implements a simple kinematic fish school, which can be controlled by
       its inputs. The fish school is modelled as a 5 degrees of freedom kinematic
       body. It has a three dimensional position represented in an inertial reference
       frame, \{NED\}, with axes pointing North, East, and Down, respectively. The body
       can rotate about its y-axis -- pitch: :math:`\theta`, and z-axis -- yaw:
       :math:`\psi`. The fish school is modeled as an under-actuated entity, meaning that
       it can only change forward speed (surge) and its angular velocities. The input to
       the simulation model are surge :math:`u`, rate of turn :math:`r`, and desired depth
       :math:`D_d`. The desired depth is achieved with a first order response using pitch
       angular velocity as manipulated variable. The commanded surge and rate of turn is
       immidiate, i.e. there is no dynamic response. Let us define some quantities to
       describe the fish school with an ordinary differential equation.

       .. math::
          :nowrap:

          \begin{align}
            \boldsymbol{p} = \begin{bmatrix}N\\E\\D\\\end{bmatrix} &\in \mathbb{R}^3 &\text{position}\\
            \boldsymbol{\Theta} = \begin{bmatrix}\theta\\\psi\end{bmatrix} &\in \mathbb{S}^2 &\text{attitude}\\
            \boldsymbol{v} = \begin{bmatrix}u\\v\\w\end{bmatrix} &\in \mathbb{R}^3 &\text{linear velocity}\\
            \boldsymbol{w} = \begin{bmatrix}p\\q\\r\\\end{bmatrix} &\in \mathbb{R}^3 &\text{angular velocity}\\
            D_d &\in \mathbb{R} &\text{desired depth} \\
            \boldsymbol{\eta}(t) = \begin{bmatrix}\boldsymbol{p}\\\Theta\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^3 \times \mathbb{S}^2&\text{state vector}
          \end{align}

       Let :math:`R_y(\theta), R_z(\psi) \in SO(3)` be rotation matrixes, so that
       :math:`R(\boldsymbol{\eta})` is the chained rotation defined as follows:

       .. math::
          :nowrap:

          \begin{align*}
            R_y(\theta) &= \left[\begin{array}{ccc}\cos(\theta)&0 &\sin(\theta)\\0&1&0\\-\sin(\theta)&0&\cos(\theta) \end{array} \right] \\
            R_z(\psi) &= \left[\begin{array}{ccc}\cos(\psi)&-\sin(\psi)&0\\\sin(\psi)&\cos(\psi)&0\\0&0&1 \end{array} \right]\\
            R(\boldsymbol{\eta}) &= R_z(\psi)R_y(\theta) \\
          \end{align*}

       We then use the angular velocity transformation :math:`T(\theta)` as defined in
       :cite:`Fossen2011` and the combined state vector transformation
       :math:`J(\boldsymbol{\eta})` for our 5-dimensional system becomes:

       .. math::
          :nowrap:

          \begin{align*}
            T(\theta) &= \left[\begin{array}{cc}1&0\\0& \frac{1}{\cos(\theta)}\end{array} \right] \\
            J(\boldsymbol{\eta}) &= \left[\begin{array}{cc}R(\boldsymbol{\eta})&0\\0& T(\theta)\end{array} \right] \\
          \end{align*}

       The depth response has a proportional feedback on depth error and a stabilizing
       term to level out the pitch: :math:`q_d = 0.1(D - D_d)\cos(2\theta) - \theta`. The
       resulting ordinary differential equation and input vector are, respectively,

       .. math::
          :nowrap:

          \begin{align*}
            \dat \eta(t) &= J(\boldsymbol{\eta}) \begin{bmatrix}v\\q_d\\q\end{bmatrix} \\
            u(t_d) &= \begin{bmatrix}v_d\\r_d\\Z_d\end{bmatrix},
          \end{align*}

       where :math:`t_d` indicates discrete time points indicated by ``time_step_ms`` in
       the YAML config.

       The table below describes the inputs, outputs and parameters of the
       algorithm. These variables are specified using the YAML configuration file defined
       further below.

       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | Name           | Symbol                     | Description                         | Causality     | Variability    | Default              | Unit            |
       +================+============================+=====================================+===============+================+======================+=================+
       | ``fish_ctrl``  | :math:`u`                  | Desired: Surge, rate of turn, depth | ``input``     | ``discrete``   | \                    | [m/s, rad/s, m] |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``kinematics`` | :math:`y`                  | ``fkin::Kinematics6D``              | ``output``    | ``continuous`` | \                    | \               |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``position``   | :math:`p_{p,0}`            | Initial; North, East, Down          | ``parameter`` | ``fixed``      | :math:`[0, 100, 50]` | m               |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``euler``      | :math:`[\theta_0, \psi_0]` | Initial; Pitch, Yaw                 | ``parameter`` | ``fixed``      | :math:`[0,0]`        | rad             |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+

       .. warning::

          This numerical model is DEPRECATED in favor of a more detailed model with
          stochastic behavior defined using FMI :cite:`fmi2`. Nevertheless, it may serve
          as an example on how to implement a simple :cpp:class:`mimir::IAlgorithm`.

       \endrst

    */
    class FishSchool : public IAlgorithm
    {
    public:
      /**
         @brief FishSchool constructor.

         The constructor parses the specification from the given YAML
         node. It establishes data structures and sets up DDS readers
         and writers according to the input/output scheme of the
         algorithm. The following YAML code block shows the expected
         layout of the ``FishSchool`` map of a input config file.  The
         inputs, outputs, and initial conditions are communicated with
         DDS communication. Common is their DDS topic and DDS
         identifier. The specification is deemed self-explanatory.

         \rst

         .. code-block:: yaml

             time_step_ms: 200
             inputs:
               fish_ctrl:                # DDS type: fkin::IdVec3d
                 topic: fkinFishCtrl
                 id: Fish
                 max_age_ms: -1
             outputs:                    # DDS type: fkin::Kinematics6D
               kinematics:
                 topic: fkinKinematics6D
                 id: Fish
             initial_conditions:
               position:                 # DDS type: fkin::IdVec3d
                 topic: fkinPosition
                 id: FishInit
                 max_wait_ms: 80
                 fallback: [0, 100, 50]
               euler:                    # DDS type: fkin::IdVec3d
                 topic: fkinEuler
                 id: FishInit
                 max_wait_ms: 80
                 fallback: [0, 0, 0]

         \endrst

         @param [in] config YAML configuration from input file.
         @param [in] scheduler State machine scheduler, needed to post events to state machine.
         @param [in] machine State machine processor handle, needed to post events to state machine.
         @param publisher DDS data writer to send data.
         @param subscriber DDS data reader to receive data.
      */
      explicit FishSchool(
          const YAML::Node& config,
          boost::statechart::fifo_scheduler<>& scheduler,
          boost::statechart::fifo_scheduler<>::processor_handle machine,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber);
      /// Destructor.
      virtual ~FishSchool();
      /**
         See base class for details.

         \rst
         The algorithm uses CVODES from SUNDIALS :cite:`Hindmarsh2005sundials`.
         \endrst
      */
      virtual void solve(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void initialize(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void timer(const std::atomic<bool>& cancel_token);
      /// Name identifier of algorithm.
      virtual inline const char* name() { return "FishSchool"; }
      /// Helper function that queues event to state machine.
      void event(boost::statechart::event_base * const event);

    private:
      /// March simulation time one time step ahead.
      inline void step_time() { m_next_step += m_time_step; }
      FishSchool() = delete;
      class Impl;
      std::unique_ptr<Impl> m_impl; ///< Holds the implementation of the algorithm.
      boost::statechart::fifo_scheduler<> & m_scheduler; ///< Members for state machine.
      boost::statechart::fifo_scheduler<>::processor_handle m_stateMachine; ///< Member for state machine.
      const std::chrono::milliseconds m_time_step; ///< Discrete time step.
      std::chrono::steady_clock::time_point m_next_step; ///< Simulation time point.
      const YAML::Node m_config; ///< YAML configuration for algorithm.
    };

  }
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example boost_party.cpp \file boost_party.cpp
    \brief Generates an attendance history of a party by inserting into an interval_map.
           Demonstrating aggregate on overlap.

    boost_party.cpp demonstrates the possibilities of an interval map
    (interval_map or split_interval_map). Boost::posix_time::ptime is used as time
    parameter. An interval_map maps intervals to a given content. In this case the
    content is a set of party guests represented by their name strings.

    As time goes by, groups of people join the party and leave later in the evening.
    So we add a time interval and a name set to the interval_map for the attendance
    of each group of people, that come together and leave together.

    On every overlap of intervals, the corresponding name sets are accumulated. At
    the points of overlap the intervals are split. The accumulation of content on
    overlap of intervals is done via an operator += that has to be implemented
    for the content parameter of the interval_map.

    Finally the interval_map contains the history of attendance and all points in
    time, where the group of party guests changed.

    boost_party.cpp demonstrates a principle that we call
    aggregate on overlap:
    On insertion a value associated to the interval is aggregated (added) to those
    values in the interval_map that overlap with the inserted value.

    There are two behavioral aspects to aggregate on overlap: a decompositional
    behavior and a accumulative behavior.

    The decompositional behavior splits up intervals on the time dimension of the
    interval_map so that the intervals change whenever associated values
    change.

    The accumulative behavior accumulates associated values on every overlap of
    an insertion for the associated values.

    \include boost_party_/boost_party.cpp
*/
//[example_boost_party
// The next line includes <boost/date_time/posix_time/posix_time.hpp>
// and a few lines of adapter code.
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

// Type set<string> collects the names of party guests. Since std::set is
// a model of the itl's set concept, the concept provides an operator +=
// that performs a set union on overlap of intervals.
typedef std::set<string> GuestSetT;

void boost_party()
{
    GuestSetT mary_harry;
    mary_harry.insert("Mary");
    mary_harry.insert("Harry");

    GuestSetT diana_susan;
    diana_susan.insert("Diana");
    diana_susan.insert("Susan");

    GuestSetT peter;
    peter.insert("Peter");

    // A party is an interval map that maps time intervals to sets of guests
    interval_map<ptime, GuestSetT> party;

    party.add( // add and element
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        mary_harry));

    party += // element addition can also be done via operator +=
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        diana_susan);

    party +=
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        peter);

    interval_map<ptime, GuestSetT>::iterator it = party.begin();
    cout << "----- History of party guests -------------------------\n";
    while(it != party.end())
    {
        interval<ptime>::type when = it->first;
        // Who is at the party within the time interval 'when' ?
        GuestSetT who = (*it++).second;
        cout << when << ": " << who << endl;
    }

}

int main()
{
    cout << ">>Interval Container Library: Sample boost_party.cpp <<\n";
    cout << "-------------------------------------------------------\n";
    boost_party();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample boost_party.cpp <<
-------------------------------------------------------
----- History of party guests -------------------------
[2008-May-20 19:30:00, 2008-May-20 20:10:00): Harry Mary
[2008-May-20 20:10:00, 2008-May-20 22:15:00): Diana Harry Mary Susan
[2008-May-20 22:15:00, 2008-May-20 23:00:00): Diana Harry Mary Peter Susan
[2008-May-20 23:00:00, 2008-May-21 00:00:00): Diana Peter Susan
[2008-May-21 00:00:00, 2008-May-21 00:30:00): Peter
-----------------------------------------------------------------------------*/
//]


#ifndef SM_EIGEN_MATRIX_SQRT_HPP
#define SM_EIGEN_MATRIX_SQRT_HPP

#include <sm/assert_macros.hpp>
#include <Eigen/Cholesky>

namespace sm {
    namespace eigen {

        /**
         * \brief Compute the square root of a matrix using the LDLt decomposition for square, positive semidefinite matrices
         *
         * To reconstruct the input matrix, \f$ \mathbf A \f$, from the returned matrix, \f$ \mathbf S \f$,
         * use, \f$ \mathbf A = \mathbf S \mathbf S^T \f$.
         *
         *
         * @param inMatrix      The square matrix whose square root should be computed.
         * @param outMatrixSqrt The output square root.
         */
        template<typename DERIVED1, typename DERIVED2>
        /*Eigen::ComputationInfo*/ void computeMatrixSqrt(const Eigen::MatrixBase<DERIVED1> & inMatrix,
                                                          const Eigen::MatrixBase<DERIVED2> & outMatrixSqrt)
        {
            SM_ASSERT_EQ_DBG(std::runtime_error, inMatrix.rows(), inMatrix.cols(), "This method is only valid for square input matrices");

            DERIVED2 & result = const_cast<DERIVED2 &>(outMatrixSqrt.derived());

            // This is tricky. Using the output matrix type causes the input matrix
            // type to be upgraded to a real numeric matrix. This is useful if,
            // for example, the inMatrix is something like Eigen::Matrix3d::Identity(),
            // which is not an actual matrix. Using DERIVED1 as the template argument
            // in that case will cause a firestorm of compiler errors.
            Eigen::LDLT< DERIVED2 > ldlt(inMatrix.derived());
            result = ldlt.matrixL();
            result = ldlt.transpositionsP().transpose() * result;
            result *= ldlt.vectorD().array().sqrt().matrix().asDiagonal();

            //return ldlt.info();

        }

    } // namespace eigen
} // namespace sm

#endif /* SM_EIGEN_MATRIX_SQRT_HPP */


#include "teca_integrated_water_vapor.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_coordinate_util.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {
template <typename coord_t, typename num_t>
void cartesian_iwv(unsigned long nx, unsigned long ny, unsigned long nz,
    const coord_t *plev, const num_t *q, num_t *iwv)
{
    unsigned long nxy = nx*ny;

    // initialize the result
    memset(iwv, 0, nxy*sizeof(num_t));

    // work an x-y slice at  a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        const num_t *q_k0 = q + knxy;
        const num_t *q_k1 = q_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long i = 0; i < nxy; ++i)
        {
            iwv[i] += h2 * (q_k0[i] + q_k1[i]);
        }
    }

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        iwv[i] *= m1g;
}

template <typename coord_t, typename num_t>
void cartesian_iwv(unsigned long nx, unsigned long ny, unsigned long nz,
    const coord_t *plev, const num_t *q, const char *q_valid, num_t *iwv)
{
    unsigned long nxy = nx*ny;

    // initialize the result
    memset(iwv, 0, nxy*sizeof(num_t));

    // work an x-y slice at a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        const num_t *q_k0 = q + knxy;
        const num_t *q_k1 = q_k0 + nxy;

        const char *q_valid_k0 = q_valid + knxy;
        const char *q_valid_k1 = q_valid_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long i = 0; i < nxy; ++i)
        {
            iwv[i] += ((q_valid_k0[i] && q_valid_k1[i]) ?
               h2 * (q_k0[i] + q_k1[i]) : num_t(0));
        }
    }

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        iwv[i] *= m1g;
}
}

// --------------------------------------------------------------------------
teca_integrated_water_vapor::teca_integrated_water_vapor() :
    specific_humidity_variable("Q"), iwv_variable("IWV"),
    fill_value(1.0e20)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_integrated_water_vapor::~teca_integrated_water_vapor()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_integrated_water_vapor::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_integrated_water_vapor":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, specific_humidity_variable,
            "name of the variable containg the specific humidity")
        TECA_POPTS_GET(double, prefix, fill_value,
            "the value of the NetCDF _FillValue attribute")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_integrated_water_vapor::set_properties(
    const string &prefix, variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, specific_humidity_variable)
    TECA_POPTS_SET(opts, double, prefix, fill_value)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_integrated_water_vapor::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_water_vapor::get_output_metadata" << std::endl;
#endif
    (void)port;

    // set things up in the first pass, and don't modify in subsequent passes
    // due to threading concerns

    if (this->get_number_of_derived_variables() == 0)
    {
        // the base class will handle dealing with the transformation of
        // mesh dimensions and reporting the array we produce, but we have
        // to determine the data type and tell the name of the produced array.
        const teca_metadata &md = input_md[0];

        teca_metadata attributes;
        if (md.get("attributes", attributes))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes are misisng")
            return teca_metadata();
        }

        teca_metadata hus_atts;
        if (attributes.get(this->specific_humidity_variable, hus_atts))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->specific_humidity_variable
                << "\" are misisng")
            return teca_metadata();
        }

        int type_code = 0;
        if (hus_atts.get("type_code", type_code))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->specific_humidity_variable
                << "\" is misisng a \"type_code\"")
            return teca_metadata();
        }

        teca_array_attributes iwv_atts(
            type_code, teca_array_attributes::point_centering,
            0, "kg m^{-1} s^{-1}", "longitudinal integrated vapor transport",
            "the longitudinal component of integrated vapor transport",
            1, this->fill_value);

        // install name and attributes of the output variables in the base classs
        this->append_derived_variable(this->iwv_variable);
        this->append_derived_variable_attribute(iwv_atts);
    }

    if (this->get_number_of_dependent_variables() == 0)
    {
        // install the names of the input variables in the base class
        this->append_dependent_variable(this->specific_humidity_variable);
    }

    // invoke the base class method, which does the work of transforming
    // the mesh and reporting the variables and their attributes.
    return teca_vertical_reduction::get_output_metadata(port, input_md);
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_integrated_water_vapor::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    // invoke the base class method
    return teca_vertical_reduction::get_upstream_request(port, input_md, request);
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_integrated_water_vapor::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_water_vapor::execute" << std::endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IWV because a cartesian mesh is required.")
        return nullptr;
    }

    // get the input dimensions
    unsigned long extent[6] = {0};
    if (in_mesh->get_extent(extent))
    {
        TECA_FATAL_ERROR("Failed to compute IWV because mesh extent is missing.")
        return nullptr;
    }

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nz = extent[5] - extent[4] + 1;

    // get the pressure coordinates
    const_p_teca_variant_array p = in_mesh->get_z_coordinates();
    if (!p)
    {
        TECA_FATAL_ERROR("Failed to compute IWV because pressure coordinates are missing")
        return nullptr;
    }

    if (p->size() < 2)
    {
        TECA_FATAL_ERROR("Failed to compute IWV because z dimensions "
            << p->size() << " < 2 as required by the integration method")
        return nullptr;
    }

    // gather the input arrays
    const_p_teca_variant_array q =
        in_mesh->get_point_arrays()->get(this->specific_humidity_variable);

    if (!q)
    {
        TECA_FATAL_ERROR("Failed to compute IWV because specific humidity \""
            << this->specific_humidity_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array q_valid =
           in_mesh->get_point_arrays()->get(this->specific_humidity_variable + "_valid");

    // the base class will construct the output mesh
    p_teca_cartesian_mesh out_mesh
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(
            std::const_pointer_cast<teca_dataset>(
                teca_vertical_reduction::execute(port, input_data, request)));

    if (!out_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IWV because the output mesh was "
            "not constructed")
        return nullptr;
    }

    // allocate the output arrays
    unsigned long nxy = nx*ny;
    p_teca_variant_array iwv = q->new_instance(nxy);

    // store the result
    out_mesh->get_point_arrays()->set(this->iwv_variable, iwv);

    // calculate IWV
    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        p.get(), _COORDS,

        const NT_COORDS *p_p = static_cast<TT_COORDS*>(p.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(teca_variant_array_impl,
            iwv.get(), _DATA,

            NT_DATA *p_iwv = static_cast<TT_DATA*>(iwv.get())->get();

            const NT_DATA *p_q = static_cast<const TT_DATA*>(q.get())->get();

            const char *p_q_valid = nullptr;
            if (q_valid)
            {
                using TT_MASK = teca_char_array;

                p_q_valid = dynamic_cast<const TT_MASK*>(q_valid.get())->get();

                ::cartesian_iwv(nx, ny, nz, p_p, p_q, p_q_valid, p_iwv);
            }
            else
            {
                ::cartesian_iwv(nx, ny, nz, p_p, p_q, p_iwv);
            }
            )
        )

    // pass 2D arrays through.
    p_teca_array_collection in_arrays =
        std::const_pointer_cast<teca_array_collection>(in_mesh->get_point_arrays());

    p_teca_array_collection out_arrays = out_mesh->get_point_arrays();

    int n_arrays = in_arrays->size();
    for (int i = 0; i < n_arrays; ++i)
    {
        p_teca_variant_array array = in_arrays->get(i);
        if (array->size() == nxy)
        {
            // pass the array.
            out_arrays->append(in_arrays->get_name(i), array);
        }
    }

    return out_mesh;
}


#include "util/statistics.hpp"

#include <map>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/error_of.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
using namespace boost::accumulators;

static uint32_t handles = 0;
static std::map<uint32_t, void *> db;

typedef accumulator_set<double, stats<tag::mean, tag::error_of<tag::mean>, tag::variance(lazy) > > stat_mean_t;

typedef accumulator_set<double, stats<tag::density > > stat_histo_t;
typedef boost::iterator_range<std::vector<std::pair<double, double> >::iterator > stat_histo_iter_t;

typedef accumulator_set<double, stats<tag::tail_quantile<left> > > stat_tail_left_t;
typedef accumulator_set<double, stats<tag::tail_quantile<right> > > stat_tail_right_t;

uint32_t init_mean_stat() {
  db[handles] = (void *)(new stat_mean_t());
  return handles++;
}

uint32_t init_histo_stat(uint32_t binN, uint32_t cacheS) {
  db[handles] = (void *)(new stat_histo_t(tag::density::num_bins = binN,
                                             tag::density::cache_size = cacheS));
  return handles++;
}

uint32_t init_tail_stat(bool dir, uint32_t cacheS) {
  if(dir)
    db[handles] = (void *)(new stat_tail_left_t(tag::tail<left>::cache_size = cacheS));
  else
    db[handles] = (void *)(new stat_tail_right_t(tag::tail<right>::cache_size = cacheS));
  return handles++;
}

void record_mean_stat(uint32_t handle, double sample) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  (*stat)(sample);
}

void record_histo_stat(uint32_t handle, double sample) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  (*stat)(sample);
}

void record_tail_stat(uint32_t handle, bool dir, double sample) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    (*stat)(sample);
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    (*stat)(sample);
  }
}

uint32_t get_mean_count(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return count(*stat);
}

double get_mean_mean(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return mean(*stat);
}

double get_mean_error(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return error_of<tag::mean>(*stat);
}

double get_mean_variance(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return variance(*stat);
}

uint32_t get_histo_count(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  return count(*stat);
}

std::vector<std::pair<double, double> > get_histo_density(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  stat_histo_iter_t hist = density(*stat);
  std::vector<std::pair<double, double> > rv(hist.size());
  for(uint32_t i=0; i<hist.size(); i++) rv[i] = hist[i];
  return rv;
}

double get_tail_quantile(uint32_t handle, bool dir, double ratio) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    assert(ratio >= 0.5);
    return quantile(*stat, quantile_probability = ratio);
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    assert(ratio <= 0.5);
    return quantile(*stat, quantile_probability = ratio);
  }
}

void close_mean_stat(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  delete stat;
  db.erase(handle);
}

void close_histo_stat(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  delete stat;
  db.erase(handle);
}

void close_tail_stat(uint32_t handle, bool dir) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    delete stat;
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    delete stat;
  }
  db.erase(handle);
}


#include <Eigen/Dense>
#include "edge_labeler.h"
#include "g2o/stuff/unscented.h"

namespace g2o {

  using namespace std;
  using namespace Eigen;

  typedef SigmaPoint<VectorXd> MySigmaPoint;

  EdgeLabeler::EdgeLabeler(SparseOptimizer* optimizer) {
    _optimizer = optimizer;
  }

  int EdgeLabeler::labelEdges(std::set<OptimizableGraph::Edge*>& edges){
    // assume the system is "solved"
    // compute the sparse pattern of the inverse
    std::set<std::pair<int, int> > pattern;
    for (std::set<OptimizableGraph::Edge*>::iterator it=edges.begin(); it!=edges.end(); it++){
      augmentSparsePattern(pattern, *it);
    }

    SparseBlockMatrix<MatrixXd> spInv;

    bool result = computePartialInverse(spInv, pattern);
    //cerr << "partial inverse computed = " << result << endl;
    //cerr << "non zero blocks" << spInv.nonZeroBlocks() << endl;

    if (! result ){
      return -1;
    }
    int count=0;
    for (std::set<OptimizableGraph::Edge*>::iterator it=edges.begin(); it!=edges.end(); it++){
      count += labelEdge(spInv, *it) ? 1 : 0;
    }
    return count;
  }

  void EdgeLabeler::augmentSparsePattern(std::set<std::pair<int, int> >& pattern, OptimizableGraph::Edge* e){
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=v->hessianIndex();
      if (ti==-1)
	continue;
      for (size_t j=i; j<e->vertices().size(); j++){
	const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[j];
	int tj = v->hessianIndex();
	if (tj==-1)
	  continue;
	if(tj<ti)
	  swap(ti,tj);
	pattern.insert(std::make_pair(ti, tj));
      }
    }
  }

  bool EdgeLabeler::computePartialInverse(SparseBlockMatrix<MatrixXd>& spinv, const std::set<std::pair<int,int> >& pattern){
    std::vector<std::pair<int, int> > blockIndices(pattern.size());
    // Why this does not work???
    //std::copy(pattern.begin(),pattern.end(),blockIndices.begin());

    int k=0;
    for(std::set<std::pair<int, int> >::const_iterator it= pattern.begin(); it!=pattern.end(); it++){
      blockIndices[k++]=*it;
    }

    //cerr << "sparse pattern contains " << blockIndices.size() << " blocks" << endl;
    return _optimizer->computeMarginals(spinv, blockIndices);
  }

  bool EdgeLabeler::labelEdge( const SparseBlockMatrix<MatrixXd>& spinv, OptimizableGraph::Edge* e){

    Eigen::Map<MatrixXd> info(e->informationData(), e->dimension(), e->dimension());
    // cerr << "original information matrix" << endl;
    // cerr << info << endl;

    int maxDim=0;
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=v->hessianIndex();
      if (ti==-1)
	continue;
      maxDim+=v->minimalEstimateDimension();
    }

    //cerr << "maxDim= " << maxDim << endl;
    MatrixXd cov(maxDim, maxDim);
    int cumRow=0;
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* vr=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=vr->hessianIndex();
      if (ti>-1) {
	int cumCol=0;
	for (size_t j=0; j<e->vertices().size(); j++){
	  const OptimizableGraph::Vertex* vc=(const OptimizableGraph::Vertex*) e->vertices()[j];
	  int tj = vc->hessianIndex();
	  if (tj>-1){
	    // cerr << "ti=" << ti << " tj=" << tj
	    //    << " cumRow=" << cumRow << " cumCol=" << cumCol << endl;
	    if (ti<=tj){
	      assert(spinv.block(ti, tj));
	      // cerr << "cblock_ptr" << spinv.block(ti, tj) << endl;
	      // cerr << "cblock.size=" << spinv.block(ti, tj)->rows() << "," << spinv.block(ti, tj)->cols() << endl;
	      // cerr << "cblock" << endl;
	      // cerr << *spinv.block(ti, tj) << endl;
	      cov.block(cumRow, cumCol, vr->minimalEstimateDimension(), vc->minimalEstimateDimension()) =
		*spinv.block(ti, tj);
	    } else {
	      assert(spinv.block(tj, ti));
	      // cerr << "cblock.size=" << spinv.block(tj, ti)->cols() << "," << spinv.block(tj, ti)->rows() << endl;
	      // cerr << "cblock" << endl;
	      // cerr << spinv.block(tj, ti)->transpose() << endl;
	      cov.block(cumRow, cumCol, vr->minimalEstimateDimension(), vc->minimalEstimateDimension()) =
		spinv.block(tj, ti)->transpose();
	    }
	    cumCol += vc->minimalEstimateDimension();
	  }
	}
	cumRow += vr->minimalEstimateDimension();
      }
    }

    // cerr << "covariance assembled" << endl;
    // cerr << cov << endl;
    // now cov contains the aggregate marginals of the state variables in the edge
    VectorXd incMean(maxDim);
    incMean.fill(0);
    std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > incrementPoints;
    if (! sampleUnscented(incrementPoints, incMean, cov)){
      cerr << "sampleUnscented fail" << endl;
      return false;
    }
    // now determine the zero-error measure by applying the error function of the edge
    // with a zero measurement
    // TODO!!!
    bool smss = e->setMeasurementFromState();
    if (! smss) {
      cerr << "FATAL: Edge::setMeasurementFromState() not implemented" << endl;
    }
    assert(smss && "Edge::setMeasurementFromState() not implemented");

    //std::vector<MySigmaPoint> globalPoints(incrementPoints.size());
    std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > errorPoints(incrementPoints.size());

    // for each sigma point, project it to the global space, by considering those variables
    // that are involved
    //cerr << "sigma points are extracted, remapping to measurement space" << endl;
    for (size_t i=0; i<incrementPoints.size(); i++) {
      int cumPos=0;
      //VectorXd globalPoint(maxDim);

      // push all the "active" state variables
      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->push();
      }

      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->oplus(&incrementPoints[i]._sample[cumPos]);
        //assert(vr->getMinimalEstimateData(&globalPoint[cumPos]) && "Vertex::getMinimalEstimateData(...) not implemented");
        cumPos+=vr->minimalEstimateDimension();
      }

      // construct the sigma point in the global space
      // globalPoints[i]._sample=globalPoint;
      // globalPoints[i]._wi=incrementPoints[i]._wi;
      // globalPoints[i]._wp=incrementPoints[i]._wp;

      // construct the sigma point in the error space
      e->computeError();
      Map<VectorXd> errorPoint(e->errorData(),e->dimension());

      errorPoints[i]._sample=errorPoint;
      errorPoints[i]._wi=incrementPoints[i]._wi;
      errorPoints[i]._wp=incrementPoints[i]._wp;

      // pop all the "active" state variables
      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->pop();
      }

    }

    // reconstruct the covariance of the error by the sigma points
    MatrixXd errorCov(e->dimension(), e->dimension());
    VectorXd errorMean(e->dimension());
    reconstructGaussian(errorMean, errorCov, errorPoints);
    info=errorCov.inverse();

    // cerr << "remapped information matrix" << endl;
    // cerr << info << endl;
    return true;
  }

}