/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
 * \file
 * \author Felix Lehmann
 */

#ifndef ICC_0_PRECOND_HH
#define ICC_0_PRECOND_HH

#include <memory>
#include <vector>

#include <boost/fusion/include/at_c.hpp>

#include "dune/grid/common/grid.hh"
#include "dune/istl/matrix.hh"
#include "dune/istl/bcrsmatrix.hh"
#include "dune/common/fmatrix.hh"
#include "dune/common/fvector.hh"
#include "dune/common/iteratorfacades.hh"
#include "dune/istl/matrixindexset.hh"

#include "dune/istl/preconditioners.hh"

namespace Kaskade
{

/**
 * \ingroup linalg
 *
 * Incomplete Cholesky factorization by algorithm from book "Matrix Computations" by Gene Golub & Charles van Loan
 *
 * algorith will create approximate cholesky factorization L by only using the sparsity pattern of A
 *
 * we will only touch the lower triangle of the matrix
 *
 * interface copied from iluprecond.hh
 *
 * iccprecond.hh (using TAUCS Incomplete Cholesky Decomposition) does the same (even faster),
 * if we set its droptolerance parameter sufficiently large
 *
 */

template <class Op>
class ICC_0Preconditioner: public Dune::Preconditioner<typename Op::Range, typename Op::Range>
{
public:
  typedef typename Op::Domain Domain;
  typedef typename Op::Range Range;
  typedef typename Op::Scalar Scalar;

private:
  typedef Dune::FieldMatrix<Scalar,1,1> BlockType;
  typedef Dune::BlockVector<Dune::FieldVector<Scalar,1> > CoeffVector ;
  typedef Dune::BCRSMatrix<BlockType> BCRS_Matrix;
  typedef typename BCRS_Matrix::ConstColIterator ColIter;

    BCRS_Matrix mL;
    std::vector<std::vector<int> > mEntries;
    // int const mChoice;
  public:
    static int const category = Dune::SolverCategory::sequential;
    ICC_0Preconditioner( Op& op );
    void pre (Domain&, Range&) {}
    void apply (Domain& x, Range const& y);
    void post (Domain&) {}
};

template <class Op>
ICC_0Preconditioner<Op>::ICC_0Preconditioner(Op& op)
{
  std::unique_ptr<BCRS_Matrix> mA = op.template getPointer<BCRS_Matrix>();
  int N = (*mA).N();
  // create kind of column iterator for BCRS
  // and simultaneously read out the sparsity pattern of mA
  mEntries.resize(N);
  for(int i=0;i<N;i++)
    for (ColIter cI=(*mA)[i].begin(); cI!=(*mA)[i].end() && cI.index()<i; ++cI)
      mEntries[cI.index()].push_back(i);

  // backward substitution in ICC_0Preconditioner::apply() with BCRSMatrix mL^T is not
  // efficiently applicable, therefore create two BCRSMatrices mL and mLT := mL^T
  // set up sparsity pattern of lower triangle of mA (upper triangle of mA^T) for mL (mLT)
  Dune::MatrixIndexSet sparsityPattern_mL(N,N);
  for (int i=0; i<N; ++i)
    for (ColIter cI=(*mA)[i].begin(); cI!=(*mA)[i].end() && cI.index()<=i; ++cI)
      sparsityPattern_mL.add(i,cI.index() );

  sparsityPattern_mL.exportIdx( mL );
  for(int i=0;i<N;++i)
    for (ColIter cI=mL[i].begin(); cI!=mL[i].end(); ++cI)
      mL[i][cI.index()] = (*mA)[i][cI.index()];
  //find factorization mL of mA
  double tmp;
  for(int k=0;k<N;k++)
  {
    mL[k][k] = sqrt(mL[k][k]);
    for(std::vector<int>::iterator it = mEntries[k].begin(); it!=mEntries[k].end(); it++)
      mL[*it][k] /= mL[k][k];
    for(std::vector<int>::iterator it = mEntries[k].begin(); it!=mEntries[k].end(); it++)
    {
      tmp = mL[*it][k];
      mL[*it][*it] -= (mL[*it][k])*(mL[*it][k]); // "mEntries" contains not diagonal indices
      for(std::vector<int>::iterator it2 = mEntries[*it].begin(); it2!=mEntries[*it].end(); it2++)
        if( mL.exists(*it2,k) )
	  mL[*it2][*it] -= mL[*it2][k]*tmp;
    }
  }
}

template <class Op>
void ICC_0Preconditioner<Op>::apply(Domain& x, Range const& b)
{
  int N = x.dim();

  CoeffVector &sol = boost::fusion::at_c<0>(x.data);
  CoeffVector const &rhs = boost::fusion::at_c<0>(b.data);
  // for forward substitution
  CoeffVector forward(N); forward = 0;

  double tmp;
  // forward substitution
  for(int i=0;i<N;i++)
  {
    tmp = rhs[i];
    for (ColIter cI = mL[i].begin(); cI != mL[i].end(); ++cI)
      if( cI.index()<i )
	tmp -= mL[i][cI.index()]*forward[cI.index()];
    forward[i] = tmp/mL[i][i];
  }
  // backward substitution
  for(int i = N-1;i>=0;i--)
  {
    tmp = forward[i];
    for(std::vector<int>::iterator it = mEntries[i].begin();it!=mEntries[i].end();it++)
      tmp -= mL[*it][i]*sol[*it];
    sol[i] = tmp/mL[i][i];
  }
}
}  // namespace Kaskade
#endif


#ifndef __BOOST_GEOMETRY_DISSOLVE_H__
#define __BOOST_GEOMETRY_DISSOLVE_H__

/*
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * Wouter van Kleunen wrote this file.  As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.
 * ----------------------------------------------------------------------------
 */

#include <vector>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/function_output_iterator.hpp>

namespace geometry {

namespace impl {

template<typename C, typename T>
static inline void result_combine(C &result, T &&new_element)
{
    result.push_back(new_element);

   	for(std::size_t i = 0; i < result.size() - 1; ) {
        if(!boost::geometry::intersects(result[i], result.back())) {
            ++i;
            continue;
        }

        std::vector<T> union_result;
        boost::geometry::union_(result[i], result.back(), union_result);

        if(union_result.size() != 1) {
			++i;
			continue;
		}

       	result.back() = std::move(union_result[0]);
       	result.erase(result.begin() + i);
    }
}

struct pseudo_vertice_key
{
    std::size_t index_1;
    double scale;
    std::size_t index_2;
    bool reroute;

    pseudo_vertice_key(std::size_t index_1 = 0, std::size_t index_2 = 0, double scale = 0.0, bool reroute = false)
        : index_1(index_1), scale(scale), index_2(index_2), reroute(reroute)
    { }
};

struct compare_pseudo_vertice_key
{
    bool operator()(pseudo_vertice_key const &a, pseudo_vertice_key const &b) const {
        if(a.index_1 < b.index_1) return true;
        if(a.index_1 > b.index_1) return false;
        if(a.scale < b.scale) return true;
        if(a.scale > b.scale) return false;
        if(a.index_2 > b.index_2) return true;
        if(a.index_2 < b.index_2) return false;
        if(a.reroute && !b.reroute) return true;
        if(!a.reroute && b.reroute) return false;
		return false;
    }
};

template<typename point_t = boost::geometry::model::d2::point_xy<double>>
struct pseudo_vertice
{
    point_t p;
    pseudo_vertice_key link;

    pseudo_vertice(point_t p, pseudo_vertice_key link = pseudo_vertice_key())
        : p(p), link(link)
    { }
};

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename ring_t = boost::geometry::model::ring<point_t>
	>
static inline void dissolve_find_intersections(
			ring_t const &ring,
			std::map<pseudo_vertice_key, pseudo_vertice<point_t>, compare_pseudo_vertice_key> &pseudo_vertices,
    		std::set<pseudo_vertice_key, compare_pseudo_vertice_key> &start_keys)
{
	if(ring.empty()) return;

	boost::geometry::index::rtree<std::pair< boost::geometry::model::segment<point_t>, std::size_t >, boost::geometry::index::quadratic<16>> index;

	// Generate all by-pass intersections in the graph
	// Generate a list of all by-pass intersections
    pseudo_vertices.emplace(pseudo_vertice_key(ring.size() - 1, ring.size() - 1, 0.0), ring.back());
    for(std::size_t i = ring.size() - 1; i--; )
    {
        pseudo_vertices.emplace(pseudo_vertice_key(i, i, 0.0), ring[i]);
		boost::geometry::model::segment<point_t> line_1(ring[i], ring[i + 1]);

		boost::geometry::index::query(
				index, boost::geometry::index::intersects(line_1),
				boost::make_function_output_iterator([&](std::pair< boost::geometry::model::segment<point_t>, std::size_t > const &iter) {

			auto const &line_2 = iter.first;
			auto j = iter.second;

			std::vector<point_t> output;
			boost::geometry::intersection(line_1, line_2, output);

			for(auto const &p: output) {
				double scale_1 = boost::geometry::comparable_distance(p, ring[i]) / boost::geometry::comparable_distance(ring[i + 1], ring[i]);
				double scale_2 = boost::geometry::comparable_distance(p, ring[j]) / boost::geometry::comparable_distance(ring[j + 1], ring[j]);
				if(scale_1 < 1.0 && scale_2 < 1.0) {
					pseudo_vertice_key key_j(j, i, scale_2);
					pseudo_vertices.emplace(pseudo_vertice_key(i, j, scale_1, true), pseudo_vertice<point_t>(p, key_j));
					pseudo_vertices.emplace(key_j, p);
					start_keys.insert(key_j);

					pseudo_vertice_key key_i(i, j, scale_1);
					pseudo_vertices.emplace(pseudo_vertice_key(j, i, scale_2, true), pseudo_vertice<point_t>(p, key_i));
					pseudo_vertices.emplace(key_i, p);
					start_keys.insert(key_i);
				}
			}
		}));

		index.insert(std::make_pair(boost::geometry::model::segment<point_t>(ring[i], ring[i+1]), i));
    }
}

// Remove invalid points (NaN) from ring
template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename ring_t = boost::geometry::model::ring<point_t>
	>
static inline void correct_invalid(ring_t &ring)
{
	for(auto i = ring.begin(); i != ring.end(); ) {
		if(!boost::geometry::is_valid(*i))
			i = ring.erase(i);
		else
			++i;
	}
}

// Correct orientation of ring
template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename ring_t = boost::geometry::model::ring<point_t>
	>
static inline double correct_orientation(ring_t &ring, boost::geometry::order_selector order)
{
	auto area = boost::geometry::area(ring);
	bool should_reverse =
		(order == boost::geometry::clockwise && area < 0) ||
		(order == boost::geometry::counterclockwise && area > 0);

	if(should_reverse) {
		std::reverse(ring.begin(), ring.end());
	}

	return area;
}

// Close ring if not closed
template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename ring_t = boost::geometry::model::ring<point_t>
	>
static inline void correct_close(ring_t &ring)
{
	// Close ring if not closed
	if(!ring.empty() && !boost::geometry::equals(ring.back(), ring.front()))
		ring.push_back(ring.front());

}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename ring_t = boost::geometry::model::ring<point_t>
	>
static inline std::vector<ring_t> dissolve_generate_rings(
			std::map<pseudo_vertice_key, pseudo_vertice<point_t>, compare_pseudo_vertice_key> &pseudo_vertices,
    		std::set<pseudo_vertice_key, compare_pseudo_vertice_key> &start_keys,
			boost::geometry::order_selector order, double remove_spike_min_area = 0.0)
{
	std::vector<ring_t> result;

	// Generate all polygons by tracing all the intersections
	// Perform union to combine all polygons into single polygon again
    while(!start_keys.empty()) {
		ring_t new_ring;

		// Store point in generated polygon
		auto push_point = [&new_ring](auto const &p) {
            if(new_ring.empty() || boost::geometry::comparable_distance(new_ring.back(), p) > 0)
                new_ring.push_back(p);
		};

		auto is_closed = [](ring_t &ring) {
			if(ring.size() < 2) return false;
			for(std::size_t i = 0; i < ring.size() - 1; ++i) {
				if(boost::geometry::comparable_distance(ring[i], ring.back()) == 0) {
					ring.erase(ring.begin(), ring.begin() + i);
					return true;
				}
			}

			return false;
		};

        auto start_iter = pseudo_vertices.find(*start_keys.begin());
        auto i = start_iter;

        do {
            auto const &key = i->first;
            auto const &value = i->second;

			// Store the point in output polygon
			push_point(value.p);

            start_keys.erase(key);
            if(key.reroute) {
				// Follow by-pass
                i = pseudo_vertices.find(value.link);
			} else {
				// Continu following original polygon
                ++i;
                if(i == pseudo_vertices.end())
                    i = pseudo_vertices.begin();
            }

			// Repeat until back at starting point
       	} while(!is_closed(new_ring));

		// Combine with already generated polygons
		auto area = boost::geometry::area(new_ring);
		if(std::abs(area) > remove_spike_min_area) {
	    	result.push_back(std::move(new_ring));
		}
   	}

    return result;
}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename ring_t = boost::geometry::model::ring<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline std::vector<ring_t> correct(ring_t const &ring, boost::geometry::order_selector order, double remove_spike_min_area = 0.0)
{
	constexpr std::size_t min_nodes = 3;
	if(ring.size() < min_nodes)
		return std::vector<ring_t>();

    std::map<pseudo_vertice_key, pseudo_vertice<point_t>, compare_pseudo_vertice_key> pseudo_vertices;
    std::set<pseudo_vertice_key, compare_pseudo_vertice_key> start_keys;

	ring_t new_ring = ring;

	// Remove invalid coordinates
	correct_invalid(new_ring);

	// Close ring
	correct_close(new_ring);

	// Correct orientation
	correct_orientation(new_ring, order);

	// Detect self-intersection points
	dissolve_find_intersections(new_ring, pseudo_vertices, start_keys);

	if(start_keys.empty()) {
		if(std::abs(boost::geometry::area(new_ring)) > remove_spike_min_area)
			return { new_ring };
		else
			return { };
	}

	return dissolve_generate_rings(pseudo_vertices, start_keys, order, remove_spike_min_area);
}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
struct combine_non_zero_winding
{
	inline void operator()(multi_polygon_t &combined_outers, multi_polygon_t &combined_inners, polygon_t &poly)
	{
		if(boost::geometry::area(poly) > 0)
			result_combine(combined_outers, std::move(poly));
		else {
			std::reverse(poly.outer().begin(), poly.outer().end());
			result_combine(combined_inners, std::move(poly));
		}
	}
};

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
struct combine_odd_even
{
	inline void operator()(multi_polygon_t &combined_outers, multi_polygon_t &combined_inners, polygon_t &poly)
	{
		if(boost::geometry::area(poly) < 0)
			std::reverse(poly.outer().begin(), poly.outer().end());

		multi_polygon_t result;
		boost::geometry::sym_difference(combined_outers, poly, result);
		combined_outers = std::move(result);
	}
};

template<
	typename combine_function_t,
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename ring_t = boost::geometry::model::ring<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct(polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area, combine_function_t combine)
{
	auto order = boost::geometry::point_order<polygon_t>::value;
	auto outer_rings = correct(input.outer(), order, remove_spike_min_area);

	// Calculate all outers and combine them if possible
	multi_polygon_t combined_outers;
	multi_polygon_t combined_inners;

	for(auto &ring: outer_rings) {
		polygon_t poly;
		poly.outer() = std::move(ring);
		combine(combined_outers, combined_inners, poly);
	}

	// Calculate all inners and combine them if possible
	for(auto const &ring: input.inners()) {
		polygon_t poly;
		poly.outer() = std::move(ring);

		multi_polygon_t new_inners;
		correct(poly, new_inners, remove_spike_min_area, combine);

		for(auto &poly: new_inners) {
			result_combine(combined_inners, std::move(poly));
		}
	}

	// Cut out all inners from all the outers
	boost::geometry::difference(combined_outers, combined_inners, output);
}

template<
	typename combine_function_t,
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename ring_t = boost::geometry::model::ring<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct(multi_polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area, combine_function_t combine)
{
	for(auto const &polygon: input)
	{
		multi_polygon_t new_polygons;
		correct(polygon, new_polygons, remove_spike_min_area, combine);

		for(auto &new_polygon: new_polygons)
			result_combine(output, std::move(new_polygon));
	}
}

}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct(polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area = 0.0)
{
	impl::correct(input, output, remove_spike_min_area, impl::combine_non_zero_winding<point_t, polygon_t, multi_polygon_t>());
}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct_odd_even(polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area = 0.0)
{
	impl::correct(input, output, remove_spike_min_area, impl::combine_odd_even<point_t, polygon_t, multi_polygon_t>());
}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename ring_t = boost::geometry::model::ring<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct(multi_polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area = 0.0)
{
	impl::correct(input, output, remove_spike_min_area, impl::combine_non_zero_winding<point_t, polygon_t, multi_polygon_t>());
}

template<
	typename point_t = boost::geometry::model::d2::point_xy<double>,
	typename polygon_t = boost::geometry::model::polygon<point_t>,
	typename ring_t = boost::geometry::model::ring<point_t>,
	typename multi_polygon_t = boost::geometry::model::multi_polygon<polygon_t>
	>
static inline void correct_odd_even(multi_polygon_t const &input, multi_polygon_t &output, double remove_spike_min_area = 0.0)
{
	impl::correct(input, output, remove_spike_min_area, impl::combine_odd_even<point_t, polygon_t, multi_polygon_t>());
}

}

#endif


#include "distribution_mixture_models.hxx"

#include <boost/range/combine.hpp>
#include <iomanip>
#include <math.h>

namespace vmf_fitting
{

template<int N_>
void ExpApproximation(Eigen::Array<float, N_, 1> &vals)
{
  // From http://spfrnd.de/posts/2018-03-10-fast-exponential.html
  // It is also implemented in the supplemental code of the Vorba paper about gaussian mixtures for path guiding.
  // Eigen also does it this way by default but with more precision.

  static_assert(std::numeric_limits<float>::is_iec559);
  static_assert(sizeof(float) == sizeof(std::uint32_t));
  constexpr int N = N_;
  constexpr float log2_e = 1.4426950408889634f;
  // This polynomial gives less than 0.3% relative error.
  constexpr float poly_coeffs[3] = { 0.34271437f, 0.6496069f , 1.f + 0.0036554f };
  constexpr uint32_t exp_mask = 255u << 23u;

  using FloatVals = Eigen::Array<float, N, 1>;
  using IntVals = Eigen::Array<int, N, 1>;

  vals *= log2_e;

  // Notes:
  //  floor() needs some fancy instructions sets to be vectorized. Compilation with -march=core2 is not enough. corei7-avx works.
  //  Apparently my CPU has it
  //  https://ark.intel.com/content/www/us/en/ark/products/52214/intel-core-i7-2600k-processor-8m-cache-up-to-3-80-ghz.html
  //  See also the Eigen manual https://eigen.tuxfamily.org/dox/group__CoeffwiseMathFunctions.html
  auto floored = vals.floor().eval();
  const IntVals xi = floored.template cast<int>();
  const FloatVals xf = vals - floored;

  vals = xf*(xf*poly_coeffs[0] + poly_coeffs[1]) + poly_coeffs[2];

  // memcpy does actually copy stuff :-(
  //std::uint32_t int_view[N];
  //std::memcpy(int_view, vals.data(), N*sizeof(float));

  // This is UB afaik. But every reasonable compiler should do the right thing ...
  std::uint32_t* int_view = reinterpret_cast<uint32_t*>(vals.data());

  // Should be auto-vectorized. Clang 9 in compiler explorer can do it!
  for (int i=0; i<N; ++i)
  {
      int_view[i] = (int_view[i] & ~exp_mask) | ((((xi[i] + 127)) << 23) & exp_mask);
  }

  //std::memcpy(vals.data(), int_view, N*sizeof(float));
}

float ExpApproximation(float x)
{
  // Should compile to ca 10 instructions.
  // And it can be inlined in contrast to std::exp().
  static_assert(std::numeric_limits<float>::is_iec559);
  static_assert(sizeof(float) == sizeof(std::uint32_t));

  constexpr float log2_e = 1.4426950408889634f;
  constexpr float poly_coeffs[3] = { 0.34271437f, 0.6496069f , 1.f + 0.0036554f };
  constexpr uint32_t exp_mask = 255u << 23u;

  x *= log2_e;

  const float floored = std::floor(x);
  const int xi = int(floored);
  const float xf = x - floored;

  const float mantisse = xf*(xf*poly_coeffs[0] + poly_coeffs[1]) + poly_coeffs[2];

  std::uint32_t int_view;
  std::memcpy(&int_view, &mantisse, sizeof(float));

  int_view = (int_view & ~exp_mask) | ((((xi + 127)) << 23) & exp_mask);

  float result;
  std::memcpy(&result, &int_view, sizeof(float));

  return result;
}

template<int N = 8>
inline Eigen::Array<float, N, 1> ComponentPdfs(const VonMisesFischerMixture<N> & mixture, const Eigen::Vector3f & pos) noexcept
{
  const auto& k = mixture.concentrations;
  assert((k >= K_THRESHOLD).all() && (k <= K_THRESHOLD_MAX).all());
  auto t1 = (-2.f*k).eval();
  ExpApproximation<N>(t1);
  //assert(t1.isFinite().all());
  const auto prefactors = (float(Pi)*2.f*(1.f - t1)).eval();
  //assert((prefactors > -0.1f).all());
  //assert((prefactors > 0.f).all());
  auto t2 = (k*((mixture.means.matrix() * pos).array() - 1.f)).eval();
  ExpApproximation<N>(t2);
  //assert(t2.isFinite().all());
  const auto result = (k / prefactors * t2).eval();
  assert(result.isFinite().all() && (result >= 0.f).all());
  return result;
}

template<int N = 8>
float Pdf(const VonMisesFischerMixture<N> & mixture, const Eigen::Vector3f & pos) noexcept
{
  const auto component_pdfs = ComponentPdfs(mixture, pos);
  return (component_pdfs * mixture.weights).sum();
}

Eigen::Vector3f Sample(const Eigen::Vector3f& mu, float k, float r1, float r2) noexcept
{
  assert (k >= K_THRESHOLD);
  assert (k <= K_THRESHOLD_MAX);
  const float vx = std::cos((float)(Pi*2.)*r1);
  const float vy = std::sin((float)(Pi*2.)*r1);
  // if (!std::isfinite(vx) || !std::isfinite(vy))
  //   std::cerr << "Oh noz vx or vy are non-finite! " << vx << ", " << vy << std::endl;
  const float w = 1.f + std::log(r2 + (1.f - r2)*ExpApproximation(-2.f*k)) / k;
  // if (!std::isfinite(w))
  //   std::cerr << "Oh noz w is not finite! " << w << std::endl;
  const float tmp = 1.f - w * w;
  // if (tmp < 0.f)
  //   std::cerr << "Oh noz tmp is negative! " << std::setprecision(std::numeric_limits<float>::digits10 + 1) << tmp << std::endl;
  const float rho = std::sqrt(std::max(0.f, tmp));
  // if (!std::isfinite(rho))
  //   std::cerr << "Oh noz rho is not finite! " << rho << ", " << w << std::endl;
  Eigen::Vector3f x{
    rho*vx, rho*vy, w
  };
  Eigen::Matrix3f frame = OrthogonalSystemZAligned(mu);
  // if (!frame.array().isFinite().all())
  //   std::cerr << "Oh noz the frame is not finite! " << frame << ", mu = " << mu << std::endl;
  return frame * x;
}

template<int N = 8>
Eigen::Vector3f Sample(const VonMisesFischerMixture<N> & mixture, std::array<double, 3> rs) noexcept
{
  const int idx = TowerSampling<N>(mixture.weights.data(), (float)rs[0]);
  return Sample(mixture.means.row(idx).matrix(), mixture.concentrations[idx], (float)rs[1], (float)rs[2]);
}

template<int N>
void InitializeForUnitSphere(VonMisesFischerMixture<N> & mixture)  noexcept
{
  assert ("!Not implemented!");
}

template<>
void InitializeForUnitSphere<2>(VonMisesFischerMixture<2> & mixture)  noexcept
{
  using MoVMF = VonMisesFischerMixture<2>;
  mixture.means <<
    -1., 0., 0.,
     1., 0., 0.;
  mixture.concentrations = MoVMF::ConcArray::Constant(0.1f);
  mixture.weights = typename MoVMF::WeightArray(1.f / MoVMF::NUM_COMPONENTS);
}

template<>
void InitializeForUnitSphere<8>(VonMisesFischerMixture<8> & mixture)  noexcept
{
  using MoVMF = VonMisesFischerMixture<8>;
    mixture.means <<
      0.66778004,0.73539025,0.11519922,
      -0.9836298,-0.008926501,0.1799797,
      -0.14629413,-0.7809748,0.60718733,
      0.49043104,0.04368747,-0.87038434,
      -0.10088497,0.42765132,0.8982965,
      -0.35999632,-0.6945797,-0.62286574,
      -0.43178025,0.7588791,-0.48751223,
      0.860208,-0.47938251,0.17388113;
  mixture.concentrations = MoVMF::ConcArray::Constant(2.f);
  mixture.weights = typename MoVMF::WeightArray(1.f / MoVMF::NUM_COMPONENTS);
}

template<>
void InitializeForUnitSphere<16>(VonMisesFischerMixture<16> & mixture)  noexcept
{
  using MoVMF = VonMisesFischerMixture<16>;
  mixture.means <<
    0.49468744,-0.1440351,-0.8570521,
    0.36735174,-0.8573688,-0.36051556,
    -0.40503788,-0.48854542,-0.772831,
    -0.32770208,-0.12784185,0.9360917,
    -0.46322507,-0.8861626,-0.011769729,
    -0.933043,0.16280192,-0.32082137,
    -0.4759586,0.8746706,-0.09173246,
    -0.2931369,0.36415973,-0.8840013,
    -0.9006278,-0.24284472,0.360411,
    -0.32578856,0.650924,0.685682,
    0.5459307,0.11810207,0.8294646,
    0.9575066,0.22513846,-0.18026012,
    0.84576833,-0.47598344,0.24107178,
    0.47158864,0.8217275,0.31995004,
    0.12246728,-0.77039504,0.6256942,
    0.34670526,0.77106386,-0.5340936;
    mixture.concentrations = MoVMF::ConcArray::Constant(5.f);
    mixture.weights = typename MoVMF::WeightArray(1.f / MoVMF::NUM_COMPONENTS);
}

template<int N, int M>
VonMisesFischerMixture<N*M> Product(const VonMisesFischerMixture<N> &m1, const VonMisesFischerMixture<M> &m2) noexcept
{
  static constexpr int NM = N*M;
  VonMisesFischerMixture<NM> result;
  for (int i=0; i<N; ++i)
  {
    result.means.block(i*M, 0, M, 3) = (m1.concentrations[i]*m1.means.row(i)).replicate(M,1) + (m2.means*m2.concentrations.replicate(1,3));
  }
  result.concentrations = result.means.matrix().rowwise().norm();
  result.means.colwise() /= result.concentrations;

  // Weights ...
  const typename VonMisesFischerMixture<N>::WeightArray  exponentials1 = 1.f+incremental::eps - (-2.f*m1.concentrations    ).exp();
  const typename VonMisesFischerMixture<M>::WeightArray  exponentials2 = 1.f+incremental::eps - (-2.f*m2.concentrations    ).exp();
  const typename VonMisesFischerMixture<NM>::WeightArray exponentialsk = 1.f+incremental::eps - (-2.f*result.concentrations).exp();

  for (int i=0; i<N; ++i)
  {
    for (int j=0; j<M; ++j)
    {
      const int k = i*M + j;
      result.weights[k] = m1.concentrations[i]*m2.concentrations[j]*exponentialsk[k] /
                          (2.f*PiFloat*result.concentrations[k]*exponentials1[i]*exponentials2[j] + incremental::eps);
      result.weights[k] *= ExpApproximation(m1.concentrations[i]*(m1.means.row(i).matrix().dot(result.means.row(k).matrix())-1.f) +
                                    m2.concentrations[j]*(m2.means.row(j).matrix().dot(result.means.row(k).matrix())-1.f));
      result.weights[k] *= m1.weights[i] * m2.weights[j];
    }
  }

  // Hack to avoid numerical problems ...
  result.concentrations = result.concentrations.max(K_THRESHOLD).min(K_THRESHOLD_MAX).eval();
  return result;
}

template<int N>
void Normalize(VonMisesFischerMixture<N> &mixture) noexcept
{
  const float wsum = mixture.weights.sum();
  if (unlikely(wsum <= 0.f))
  {
    // Case may happen due to underflow in calculations with exponentials
    mixture.weights.setConstant(1.f/N);
  }
  else
  {
    mixture.weights /= wsum;
  }
}

namespace incremental
{

template<int N>
void UpdateStatistics(VonMisesFischerMixture<N> &mixture, Data<N> &dta, const Params<N> &params, const Eigen::Vector3f &x, float weight) noexcept;

template<int N>
void MaximizationStep(VonMisesFischerMixture<N> &mixture, const Data<N> &dta, const Params<N> &params) noexcept;

template<int N>
void Fit(VonMisesFischerMixture<N> &mixture, Data<N> &dta, const Params<N> &params, Span<const Eigen::Vector3f> data, Span<const float> data_weights) noexcept
{
  assert(data.size() == data_weights.size());
  assert(params.prior_mode != nullptr);

  for (int i=0; i<data.size(); ++i)
  {
    UpdateStatistics(mixture, dta, params, data[i], data_weights[i]);
    if (dta.avg_positions.Count() % params.maximization_step_every == 0)
    {
      MaximizationStep(mixture, dta, params);

      // Clear the statistics. Only keep average weights since they don't depend on the mixture parameters.
      auto backup = dta.avg_weights;
      dta = Data<N>{};
      dta.avg_weights = backup;
    }
  }
}

template<int N>
void UpdateStatistics(VonMisesFischerMixture<N> & mixture, Data<N> &fitdata, const Params<N> &params, const Eigen::Vector3f & x, float weight) noexcept
{
  Eigen::Array<float, N, 1> responsibilities = mixture.weights * ComponentPdfs(mixture, x) + eps;
  responsibilities /= responsibilities.sum();

#if 0
  if (fitdata.data_count_weights == 0)
    fitdata.avg_weights = weight;
  else
    fitdata.avg_weights = Lerp(fitdata.avg_weights, weight, (float)std::pow(double(fitdata.data_count_weights), -0.75));

  if (fitdata.data_count == 0)
  {
    fitdata.avg_responsibilities_unweighted = responsibilities;
    fitdata.avg_responsibilities = weight * responsibilities;
    fitdata.avg_positions = weight*(responsibilities.matrix()*x.transpose()).array();
  }
  else
  {
    // How much to "trust" new data.
    const float mix_factor = (float)std::pow(double(fitdata.data_count), -0.75f);
    fitdata.avg_responsibilities_unweighted += mix_factor*(responsibilities        - fitdata.avg_responsibilities_unweighted);
    fitdata.avg_responsibilities            += mix_factor*(weight*responsibilities - fitdata.avg_responsibilities);
    fitdata.avg_positions                   += mix_factor*(weight*(responsibilities.matrix()*x.transpose()).array() - fitdata.avg_positions);
  }
  ++fitdata.data_count;
  ++fitdata.data_count_weights;
#else
  fitdata.avg_weights += weight;
  fitdata.avg_responsibilities_unweighted += responsibilities;
  fitdata.avg_responsibilities  += weight * responsibilities;
  fitdata.avg_positions += weight*(responsibilities.matrix()*x.transpose()).array();
#endif
}

template<int N>
void MaximizationStep(VonMisesFischerMixture<N> & mixture, const Data<N> &dta, const Params<N> &params) noexcept
{

  const std::uint64_t unique_data_count = dta.avg_responsibilities.Count();
  for (int k = 0; k < VonMisesFischerMixture<N>::NUM_COMPONENTS; ++k)
  {
    const float scaled_responsibilities = dta.avg_responsibilities()[k] / (dta.avg_weights() + eps);

    // The eps is there in case both of the former terms evaluate to zero!
    mixture.weights[k] = (params.prior_nu-1)/unique_data_count*params.prior_mode->weights[k] + scaled_responsibilities + eps;

    { // Posterior of mean.
      const float diminished_prior_factor = dta.avg_weights() * params.prior_tau / unique_data_count;
      mixture.means.row(k) = (diminished_prior_factor*params.prior_mode->means.row(k) + dta.avg_positions().row(k)) /
        (diminished_prior_factor + dta.avg_responsibilities()[k] + eps);
      float norm = mixture.means.row(k).matrix().norm();
      if (norm > eps)
        mixture.means.row(k) /= norm;
      else
        mixture.means.row(k).matrix() = Eigen::Vector3f{1.f, 0.f, 0.f};
    }

    {
      #if 1
      // Note: Don't use avg_responsibilities_unweighted[k]*avg_weights here. It computes the average position wrong
      // in a way that mean_cosine is way overestimated.
      const float mean_cosine = (1.f/(dta.avg_responsibilities()[k] + eps)) * dta.avg_positions().row(k).matrix().norm();
      const float conc_estimate = MeanCosineToConc(mean_cosine);
      const float diminished_alpha = params.prior_alpha/unique_data_count;
      const float post_conc =
        (params.prior_mode->concentrations[k]*diminished_alpha + conc_estimate) / (diminished_alpha + 1.f);
      mixture.concentrations[k] = post_conc;
      #endif

      #if 0
      // This does not do percievably better than the simpler code above!!
      // Here the prior is on the mean cosine, forcing me to go back and forth with the cosine-k-conversions.
      const float prior_cos = ConcToMeanCos(params.prior_mode->concentrations[k]);
      const float mean_cosine = (1.f/(dta.avg_responsibilities()[k] + eps)) * dta.avg_positions().row(k).matrix().norm();
      const float diminished_alpha = params.prior_alpha/unique_data_count;
      const float mean_cosine_post = (diminished_alpha*prior_cos + mean_cosine) / (diminished_alpha + 1.f);
      mixture.concentrations[k] = MeanCosineToConc(mean_cosine_post);
      #endif
    }
  }

  mixture.concentrations = mixture.concentrations.max(K_THRESHOLD).min(K_THRESHOLD_MAX).eval();

  assert(mixture.weights.sum() > 0.f);
  mixture.weights /= mixture.weights.sum();
}

} // namespace incremental

} // namespace vmf_fitting

#define INSTANTIATE_VonMisesFischerMixture(n) \
  template void vmf_fitting::incremental::Fit<n>(VonMisesFischerMixture<n> &mixture, Data<n> &fitdata, const Params<n> &params, Span<const Eigen::Vector3f> data, Span<const float> data_weights) noexcept; \
  template float  vmf_fitting::Pdf<n>(const VonMisesFischerMixture<n> &mixture, const Eigen::Vector3f &pos) noexcept; \
  template Eigen::Vector3f  vmf_fitting::Sample<n>(const VonMisesFischerMixture<n> &mixture, std::array<double, 3> rs) noexcept; \
  template void  vmf_fitting::InitializeForUnitSphere(VonMisesFischerMixture<n> &mixture) noexcept; \
  template void vmf_fitting::Normalize(VonMisesFischerMixture<n> &mixture) noexcept; \
  template void vmf_fitting::ExpApproximation<n>(Eigen::Array<float, n, 1> &vals);

INSTANTIATE_VonMisesFischerMixture(2)
INSTANTIATE_VonMisesFischerMixture(8)
INSTANTIATE_VonMisesFischerMixture(16)

template vmf_fitting::VonMisesFischerMixture<16> vmf_fitting::Product(const vmf_fitting::VonMisesFischerMixture<2> &m1, const vmf_fitting::VonMisesFischerMixture<8> &m2) noexcept;


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

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

#include <mrpt/config.h>
#include <mrpt/vision/utils.h>
#include <mrpt/vision/pnp_algos.h>

// Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable PNP classes
#include <mrpt/config.h>

#if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM<0x240
#	undef MRPT_HAS_OPENCV
#	define MRPT_HAS_OPENCV 0
#endif

#include <iostream>

#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Core>
#include <Eigen/Dense>

#include <mrpt/otherlibs/do_opencv_includes.h>
#if MRPT_HAS_OPENCV
#	include <opencv2/core/eigen.hpp>
#endif

#include "dls.h"
#include "epnp.h"
#include "upnp.h"
#include "p3p.h"
#include "ppnp.h"
#include "posit.h"
#include "lhm.h"
#include "rpnp.h"
#include "so3.h"

bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
        bool ret = d.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return ret;

        #else
		throw(-1);
        #endif
    }
    catch(int e)
    {
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::epnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::epnp e(cam_in_cv, obj_pts_cv, img_pts_cv);
        e.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return true;

        #else
        throw(-1);
        #endif
    }
    catch(int e){
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::upnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::upnp u(cam_in_cv, obj_pts_cv, img_pts_cv);
        u.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return true;
        #else
        throw(-1);
        #endif
    }
    catch(int e)
    {
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::p3p(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){

    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::p3p p(cam_in_eig);
        bool ret = p.solve(R,t, obj_pts_eig, img_pts_eig);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::rpnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::rpnp r(obj_pts_eig, img_pts_eig, cam_in_eig, n);
        bool ret = r.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::ppnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
	try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::ppnp p(obj_pts_eig,img_pts_eig, cam_in_eig);

        bool ret = p.compute_pose(R,t,n);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::posit(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
	try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::posit p(obj_pts_eig,img_pts_eig, cam_in_eig, n);

        bool ret = p.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }

}

bool mrpt::vision::pnp::CPnP::lhm(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::lhm l(obj_pts_eig, img_pts_eig, cam_intrinsic, n);

        bool ret = l.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat<<t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::so3(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::p3p p(cam_in_eig);
        p.solve(R,t, obj_pts_eig, img_pts_eig);

        mrpt::vision::pnp::so3 s(obj_pts_eig, img_pts_eig, cam_in_eig, n);
        bool ret = s.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat<<t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}


#include <boost/math/tools/roots.hpp>
#include <ibs>
#include <ste>
//#include <boost/math/tools/roots.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <map>
#include <math.h>
#include <string>

template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, float charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {
    T volt1, volt2, volt3;
    T dvolt1, dvolt2, dvolt3;
    volt1 = ch * volts[0] * sin(phi);
    volt2 = ch * volts[1] * sin((hs[1] / hs[0]) * phi);
    volt3 = ch * volts[2] * sin((hs[2] / hs[0]) * phi);

    dvolt1 = ch * volts[0] * cos(phi);
    dvolt2 = ch * volts[1] * (hs[1] / hs[0]) * cos((hs[1] / hs[0]) * phi);
    dvolt3 = ch * volts[2] * (hs[2] / hs[0]) * cos((hs[2] / hs[0]) * phi);
    std::tuple<double, double> out = {volt1 + volt2 + volt3 - U0,
                                      dvolt1 + dvolt2 + dvolt3};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

template <class T>
T synchronousPhaseFunctorDeriv(T x, std::vector<double> &voltages,
                               std::vector<double> &harmnumbers, double charge,
                               T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
}
double pcoeff(double voltage, double angularFreq, double p0,
              double betaRelativistic, double charge) {
  return (angularFreq * voltage * charge) / (2.0 * pi * p0 * betaRelativistic);
}

double HamiltonianTripleRf(double tcoeff, std::vector<double> &voltages,
                           std::vector<double> &harmonicNumbers,
                           double phiSynchronous, double t, double delta,
                           double omega0, double p0, double betaRelativistic,
                           double charge) {
  double kinetic, potential1, potential2, potential3;
  kinetic = 0.5 * tcoeff * pow(delta, 2);

  std::printf("%-30s %16.8e\n", "kinetic", kinetic);
  potential1 = pcoeff(voltages[0], omega0, p0, betaRelativistic, charge) *
               (cos(harmonicNumbers[0] * omega0 * t) - cos(phiSynchronous) +
                (harmonicNumbers[0] * omega0 * t - phiSynchronous) *
                    sin(phiSynchronous));
  std::printf("%-30s %16.8e\n", "pcoeff0",
              pcoeff(voltages[0], omega0, p0, betaRelativistic, charge));
  std::printf("%-30s %16.8e\n", "phase0", harmonicNumbers[0] * omega0 * t);
  std::printf("%-30s %16.8e\n", "potential1", potential1);

  potential2 =
      pcoeff(voltages[1], omega0, p0, betaRelativistic, charge) *
      (harmonicNumbers[0] / harmonicNumbers[1]) *
      (cos(harmonicNumbers[1] * omega0 * t) -
       cos(harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]) +
       (harmonicNumbers[1] * omega0 * t -
        harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]) *
           sin(harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]));

  potential3 =
      pcoeff(voltages[2], omega0, p0, betaRelativistic, charge) *
      (harmonicNumbers[0] / harmonicNumbers[2]) *
      (cos(harmonicNumbers[2] * omega0 * t) -
       cos(harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]) +
       (harmonicNumbers[2] * omega0 * t -
        harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]) *
           sin(harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]));

  return kinetic + potential1 + potential2 + potential3;
};

double synchrotronTune(std::map<string, double> &twissheadermap,
                       std::vector<double> h, std::vector<double> v) {
  double p0 = twissheadermap["PC"] * 1.0e9;
  double phis = twissheadermap["phis"];
  double charge = twissheadermap["CHARGE"];
  double Omega2 =
      (h[0] * twissheadermap["eta"] * charge) / (2.0 * pi * p0) *
      (v[0] * cos(phis) + v[1] * (h[1] / h[0]) * cos((h[1] / h[0]) * phis) +
       v[2] * (h[2] / h[0]) * cos((h[2] / h[0]) * phis));
  return sqrt(abs(Omega2));
};

int main() {
  string twissfilename = "../src/b2_design_lattice_1996.twiss";
  map<string, double> twissheadermap;
  twissheadermap = GetTwissHeader(twissfilename);

  // rf settings
  std::vector<double> h, v;
  h.push_back(400.0);
  v.push_back(-1.5e6);

  // bunch length
  double sigs = 0.005;
  // aatom
  double aatom = emass / pmass;
  // set energy loss per turn manually
  // TODO: implement radiation update of twiss
  twissheadermap["U0"] = 174e3;
  // update twiss header with long parameters
  ste_longitudinal::updateTwissHeaderLong(twissheadermap, h, v, aatom, sigs);

  double energyLostPerTurn = 174000;    // radation losses per turn per particle
  double acceleratorLength = 240.00839; // length in meter
  double gammar = twissheadermap["GAMMA"]; // relativistic gamma
  // double eta = 0.0007038773471 -
  //          1 / pow(gammar, 2); // slip factor approx alpha - 1/ gammar**2
  double betar = twissheadermap["betar"]; // relativistic beta
  double trev = twissheadermap["trev"];
  double h0 = 400.0;
  double h1 = 400; // 1200.0;
  double h2 = 400; // 1400.0;
  double v0 = -1.5e6;
  double v1 = 0.0; // 20.0e6;
  double v2 = 0.0; // 17.14e6;
  double omega0 = (2 * pi) / trev;
  double p0 = 1.7e9;
  double charge = -1.0;
  std::vector<double> hnumbers = {h0, h1, h2};
  std::vector<double> voltages = {v0, v1, v2};

  double search1 =
      trev * h0 * omega0 /
      (8 * max(max(h0, h1), h2)); // give positive offset to find upstream
                                  // root and not downstream root
  double search2 = trev * h0 * omega0 / (8 * max(max(h0, h1), h2)) -
                   trev * h0 * omega0 / min(min(h0, h1), h2);
  double searchWidth = trev * h0 * omega0 / (2 * max(max(h0, h1), h2));
  ste_output::cyan();
  std::printf("%-30s : %16.6f\n", "Search1", search1 / pi * 180);
  std::printf("%-30s : %16.6f\n", "Search2", search2 / pi * 180);
  std::printf("%-30s : %16.6f\n", "Width", searchWidth / pi * 180);
  ste_output::reset();

  double synchronousPhase0 = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search1,
      search1 - searchWidth, search1 + searchWidth);
  double synchronousPhase1 = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search2,
      search2 - searchWidth, search2 + searchWidth);

  ste_output::blue();
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase", search1,
              synchronousPhase0 / pi * 180, synchronousPhase0 / (h0 * omega0));
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase", search2,
              synchronousPhase1 / pi * 180, synchronousPhase1 / (h0 * omega0));
  ste_output::yellow();
  std::printf("%-30s : %16.6f %16.6f %16.6e\n", "synchronous phase", 173.0,
              twissheadermap["phis"],
              twissheadermap["phis"] / 180.0 * pi / (h0 * omega0));
  ste_output::reset();

  std::cout << "Find next extremum of Hamiltonian" << std::endl;

  double synchronousPhase0Next = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search1 + searchWidth,
      search1 + searchWidth / 2, search1 + 2 * searchWidth);
  double synchronousPhase1Next = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search2 + searchWidth,
      search2 + searchWidth / 2, search2 + 2 * searchWidth);

  ste_output::blue();
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase next",
              search1 + searchWidth, synchronousPhase0Next / pi * 180,
              synchronousPhase0Next / (h0 * omega0));
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase next ",
              search2 + searchWidth, synchronousPhase1Next / pi * 180,
              synchronousPhase1Next / (h0 * omega0));
  ste_output::yellow();
  std::printf("%-30s : %16.6f\n", "synchronous phase", twissheadermap["phis"]);
  ste_output::reset();

  std::printf("%s", "\nHamiltonians\n");
  ste_output::blue();
  double tc = ste_longitudinal::tcoeff(twissheadermap, h0);
  double tcval = tc / (h0 * omega0);
  double ohammax = HamiltonianTripleRf(
      tcval, voltages, hnumbers, synchronousPhase1,
      synchronousPhase1Next / (h0 * omega0), 0.0, omega0, p0, betar, charge);
  double hammax =
      ste_longitudinal::Hamiltonian(twissheadermap, hnumbers, voltages, tc,
                                    synchronousPhase1Next / (h0 * omega0), 0.0);
  std::printf("%-30s : %16.6f \n", "tcoeff", tcval);
  std::printf("%-30s : %16.6f \n", "ohammax", ohammax);
  ste_output::yellow();
  std::printf("%-30s : %16.6f \n", "hammax", hammax);
  ste_output::reset();

  std::cout << "Synchrotron tune : "
            << synchrotronTune(twissheadermap, hnumbers, voltages) << std::endl;
  std::cout << "Synchrotron tune (Hz): "
            << synchrotronTune(twissheadermap, hnumbers, voltages) * omega0 /
                   (2.0 * pi)
            << std::endl;
  std::printf("%-30s : %16.6f \n", "syncTune",
              synchrotronTune(twissheadermap, hnumbers, voltages));
  std::printf("%-30s : %16.6f \n", "syncTune Hz",
              synchrotronTune(twissheadermap, hnumbers, voltages) * omega0 /
                  (2.0 * pi));
  ste_output::yellow();
  std::printf("%-30s : %16.6f \n", "syncTune", twissheadermap["qs"]);
  std::printf("%-30s : %16.6f \n", "syncTune Hz",
              twissheadermap["qs"] * omega0 / (2.0 * pi));
  ste_output::reset();

  std::printf("%16.6e  %16.6e %16.6e %16.6e %16.6e\n", synchronousPhase0Next,
              synchronousPhase0,
              abs((synchronousPhase0Next - synchronousPhase0) / pi * 180.0), h0,
              omega0);
  std::printf("%-30s : %16.6e \n", "tauhat",
              abs((synchronousPhase0Next - synchronousPhase0) / (h0 * omega0)));
  ste_output::yellow();
  std::printf("%-30s : %16.6e \n", "tauhat", twissheadermap["tauhat"]);
  std::printf("%-30s : %16.6e \n", "tauhat",
              (twissheadermap["phis"] - (twissheadermap["phis"] - 180.0)) /
                  180.0 * pi / (h0 * omega0));
  ste_output::reset();
  return 0;
}