/**
 * \file boost/numeric/ublasx/operation/balance.hpp
 *
 * \brief Balance a matrix or a pair of matrices to improve eigenvalue accuracy.
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_BALANCE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_BALANCE_HPP

#include <boost/mpl/assert.hpp>
#include <boost/numeric/bindings/lapack/computational/gebak.hpp>
#include <boost/numeric/bindings/lapack/computational/gebal.hpp>
#include <boost/numeric/bindings/lapack/computational/ggbak.hpp>
#include <boost/numeric/bindings/lapack/computational/ggbal.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_same.hpp>

//TODO: implement overloaded functions for matrices with special structure (e.g., symmetric matrices).

namespace boost { namespace numeric { namespace ublasx {

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

namespace detail { namespace /*<unnamed>*/ {

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 * \tparam SVectorT The type of the scaling vector.
 * \tparam PVectorT The type of the permuting vector.
 * \tparam BVectorT The type of the balancing matrix.
 *
 * \param A The matrix to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \param want_scaling_vec Tells if the caller has requested to compute the
 *  scaling vector.
 * \param scaling_vec The computed scaling vector.
 * \param want_permuting_vec Tells if the caller has requested to compute the
 *  permutation vector.
 * \param permuting_vec The computed permuting vector.
 * \param want_balancing_mat Tells if the caller has requested to compute the
 *  balancing matrix.
 * \param balancing_mat The computed balancing matrix.
 * \return none, but \a A, \a scaling_vec, \a permuting_vec, and
 *  \a balancing_mat are changed (possibly, if requested).
 *
 * Version for matrices with column-major layout.
 */
template <
	typename MatrixT,
	typename SVectorT,
	typename PVectorT,
	typename BMatrixT
>
void balance_impl(MatrixT& A,
				  bool scale,
				  bool permute,
				  bool want_scaling_vec,
				  SVectorT& scaling_vec,
				  bool want_permuting_vec,
				  PVectorT& permuting_vec,
				  bool want_balancing_mat,
				  BMatrixT& balancing_mat,
				  column_major_tag)
{
	typedef typename matrix_traits<MatrixT>::value_type value_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;
	typedef typename type_traits<value_type>::real_type real_type;
	typedef vector<real_type> work_vector_type;

	// pre: A must be square
	BOOST_UBLAS_CHECK(
			ublasx::num_rows(A) == ublasx::num_columns(A),
			bad_size()
		);

	char job;

	if (scale && !permute)
	{
		// Only scale
		job = 'S';
	}
	else if (!scale && permute)
	{
		// Only permute
		job = 'P';
	}
	else if (!scale && !permute)
	{
		// Do nothing but simply set ILO = 1, IHI = N, SCALE(I) = 1.0 for i = 1,...,N;
		job = 'N';
	}
	else
	{
		// Both scale and permute
		job = 'B';
	}

	size_type n = num_rows(A);
	::fortran_int_t ilo;
	::fortran_int_t ihi;
	work_vector_type tmp_scale_vec(n);

	::boost::numeric::bindings::lapack::gebal(job,
											  A,
											  ilo,
											  ihi,
											  tmp_scale_vec);

	if (want_scaling_vec)
	{
		if (size(scaling_vec) != n)
		{
			scaling_vec.resize(n, false);
		}

		for (size_type i = 0; i < (static_cast<size_type>(ilo)-1); ++i)
		{
			scaling_vec(i) = real_type(1);
		}
		for (size_type i = ilo-1; i < static_cast<size_type>(ihi); ++i)
		{
			scaling_vec(i) = tmp_scale_vec(i);
		}
		for (size_type i = ihi; i < n; ++i)
		{
			scaling_vec(i) = real_type(1);
		}
	}
	if (want_permuting_vec)
	{
		if (size(permuting_vec) != n)
		{
			permuting_vec.resize(n, false);
		}

		for (size_type i = 0; i < n; ++i)
		{
			permuting_vec(i) = i;
		}
		for (size_type i = n-1; i >= static_cast<size_type>(ihi); --i)
		{
			size_type j(tmp_scale_vec(i)-1);
			::std::swap(permuting_vec(i), permuting_vec(j));
		}
		for (size_type i = 0; i < (static_cast<size_type>(ilo)-1); ++i)
		{
			size_type j(tmp_scale_vec(i)-1);
			::std::swap(permuting_vec(i), permuting_vec(j));
		}
	}
	if (want_balancing_mat)
	{
		balancing_mat = identity_matrix<real_type>(n,n);

		::boost::numeric::bindings::tag::right side;

		::boost::numeric::bindings::lapack::gebak(job,
												  side,
												  ilo,
												  ihi,
												  tmp_scale_vec,
												  balancing_mat);
	}
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 * \tparam SVectorT The type of the scaling vector.
 * \tparam PVectorT The type of the permuting vector.
 * \tparam BVectorT The type of the balancing matrix.
 *
 * \param A The matrix to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \param want_scaling_vec Tells if the caller has requested to compute the
 *  scaling vector.
 * \param scaling_vec The computed scaling vector.
 * \param want_permuting_vec Tells if the caller has requested to compute the
 *  permutation vector.
 * \param permuting_vec The computed permuting vector.
 * \param want_balancing_mat Tells if the caller has requested to compute the
 *  balancing matrix.
 * \param balancing_mat The computed balancing matrix.
 * \return none, but \a A, \a scaling_vec, \a permuting_vec, and
 *  \a balancing_mat are changed (possibly, if requested).
 *
 * Version for matrices with row-major layout.
 */
template <
	typename MatrixT,
	typename SVectorT,
	typename PVectorT,
	typename BMatrixT
>
void balance_impl(MatrixT& A,
				  bool scale,
				  bool permute,
				  bool want_scaling_vec,
				  SVectorT& scaling_vec,
				  bool want_permuting_vec,
				  PVectorT& permuting_vec,
				  bool want_balancing_mat,
				  BMatrixT& balancing_mat,
				  row_major_tag)
{
    // Note: LAPACK works with column-major matrices

    typedef typename matrix_traits<MatrixT>::value_type value_type;

    typedef matrix<value_type, column_major> colmaj_matrix_type;

	colmaj_matrix_type tmp_A(A);
	colmaj_matrix_type tmp_balancing_mat;

	balance_impl(tmp_A,
				 scale,
				 permute,
				 want_scaling_vec,
				 scaling_vec,
				 want_permuting_vec,
				 permuting_vec,
				 want_balancing_mat,
				 tmp_balancing_mat,
				 column_major_tag());

	A = tmp_A;

	if (want_balancing_mat)
	{
		balancing_mat = tmp_balancing_mat;
	}
}

/**
 * \brief Diagonal matrix balancing to improve generalized eigenvalue accuracy.
 *
 * \tparam Matrix1T The type of the first matrix in the input pencil to be
 *  balanced.
 * \tparam Matrix2T The type of the second matrix in the input pencil to be
 *  balanced.
 * \tparam SLVectorT The type of the scaling vector applied to the left side
 *  of the input pencil.
 * \tparam SRVectorT The type of the scaling vector applied to the right side
 *  of the input pencil.
 * \tparam PLVectorT The type of the permuting vector applied to the left side
 *  pf the input pencil.
 * \tparam PRVectorT The type of the permuting vector applied to the right side
 *  pf the input pencil.
 * \tparam BVectorT The type of the balancing matrix.
 *
 * \param A The first matrix in the input pencil (A,B) to be balanced.
 * \param B The second matrix in the input pencil (A,B) to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \param want_scaling_vec Tells if the caller has requested to compute the
 *  scaling vectors.
 * \param left_scaling_vec The computed scaling vector applied to the left side
 *  of the input pencil (A,B).
 * \param right_scaling_vec The computed scaling vector applied to the right
 *  side of the input pencil (A,B).
 * \param want_permuting_vec Tells if the caller has requested to compute the
 *  permutation vectors.
 * \param left_permuting_vec The computed permuting vector applied to the left
 *  size of the input pencil (A,B).
 * \param right_permuting_vec The computed permuting vector applied to the right
 *  size of the input pencil (A,B).
 * \param want_balancing_mat Tells if the caller has requested to compute the
 *  balancing matrix.
 * \param balancing_mat The computed balancing matrix.
 * \return none, but \a A, \a B, \a left_scaling_vec, \a right_scaling_vec,
 *  \a left_permuting_vec, \a right_permuting_vec, and \a balancing_mat are
 *  changed (possibly, if requested).
 *
 * Version for matrices with column-major layout.
 */
template <
	typename Matrix1T,
	typename Matrix2T,
	typename SLVectorT,
	typename PLVectorT,
	typename SRVectorT,
	typename PRVectorT,
	typename BMatrixT
>
void balance_impl(Matrix1T& A,
				  Matrix2T& B,
				  bool scale,
				  bool permute,
				  bool want_scaling_vec,
				  SLVectorT& left_scaling_vec,
				  SRVectorT& right_scaling_vec,
				  bool want_permuting_vec,
				  PLVectorT& left_permuting_vec,
				  PRVectorT& right_permuting_vec,
				  bool want_balancing_mat,
				  BMatrixT& balancing_mat,
				  column_major_tag)
{
	typedef typename promote_traits<
						typename matrix_traits<Matrix1T>::value_type,
						typename matrix_traits<Matrix2T>::value_type
				>::promote_type value_type;
	typedef typename promote_traits<
						typename matrix_traits<Matrix1T>::size_type,
						typename matrix_traits<Matrix2T>::size_type
				>::promote_type size_type;
	typedef typename type_traits<value_type>::real_type real_type;
	typedef vector<real_type> work_vector_type;

    // pre: same orientation category
	BOOST_MPL_ASSERT(
		(
			::boost::is_same<
					typename matrix_traits<Matrix1T>::orientation_category,
					typename matrix_traits<Matrix2T>::orientation_category
			>
		)
	);
	// pre: A must be square
	BOOST_UBLAS_CHECK(
			ublasx::num_rows(A) == ublasx::num_columns(A),
			bad_size()
		);
	// pre: B must be square
	BOOST_UBLAS_CHECK(
			ublasx::num_rows(B) == ublasx::num_columns(B),
			bad_size()
		);
	// pre: A and B must be of the same order
	BOOST_UBLAS_CHECK(
			ublasx::num_rows(A) == ublasx::num_rows(B),
			bad_size()
		);

	char job;

	if (scale && !permute)
	{
		// Only scale
		job = 'S';
	}
	else if (!scale && permute)
	{
		// Only permute
		job = 'P';
	}
	else if (!scale && !permute)
	{
		// Do nothing but simply set ILO = 1, IHI = N, LSCALE(I) = 1.0 and RSCALE(I) for i = 1,...,N;
		job = 'N';
	}
	else
	{
		// Both scale and permute
		job = 'B';
	}

	size_type n = num_rows(A);
	::fortran_int_t ilo;
	::fortran_int_t ihi;
	work_vector_type tmp_lscale_vec(n);
	work_vector_type tmp_rscale_vec(n);

	::boost::numeric::bindings::lapack::ggbal(job,
											  A,
											  ilo,
											  ihi,
											  tmp_lscale_vec,
											  tmp_rscale_vec);

	if (want_scaling_vec)
	{
		if (size(left_scaling_vec) != n)
		{
			left_scaling_vec.resize(n, false);
		}
		if (size(right_scaling_vec) != n)
		{
			right_scaling_vec.resize(n, false);
		}

		for (size_type i = 0; i < (static_cast<size_type>(ilo)-1); ++i)
		{
			left_scaling_vec(i) = right_scaling_vec(i)
								= real_type(1);
		}
		for (size_type i = ilo-1; i < static_cast<size_type>(ihi); ++i)
		{
			left_scaling_vec(i) = tmp_lscale_vec(i);
			right_scaling_vec(i) = tmp_rscale_vec(i);
		}
		for (size_type i = ihi; i < n; ++i)
		{
			left_scaling_vec(i) = right_scaling_vec(i)
								= real_type(1);
		}
	}
	if (want_permuting_vec)
	{
		if (size(left_permuting_vec) != n)
		{
			left_permuting_vec.resize(n, false);
		}
		if (size(right_permuting_vec) != n)
		{
			right_permuting_vec.resize(n, false);
		}

		for (size_type i = 0; i < n; ++i)
		{
			left_permuting_vec(i) = right_permuting_vec(i)
								  = i;
		}
		for (size_type i = n-1; i >= static_cast<size_type>(ihi); --i)
		{
			size_type j;
			j = tmp_lscale_vec(i)-1;
			::std::swap(left_permuting_vec(i), left_permuting_vec(j));
			j = tmp_rscale_vec(i)-1;
			::std::swap(right_permuting_vec(i), right_permuting_vec(j));
		}
		for (size_type i = 0; i < (static_cast<size_type>(ilo)-1); ++i)
		{
			size_type j;
			j = tmp_lscale_vec(i)-1;
			::std::swap(left_permuting_vec(i), left_permuting_vec(j));
			j = tmp_rscale_vec(i)-1;
			::std::swap(right_permuting_vec(i), right_permuting_vec(j));
		}
	}
	if (want_balancing_mat)
	{
		balancing_mat = identity_matrix<real_type>(n,n);

		::boost::numeric::bindings::tag::right side;

		::boost::numeric::bindings::lapack::ggbak(job,
												  side,
												  ilo,
												  ihi,
												  tmp_lscale_vec,
												  tmp_rscale_vec,
												  balancing_mat);
	}
}

/**
 * \brief Diagonal matrix balancing to improve generalized eigenvalue accuracy.
 *
 * \tparam Matrix1T The type of the first matrix in the input pencil to be
 *  balanced.
 * \tparam Matrix2T The type of the second matrix in the input pencil to be
 *  balanced.
 * \tparam SLVectorT The type of the scaling vector applied to the left side
 *  of the input pencil.
 * \tparam SRVectorT The type of the scaling vector applied to the right side
 *  of the input pencil.
 * \tparam PLVectorT The type of the permuting vector applied to the left side
 *  pf the input pencil.
 * \tparam PRVectorT The type of the permuting vector applied to the right side
 *  pf the input pencil.
 * \tparam BVectorT The type of the balancing matrix.
 *
 * \param A The first matrix in the input pencil (A,B) to be balanced.
 * \param B The second matrix in the input pencil (A,B) to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \param want_scaling_vec Tells if the caller has requested to compute the
 *  scaling vectors.
 * \param left_scaling_vec The computed scaling vector applied to the left side
 *  of the input pencil (A,B).
 * \param right_scaling_vec The computed scaling vector applied to the right
 *  side of the input pencil (A,B).
 * \param want_permuting_vec Tells if the caller has requested to compute the
 *  permutation vectors.
 * \param left_permuting_vec The computed permuting vector applied to the left
 *  size of the input pencil (A,B).
 * \param right_permuting_vec The computed permuting vector applied to the right
 *  size of the input pencil (A,B).
 * \param want_balancing_mat Tells if the caller has requested to compute the
 *  balancing matrix.
 * \param balancing_mat The computed balancing matrix.
 * \return none, but \a A, \a B, \a left_scaling_vec, \a right_scaling_vec,
 *  \a left_permuting_vec, \a right_permuting_vec, and \a balancing_mat are
 *  changed (possibly, if requested).
 *
 * Version for matrices with row-major layout.
 */
template <
	typename Matrix1T,
	typename Matrix2T,
	typename SLVectorT,
	typename PLVectorT,
	typename SRVectorT,
	typename PRVectorT,
	typename BMatrixT
>
void balance_impl(Matrix1T& A,
				  Matrix2T& B,
				  bool scale,
				  bool permute,
				  bool want_scaling_vec,
				  SLVectorT& left_scaling_vec,
				  SRVectorT& right_scaling_vec,
				  bool want_permuting_vec,
				  PLVectorT& left_permuting_vec,
				  PRVectorT& right_permuting_vec,
				  bool want_balancing_mat,
				  BMatrixT& balancing_mat,
				  row_major_tag)
{
    // Note: LAPACK works with column-major matrices

	typedef typename promote_traits<
						typename matrix_traits<Matrix1T>::value_type,
						typename matrix_traits<Matrix2T>::value_type
				>::promote_type value_type;

    typedef matrix<value_type, column_major> colmaj_matrix_type;

	colmaj_matrix_type tmp_A(A);
	colmaj_matrix_type tmp_B(B);
	colmaj_matrix_type tmp_balancing_mat;

	balance_impl(tmp_A,
				 tmp_B,
				 scale,
				 permute,
				 want_scaling_vec,
				 left_scaling_vec,
				 right_scaling_vec,
				 want_permuting_vec,
				 left_permuting_vec,
				 right_permuting_vec,
				 want_balancing_mat,
				 tmp_balancing_mat,
				 column_major_tag());

	A = tmp_A;
	B = tmp_B;

	if (want_balancing_mat)
	{
		balancing_mat = tmp_balancing_mat;
	}
}

}} // Namespace detail::<unnamed>

//@{ Balance of Single Matrix

/// Traits type class for the \c balance operation (single matrix version).
template <typename MatrixT>
struct balance_traits
{
	/// The type of the balanced matrix.
	typedef matrix<typename matrix_traits<MatrixT>::value_type,
				   typename layout_type<MatrixT>::type> balanced_matrix_type;
	/// The type of the balancing matrix.
	typedef matrix<typename matrix_traits<MatrixT>::value_type,
				   typename layout_type<MatrixT>::type> balancing_matrix_type;
	/// The type of the scaling vector.
	typedef vector<typename type_traits<
						typename matrix_traits<MatrixT>::value_type
					>::real_type> scaling_vector_type;
	/// The type of the permuting vector.
	typedef vector<typename matrix_traits<MatrixT>::size_type> permuting_vector_type;
};

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 *
 * \param A The matrix to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return none, but matrix \a A is overwritten by its balanced counterpart.
 */
template <typename MatrixT>
BOOST_UBLAS_INLINE
void balance_inplace(MatrixT& A, bool scale = true, bool permute = true)
{
	typedef typename matrix_traits<MatrixT>::orientation_category orientation_category;
//	typedef typename matrix_traits<MatrixT>::size_type size_type;
//	typedef typename matrix_traits<MatrixT>::value_type value_type;
//	typedef typename type_traits<value_type>::real_type real_type;

//	vector<real_type> dummy_scaling_vec;
//	vector<size_type> dummy_permuting_vec;
//	matrix<value_type,column_major> dummy_balancing_mat;
	typename balance_traits<MatrixT>::scaling_vector_type dummy_scaling_vec;
	typename balance_traits<MatrixT>::permuting_vector_type dummy_permuting_vec;
	typename balance_traits<MatrixT>::balancing_matrix_type dummy_balancing_mat;

	detail::balance_impl(A,
						 scale,
						 permute,
						 false,
						 dummy_scaling_vec,
						 false,
						 dummy_permuting_vec,
						 false,
						 dummy_balancing_mat,
						 orientation_category());
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 * \tparam MatrixExprT The type of the balancing matrix.
 *
 * \param A The matrix to be balanced.
 * \param balancing_mat The computed balancing matrix.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return none, but matrices \a A and \a balancing_mat are overwritten by the
 *  balanced counterpart of \a A and by the computed balanced matrix,
 *  respectively.
 */
template <typename MatrixT, typename MatrixExprT>
BOOST_UBLAS_INLINE
void balance_inplace(MatrixT& A,
					 matrix_container<MatrixExprT>& balancing_mat,
					 bool scale = true,
					 bool permute = true)
{
	typedef typename matrix_traits<MatrixT>::orientation_category orientation_category;
//	typedef typename matrix_traits<MatrixT>::size_type size_type;
//	typedef typename matrix_traits<MatrixT>::value_type value_type;
//	typedef typename type_traits<value_type>::real_type real_type;

//	vector<real_type> dummy_scaling_vec;
//	vector<size_type> dummy_permuting_vec;
	typename balance_traits<MatrixT>::scaling_vector_type dummy_scaling_vec;
	typename balance_traits<MatrixT>::permuting_vector_type dummy_permuting_vec;

	detail::balance_impl(A,
						 scale,
						 permute,
						 false,
						 dummy_scaling_vec,
						 false,
						 dummy_permuting_vec,
						 true,
						 balancing_mat(),
						 orientation_category());
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 * \tparam VectorExprT The type of the scaling vector.
 *
 * \param A The matrix to be balanced.
 * \param scaling_vec The computed scaling vector.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return none, but matrix \a A and vector \a scaling_vec are overwritten by
 *  the balanced counterpart of \a A and by the computed scaling vector,
 *  respectively.
 */
template <typename MatrixT, typename VectorExprT>
BOOST_UBLAS_INLINE
void balance_inplace(MatrixT& A,
					 vector_container<VectorExprT>& scaling_vec,
					 bool scale = true,
					 bool permute = true)
{
	typedef typename matrix_traits<MatrixT>::orientation_category orientation_category;
//	typedef typename matrix_traits<MatrixT>::size_type size_type;
//	typedef typename matrix_traits<MatrixT>::value_type value_type;

//	vector<size_type> dummy_permuting_vec;
//	matrix<value_type,column_major> dummy_balancing_mat;
	typename balance_traits<MatrixT>::permuting_vector_type dummy_permuting_vec;
	typename balance_traits<MatrixT>::balancing_matrix_type dummy_balancing_mat;

	detail::balance_impl(A,
						 scale,
						 permute,
						 true,
						 scaling_vec(),
						 false,
						 dummy_permuting_vec,
						 false,
						 dummy_balancing_mat,
						 orientation_category());
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 * \tparam SVectorExprT The type of the scaling vector.
 * \tparam PVectorExprT The type of the permuting vector.
 *
 * \param A The matrix to be balanced.
 * \param scaling_vec The computed scaling vector.
 * \param permuting_vec The computed permuting vector.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return none, but matrix \a A and vectors \a scaling_vec and \a permuting_vec
 *  are overwritten by the balanced counterpart of \a A, by the computed
 *  scaling vector, and by the computed permuting vector, respectively.
 */
template <typename MatrixT, typename SVectorExprT, typename PVectorExprT>
BOOST_UBLAS_INLINE
void balance_inplace(MatrixT& A,
					 vector_container<SVectorExprT>& scaling_vec,
					 vector_container<PVectorExprT>& permuting_vec,
					 bool scale = true,
					 bool permute = true)
{
	typedef typename matrix_traits<MatrixT>::orientation_category orientation_category;
//	typedef typename matrix_traits<MatrixT>::value_type value_type;

//	matrix<value_type,column_major> dummy_balancing_mat;
	typename balance_traits<MatrixT>::balancing_matrix_type dummy_balancing_mat;

	detail::balance_impl(A,
						 scale,
						 permute,
						 true,
						 scaling_vec(),
						 true,
						 permuting_vec(),
						 false,
						 dummy_balancing_mat,
						 orientation_category());
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 *
 * \param A The matrix to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return The balanced matrix.
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename balance_traits<MatrixExprT>::balanced_matrix_type balance(matrix_expression<MatrixExprT> const& A,
																   bool scale = true,
																   bool permute = true)
{
	//typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	//typedef typename layout_type<MatrixExprT>::type balanced_layout_type;
	//typedef matrix<value_type,balanced_layout_type> balanced_matrix_type;
	typedef typename balance_traits<MatrixExprT>::balanced_matrix_type balanced_matrix_type;

	balanced_matrix_type X(A);

	balance_inplace(X, scale, permute);

	return X;
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam AMatrixExprT The type of the matrix to be balanced.
 * \tparam BMatrixExprT The type of the balancing matrix.
 *
 * \param A The matrix to be balanced.
 * \param balancing_mat The balancing matrix.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return The balanced matrix; furthermore, the matrix \a balancing_mat is
 *  overwritten by the computed balancing matrix.
 */
template <typename AMatrixExprT, typename BMatrixExprT>
BOOST_UBLAS_INLINE
typename balance_traits<AMatrixExprT>::balanced_matrix_type balance(matrix_expression<AMatrixExprT> const& A,
																	matrix_container<BMatrixExprT>& balancing_mat,
																	bool scale = true,
																	bool permute = true)
{
	//typedef typename matrix_traits<AMatrixExprT>::value_type value_type;
	//typedef typename layout_type<AMatrixExprT>::type balanced_layout_type;
	//typedef matrix<value_type,balanced_layout_type> balanced_matrix_type;
	typedef typename balance_traits<AMatrixExprT>::balanced_matrix_type balanced_matrix_type;

	balanced_matrix_type X(A);

	balance_inplace(X, balancing_mat, scale, permute);

	return X;
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixExprT The type of the matrix to be balanced.
 * \tparam VectorExprT The type of the scaling vector.
 *
 * \param A The matrix to be balanced.
 * \param scaling_vec The scaling vector.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return The balanced matrix; furthermore, the vector \a scaling_vec is
 *  overwritten by the computed scaling vector.
 */
template <typename MatrixExprT, typename VectorExprT>
BOOST_UBLAS_INLINE
typename balance_traits<MatrixExprT>::balanced_matrix_type balance(matrix_expression<MatrixExprT> const& A,
																   vector_container<VectorExprT>& scaling_vec,
																   bool scale = true,
																   bool permute = true)
{
	//typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	//typedef typename layout_type<MatrixExprT>::type balanced_layout_type;
	//typedef matrix<value_type,balanced_layout_type> balanced_matrix_type;
	typedef typename balance_traits<MatrixExprT>::balanced_matrix_type balanced_matrix_type;

	balanced_matrix_type X(A);

	balance_inplace(X, scaling_vec, scale, permute);

	return X;
}

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixExprT The type of the matrix to be balanced.
 * \tparam SVectorExprT The type of the scaling vector.
 * \tparam PVectorExprT The type of the permuting vector.
 *
 * \param A The matrix to be balanced.
 * \param scaling_vec The scaling vector.
 * \param permuting_vec The permuting vector.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return The balanced matrix; furthermore, the vector \a scaling_vec is
 *  overwritten by the computed scaling vector.
 */
template <typename MatrixExprT, typename SVectorExprT, typename PVectorExprT>
BOOST_UBLAS_INLINE
typename balance_traits<MatrixExprT>::balanced_matrix_type balance(matrix_expression<MatrixExprT> const& A,
																   vector_container<SVectorExprT>& scaling_vec,
																   vector_container<PVectorExprT>& permuting_vec,
																   bool scale = true,
																   bool permute = true)
{
	typedef typename balance_traits<MatrixExprT>::balanced_matrix_type balanced_matrix_type;

	balanced_matrix_type X(A);

	balance_inplace(X, scaling_vec, permuting_vec, scale, permute);

	return X;
}

//@} Balance of Single Matrix

//@{ Balance of Matrix Pair

/// Traits type class for the \c balance operation (matrix pencil version).
template <typename Matrix1T, typename Matrix2T>
struct pair_balance_traits
{
	/// The type of the balanced matrix.
	typedef matrix<typename promote_traits<
							typename matrix_traits<Matrix1T>::value_type,
							typename matrix_traits<Matrix2T>::value_type
						>::promote_type,
				   typename layout_type<Matrix1T>::type
				> balanced_matrix_type;
	/// The type of the balancing matrix.
	typedef matrix<typename promote_traits<
							typename matrix_traits<Matrix1T>::value_type,
							typename matrix_traits<Matrix2T>::value_type
						>::promote_type,
				   typename layout_type<Matrix1T>::type
				> balancing_matrix_type;
	/// The type of the scaling vector.
	typedef vector<typename type_traits<
						typename promote_traits<
							typename matrix_traits<Matrix1T>::value_type,
							typename matrix_traits<Matrix2T>::value_type
						>::promote_type
					>::real_type> scaling_vector_type;
	/// The type of the permuting vector.
	typedef vector<typename promote_traits<
							typename matrix_traits<Matrix1T>::size_type,
							typename matrix_traits<Matrix2T>::size_type
						>::promote_type
				> permuting_vector_type;
};

/**
 * \brief Diagonal matrix balancing to improve eigenvalue accuracy.
 *
 * \tparam MatrixT The type of the matrix to be balanced.
 *
 * \param A The matrix to be balanced.
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return none, but matrix \a A is overwritten by its balanced counterpart.
 */
template <typename MatrixExpr1T, typename MatrixExpr2T>
BOOST_UBLAS_INLINE
void balance_inplace(matrix_container<MatrixExpr1T>& A, matrix_container<MatrixExpr2T>& B, bool scale = true, bool permute = true)
{
	typedef typename matrix_traits<MatrixExpr1T>::orientation_category orientation_category;
	typedef typename pair_balance_traits<MatrixExpr1T,MatrixExpr2T>::scaling_vector_type scaling_vector_type;
	typedef typename pair_balance_traits<MatrixExpr1T,MatrixExpr2T>::permuting_vector_type permuting_vector_type;
	typedef typename pair_balance_traits<MatrixExpr1T,MatrixExpr2T>::balancing_matrix_type balancing_matrix_type;

	scaling_vector_type dummy_left_scaling_vec;
	scaling_vector_type dummy_right_scaling_vec;
	permuting_vector_type dummy_left_permuting_vec;
	permuting_vector_type dummy_right_permuting_vec;
	balancing_matrix_type dummy_balancing_mat;

	detail::balance_impl(A,
						 B,
						 scale,
						 permute,
						 false,
						 dummy_left_scaling_vec,
						 dummy_right_scaling_vec,
						 false,
						 dummy_left_permuting_vec,
						 dummy_right_permuting_vec,
						 false,
						 dummy_balancing_mat,
						 orientation_category());
}

/**
 * \brief Diagonal matrix balancing to improve generalized eigenvalue accuracy.
 *
 * \tparam MatrixExpr1T The type of the first matrix of the input pencil to be
 *  balanced.
 * \tparam MatrixExpr2T The type of the second matrix of the input pencil to be
 *  balanced.
 * \tparam SLVectorExprT The type of the scaling vector applied to the left side
 *  of the input pencil.
 * \tparam SRVectorExprT The type of the scaling vector applied to the right
 *  side of the input pencil.
 * \tparam PLVectorExprT The type of the permuting vector applied to the left
 *  size of the input pencil.
 * \tparam PRVectorExprT The type of the permuting vector applied to the right
 *  size of the input pencil.
 *
 * \param A The first matrix of the input pencil (A,B) to be balanced.
 * \param B The second matrix of the input pencil (A,B) to be balanced.
 * \param left_scaling_vec The scaling vector applied to the left side of the
 *  input pencil (A,B).
 * \param right_scaling_vec The scaling vector applied to the right side of the
 *  input pencil (A,B).
 * \param left_permuting_vec The permuting vector applied to the left side of
 *  the input pencil (A,B)..
 * \param right_permuting_vec The permuting vector applied to the right side of
 *  the input pencil (A,B)..
 * \param scale Tells if scaling is to be applied.
 * \param permute Tells if permutation is to be applied.
 * \return The balanced matrix; furthermore, the vectors \a left_scaling_vec,
 *  \a right_scaling_vec, \a left_permuting_vec, and \a right_permuting_vec are
 *  overwritten by the computed scaling vectors and by the permuting scaling
 *  vectors, respectively.
 */
template <
	typename MatrixExpr1T,
	typename MatrixExpr2T,
	typename BMatrixExpr1T,
	typename BMatrixExpr2T,
	typename SLVectorExprT,
	typename SRVectorExprT,
	typename PLVectorExprT,
	typename PRVectorExprT
>
BOOST_UBLAS_INLINE
void balance(matrix_expression<MatrixExpr1T> const& A,
			 matrix_expression<MatrixExpr2T> const& B,
			 matrix_container<BMatrixExpr1T>& BA,
			 matrix_container<BMatrixExpr2T>& BB,
			 vector_container<SLVectorExprT>& left_scaling_vec,
			 vector_container<SRVectorExprT>& right_scaling_vec,
			 vector_container<PLVectorExprT>& left_permuting_vec,
			 vector_container<PRVectorExprT>& right_permuting_vec,
			 bool scale = true,
			 bool permute = true)
{
	typedef typename pair_balance_traits<MatrixExpr1T,MatrixExpr2T>::balanced_matrix_type balanced_matrix_type;

	balanced_matrix_type X(A);
	balanced_matrix_type Y(B);

	balance_inplace(X,
					Y,
					left_scaling_vec,
					right_scaling_vec,
					left_permuting_vec,
					right_permuting_vec,
					scale,
					permute);

	BA() = X;
	BB() = Y;
}

//@} Balance of Matrix Pair

}}} // Namespace boost:numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_BALANCE_HPP


/// \file   tatonnement.hpp
///
/// \brief  Implements the tâtonnement process (hill climbing), implemented as a
///         numerical optimisation (L-BFGS) with
///         automatic differentiation using the Stan-math library.
///
/// \remark This code uses the spelling `tatonnement`, as the accent on `â` can
///         not be rendered in some filesystem character sets.
///
/// \authors    Maarten P. Scholl
/// \date       2018-02-02
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#ifndef PROJECT_TATONNEMENT_HPP
#define PROJECT_TATONNEMENT_HPP

#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <adept.h>

#include <esl/economics/markets/differentiable_demand_supply_function.hpp>
#include <esl/economics/markets/quote.hpp>

namespace tatonnement {
    ///
    /// \brief
    ///
    class excess_demand_model
    {
    public:
        std::vector<std::shared_ptr<differentiable_demand_supply_function>>
            excess_demand_functions_;

        explicit excess_demand_model(
            std::map<esl::identity<esl::law::property>, esl::economics::quote> initial_quotes);

        virtual ~excess_demand_model();

    public://protected:
        std::map<esl::identity<esl::law::property>, esl::economics::quote> quotes_;

        adept::Stack stack_;                    // Adept stack object
        std::vector<adept::adouble> active_x_;  // Active state variables

        adept::adouble calc_function_value(const adept::adouble *x);
        std::vector<adept::adouble> multiroot_function_value(const adept::adouble *x);

#ifndef ADEPT_NO_AUTOMATIC_DIFFERENTIATION
        double         calc_function_value(const double *x);

        std::vector<double> multiroot_function_value(const double *x);
#endif

        double calc_function_value_and_gradient(const double *x, double *dJ_dx) ;

        std::vector<double> multiroot_function_value_and_gradient(const double *x, double *dJ_dx) ;

        //friend double extern "C" my_function_value(const gsl_vector *variables, void *params);
        //friend void extern "C" my_function_gradient(const gsl_vector *x, void *params, gsl_vector *gradJ);
        //friend void extern "C" my_function_value_and_gradient(const gsl_vector *x, void *params, double *J, gsl_vector *gradJ);

    public:
        std::optional<std::map<esl::identity<esl::law::property>, double>> do_compute();
    };  // model
}  // namespace tatonnement

#endif  // PROJECT_TATONNEMENT_HPP


// Std includes
#include <cmath>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "Eigen/src/Core/Matrix.h"
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/spheroid.h"
// Simple includes
#include "flow.h"

// Types
using TypeScalar = double;
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 3;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeSpaceMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<Eigen::Dynamic>, TypeView>;
// Flow
using TypeFlow = Flow<TypeSpaceVector, TypeSpaceMatrix, TypeRef>;

int main () {
    TypeSpaceVector x0;
    x0 << 0,
          0,
          0;
    TypeSpaceVector p0;
    p0 << 1,
          0,
          0;
    double t0 = 0.0;
    double dt = 1e-2;
    double tEnd = 1.0;
    // Create ellipsoid
    sl0::Spheroid<TypeVector, DIM, TypeView, TypeFlow, TypeSolver> spheroid(std::make_shared<TypeFlow>(), 1.0);
    // Set initial state
    spheroid.sStep->x(spheroid.state.data()) = x0;
    spheroid.sStep->axis(spheroid.state.data()) = p0;
    spheroid.t = t0;
    // Compute
    std::cout << "Spheroid in a simple shear flow : \n";
    std::cout << "\n";
    std::cout << "Orientation after each step : \n";
    for(std::size_t i = 0; i < (tEnd - t0)/dt; i++) {
        spheroid.update(dt);
        std::cout << spheroid.sStep->axis(spheroid.state.data()) << "\n";
    }
    // out
    std::cout << "Spheroid in a simple shear flow : \n";
    std::cout << "\n";
    std::cout << "Final orientation : \n";
    std::cout << spheroid.sStep->axis(spheroid.state.data()) << "\n";
    std::cout << std::endl;
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_Lstm_hpp
#define poplibs_test_Lstm_hpp

#include <boost/multi_array.hpp>
#include <popnn/LstmDef.hpp>

namespace poplibs_test {
namespace lstm {

// Defines for state information in forward pass
#define LSTM_NUM_FWD_STATES 7
#define LSTM_FWD_STATE_ACTS_IDX 0
#define LSTM_FWD_STATE_CELL_STATE_IDX 1

// Defines for state information in backward pass
#define LSTM_NUM_BWD_STATES BASIC_LSTM_CELL_NUM_UNITS

/**
 * Compute the cell state and output of a basic non-fused LSTM cell (without
 * peephole connections). The cell state and outputs are concatented into a
 * single dimension.
 *
 * \param input               Input to the LSTM cell of dimension
 *                            [sequenceSize][batchSize][inputSize]
 * \param weightsInput        Weights in the LSTM cell which weigh the input
 *                            sequence. It is of dimension
 *                            [NUM_LSTM_UNITS][inputSize][outputSize]
 * \param weightsOutput       Weights in the LSTM cell which weigh the output
 *                            sequence. It is of dimension
 *                            [NUM_LSTM_UNITS][outputSize][outputSize]
 * \param biases              Biases in the LSTM cell of dimension
 *                            [NUM_LSTM_UNITS][outputSize]
 * \param prevOutput          Previous output used in the first time step
 *                            [batchSize][outputSize]
 * \param prevCellState       Initial cell state of shape
 *                            [batchSize][outputSize]
 * \param state               The forward state for all the sequence steps of
 *                            dimension
 *                            [LSTM_NUM_FWD_STATES][sequenceSize][batchSize]
 *                            [outputSize]
 * \param cellOrder           The order that the weights for each gate are
 *                            stored in the input.
 */
void basicLstmCellForwardPass(
    const boost::multi_array_ref<double, 3> input,
    const boost::multi_array_ref<double, 2> biases,
    const boost::multi_array_ref<double, 2> prevOutput,
    const boost::multi_array_ref<double, 3> weightsInput,
    const boost::multi_array_ref<double, 3> weightsOutput,
    boost::multi_array_ref<double, 2> prevCellState,
    boost::multi_array_ref<double, 4> state,
    const std::vector<BasicLstmCellUnit> &cellOrder);

/** Run backward pass given forward sequence
 *
 * \param weightsInput    Input weights
 *                        shape: [NUM_LSTM_UNITS][input ch][output ch]
 * \param weightsOutput   Output weights
 *                        shape: [NUM_LSTM_UNITS][output ch][output ch]
 * \param gradsNextLayer  Gradients from next layer needed to compute gradients
 *                        for this layer. shape: [sequence][batch][output ch]
 * \param prevCellState   Cell state of the initial step in the forward pass
 *                        shape: [batch][output ch]
 * \param fwdState        Forward state returned by \see
 *                        basicLstmCellForwardPass.
 *                        shape: [LSTM_NUM_FWD_STATES][sequence][batch]
 *                               [output ch]
 * \param bwdState        Backward state returned by this function
 *                        shape:[LSTM_NUM_BWD_STATES][sequence]
 *                              [batch][output ch]
 * \param gradsPrevLayer  Gradients for previous layer computed by this function
 *                        shape: [sequence][batch][input ch]
 * \param cellOrder       The order that the weights for each gate are
 *                        stored in the input.
 */
void basicLstmCellBackwardPass(
    const boost::multi_array_ref<double, 3> weightsInput,
    const boost::multi_array_ref<double, 3> weightsOutput,
    const boost::multi_array_ref<double, 3> gradsNextLayer,
    const boost::multi_array_ref<double, 2> prevCellState,
    const boost::multi_array_ref<double, 4> fwdState,
    boost::multi_array_ref<double, 4> bwdState,
    boost::multi_array_ref<double, 3> gradsPrevLayer,
    const std::vector<BasicLstmCellUnit> &cellOrder);

/** Param update
 *
 * \param prevLayerActs   Activations from previous layer
 *                        shape: [sequence][batch][input ch]
 * \param fwdState        Forward state compute by \see basicLstmCellForwardPass
 *                        shape: [LSTM_NUM_FWD_STATES][sequence][batch][output]
 * \param outputActsInit  Initial activations for the forward pass
 *                        shape: [batch][output ch]
 * \param bwdState        Backward channel state generated by backward pass
 *                        shape" [LSTM_NUM_BWD_STATES][sequence][batch][output]
 * \param weightsInputDeltas  Weight deltas computed by this function
 *                            shape: [NUM_LSTM_UNITS][input ch][output ch]
 * \param weightsOutputDeltas Weight deltas computed by this function
 *                            shape: [NUM_LSTM_UNITS][output ch][output ch]
 *
 * \param biasDeltas      Bias deltas computed by this function
 *                        shape: [NUM_LSTM_UNITS][output ch]
 * \param cellOrder       The order that the weights for each gate are
 *                        stored in the input.
 */
void basicLstmCellParamUpdate(
    const boost::multi_array_ref<double, 3> prevLayerActs,
    const boost::multi_array_ref<double, 4> fwdState,
    const boost::multi_array_ref<double, 2> outputActsInit,
    const boost::multi_array_ref<double, 4> bwdState,
    boost::multi_array_ref<double, 3> weightsInputDeltas,
    boost::multi_array_ref<double, 3> weightsOutputDeltas,
    boost::multi_array_ref<double, 2> biasDeltas,
    const std::vector<BasicLstmCellUnit> &cellOrder);
} // namespace lstm
} // namespace poplibs_test

#endif // poplibs_test_Lstm_hpp


#include "tbb/parallel_for.h"
#include <iostream>
#include <mex.h>
#include <omp.h>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
// #include <Eigen/StdVector>

// #include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

// #include "storage_aliases.h"

// #define ngp int(*mxGetPr(prhs[0]))
#define input prhs[0]
#define output plhs[0]

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  if (!(mxIsStruct(prhs[0]) && mxGetNumberOfFields(prhs[0]) > 0 && (nlhs > 0) &&
        (nrhs > 0))) {
    mexErrMsgIdAndTxt("MATLAB:mexcpp:typeargin",
                      "First argument has to be double scalar.");
    return;
  }

  auto eps_p_bar = mxGetField(prhs[0], 0, "eps_p_bar");
  auto sigma_bar = mxGetField(prhs[0], 0, "sigma_bar");
  auto Hdo = mxGetField(prhs[0], 0, "Hdo");
  auto inv_Hdo = mxGetField(prhs[0], 0, "iHdo");
  auto delta_in = mxGetField(prhs[0], 0, "delta");
  // auto DG_proj = mxGetField(prhs[0], 0, "DG_proj");
  // auto D_operator = mxGetField(prhs[0], 0, "D_operator");
  // auto ntp_in = mxGetField(prhs[0], 0, "ntp");
  auto dt = mxGetField(prhs[0], 0, "dt");
  auto I_g = mxGetField(prhs[0], 0, "I_g");
  auto sgn = mxGetField(prhs[0], 0, "sign");
  auto sign = int(*mxGetPr(sgn));
  auto d_t = double(*mxGetPr(dt));

  auto Hdo_Dimensions = mxGetDimensions(Hdo);
  auto sigma_Dimensions = mxGetDimensions(sigma_bar);
  size_t const ncomp = 6; // TODO 3D only
  if (Hdo_Dimensions[0] != 6)
    mexErrMsgIdAndTxt("MATLAB:mexcpp:nargin", "3D only, ncomp should equal 6.");
  size_t const ngp = Hdo_Dimensions[2];
  size_t const ntp = Hdo_Dimensions[3];
  size_t const nmodes = sigma_Dimensions[1];

  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;
  using matrix6 = Eigen::Matrix<double, 6, 6>;

  // Eigen::Map<Vector> Hdo_v(mxGetPr(Hdo), ntp * ngp * ncomp * ncomp);
  // Eigen::Map<Vector> inv_Hdo_v(mxGetPr(inv_Hdo), ntp * ngp * ncomp * ncomp);
  // Eigen::Map<Vector> delta_v(mxGetPr(delta_in), ntp * ngp * ncomp);
  // Eigen::Map<Vector> sigma_v(mxGetPr(sigma_bar), ngp * ncomp * nmodes);
  // Eigen::Map<Vector> eps_p_v(mxGetPr(eps_p_bar), ngp * ncomp * nmodes);

  // clang-format off
  // time_matrix6_storage H(ntp, gauss_matrix6_storage(ngp, matrix6_storage()));
  // time_matrix6_storage iH(ntp, gauss_matrix6_storage(ngp, matrix6_storage()));
  // time_vector_storage delta(ntp, gauss_vector_storage(ngp, vector_storage(ncomp)));
  // gauss_matrix_storage sigma(ngp, matrix_storage(ncomp, nmodes));
  // gauss_matrix_storage eps_p(ngp, matrix_storage(ncomp, nmodes));
  // clang-format on

  auto const tensor_shift = ncomp * ncomp;
  auto const vector_shift = ncomp * nmodes;
  auto const delta_shift = ncomp;

  // using sparse_matrix = Eigen::SparseMatrix<double, Eigen::RowMajor>;
  // sparse_matrix A(nmodes * ntp, nmodes * ntp);
  // A.reserve(2 * nmodes * nmodes * ntp - (2 * nmodes - 1));
  // A.coeffs() = 0.0;
  Matrix A = Matrix::Zero(nmodes * ntp, nmodes * ntp);
  Vector b = Vector::Zero(ntp * nmodes);

  auto const nel = ntp - 1;

  // TODO
  // std::vector<std::int32_t> dofs(nel);
  // std::generate(begin(dofs), end(dofs), [j = -nmodes] () mutable {
  //      j += nmodes;
  //      return j;
  // });

  std::vector<std::int32_t> dofs;
  for (size_t j = 0; j < nel * nmodes; j += nmodes)
    dofs.push_back(j);

  std::vector<std::int32_t> fixed_dofs;
  for (size_t j = 0; j < nmodes; j++)
    fixed_dofs.push_back(j);

    // how come I don't have race condition here? TODO
    // clang-format off
// #pragma omp parallel for collapse(2)
#pragma omp declare reduction(+ : Eigen::MatrixXd : omp_out =  omp_out + omp_in)
#pragma omp declare reduction(+ : Eigen::VectorXd : omp_out =  omp_out + omp_in)

#pragma omp parallel for
  for (size_t i = 0; i < ntp; i++) {
    Matrix a11 = Matrix::Zero(nmodes, nmodes);
    Matrix a10 = Matrix::Zero(nmodes, nmodes);
    Matrix a00 = Matrix::Zero(nmodes, nmodes);
    Vector d1 = Vector::Zero(nmodes);
    Vector d0 = Vector::Zero(nmodes);

// #pragma omp parallel for reduction(+ : a11,a10,a00,d1,d0)
// #pragma omp parallel for
    for (size_t j = 0; j < ngp; j++) {
      int const int_idx = j * tensor_shift;
      int const tensor_idx = (i * ngp + j) * tensor_shift;
      int const delta_idx = (i * ngp + j) * delta_shift;
      int const vector_idx = j * vector_shift;

      //Hdo_v.segment(tensor_idx, tensor_shift);
      // Eigen::Map<matrix6> Int_space(mxGetPr(I_g));
      matrix6 Int_space = Eigen::Map<matrix6>(mxGetPr(I_g) + int_idx, ncomp, ncomp);
      matrix6 H = Eigen::Map<matrix6>(mxGetPr(Hdo) + tensor_idx, ncomp, ncomp);
      matrix6 iH = Eigen::Map<matrix6>(mxGetPr(inv_Hdo) + tensor_idx);
      Vector delta = Eigen::Map<Vector>(mxGetPr(delta_in) + delta_idx, ncomp);
      Matrix sigma = Eigen::Map<Matrix>(mxGetPr(sigma_bar) + vector_idx, ncomp,nmodes);
      Matrix eps_p = Eigen::Map<Matrix>(mxGetPr(eps_p_bar) + vector_idx, ncomp,nmodes);

      a11.noalias() += eps_p.transpose() * (iH * (Int_space * eps_p)); // TODO noalias
      a10 += -1.0 * sign * eps_p.transpose() * Int_space * sigma;
      a00 += sigma.transpose() * (H * (Int_space * sigma)); // TODO I_g is the same for all elements? shouldn't be
      d1 += -1.0 * sign * eps_p.transpose() * (iH * (Int_space * delta));
      d0 += sigma.transpose() * Int_space * delta;
    }

    Matrix t11 = a11 / (d_t * d_t); // TODO use cwiseQuotient
    Matrix t12 = a10 / (d_t);       //   a01 = a10;
    Matrix t22 = t11 + t12 + t12 + a00;
    t12 = -t12 - t11;
    Vector b0 = -d1 / (d_t);
    Vector b1 = d1 / (d_t) + d0;

    if (i>0) { //for (size_t i = 0; i < nel; i++) {
      Matrix a_el(2 * nmodes, 2 * nmodes);
      Vector b_el(2 * nmodes);

      a_el << t11, t12,
              t12, t22;

      b_el << b0,
              b1; // TODO use I_time

//       for (size_t j = 0; j < 2 * nmodes; j++) {
//         for (size_t k = 0; k < 2 * nmodes; k++) {
// #pragma omp critical
//           A.coeffRef(dofs[i-1]+j, dofs[i-1]+k) += a_el(j,k);
//         }
//       }
//       for (size_t j = 0; j < 2 * nmodes; j++) {
// #pragma omp atomic
//           b.coeffRef(dofs[i-1]+j) += b_el(j);
//       }

#pragma omp critical
{
      A.block(dofs[i-1], dofs[i-1], 2 * nmodes, 2 * nmodes) += a_el; // TODO this assembly is not final
      b.segment(dofs[i-1], 2 * nmodes) += b_el;
}
    }
  }
  // clang-format on

  for (auto const fixed_dof : fixed_dofs) {
    auto const diagonal_entry = A.coeff(fixed_dof, fixed_dof); // TODO // sparse

    b(fixed_dof) = 0.0;

    std::vector<std::int64_t> non_zero_visitor;

    // // Zero the rows and columns
    // for (sparse_matrix::InnerIterator it(A, fixed_dof); it; ++it) {
    //   // Set the value of the col or row resp. to zero
    //   it.valueRef() = 0.0;
    //   non_zero_visitor.push_back(A.IsRowMajor ? it.col() : it.row());
    // }
    // // Zero the row or col respectively
    // for (auto const &non_zero : non_zero_visitor) {
    //   const auto row = A.IsRowMajor ? non_zero : fixed_dof;
    //   const auto col = A.IsRowMajor ? fixed_dof : non_zero;
    //
    //   A.coeffRef(row, col) = 0.0;
    // }
    for (int i = 0; i < A.rows(); i++) {
      for (int j = 0; j < A.cols(); j++) {
        A.coeffRef(fixed_dof, j) = 0.0;
        A.coeffRef(i, fixed_dof) = 0.0;
      }
    }
    // Reset the diagonal to the same value to preserve conditioning
    A.coeffRef(fixed_dof, fixed_dof) = diagonal_entry;
  }

  // TODO //Vector x = (b.norm() > 0.0) ? A.partialPivLu().solve(b)

  Vector x;
  if (b.norm() > 0)
    x = A.partialPivLu().solve(b); // fullPivLu
  else {
    x = b;
  }
  // Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> chol(
  //     A); // performs a Cholesky factorization of A
  // Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> chol(
  //     A); // performs a Cholesky factorization of A
  // Eigen::VectorXd x = chol.solve(
  //     b); // use the factorization to solve for the given right hand side

  output = mxCreateDoubleMatrix(nmodes, ntp, mxREAL);
  auto out_pt = mxGetPr(output);
  Eigen::Map<Eigen::MatrixXd> mapp(out_pt, nmodes, ntp);
  mapp = x; // copy
  // std::copy(x.begin(), x.end(), mxGetPr(output));
  return;
}

// mxArray *cpp_to_MexArray(const std::vector<double> &v) {
//   mxArray *mx = mxCreateDoubleMatrix(1, v.size(), mxREAL);
//   std::copy(v.begin(), v.end(), mxGetPr(mx));
//
//   return mx;
// }


#include <LBFGSB.h>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include <MathUtil.h>
#include <fstream>
#include <iomanip>

#include <nlopt.hpp>
#include <Pluckertree.h>
#include <PluckertreeSegments.h>

using namespace LBFGSpp;

using Vector = Eigen::VectorXd;

class FixedMomentMinDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t h1;
    Vector3_t h2;
public:
    FixedMomentMinDist(Vector3_t q, Vector3_t dirLowerBound, Vector3_t dirUpperBound)
            : q(std::move(q)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)) {}

    double operator()(const Vector& x, Vector& grad)
     {
        // Careful, don't disturb the big pile of math!
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k / r_k;
        Vector3_t qp = q - kd*(q.dot(kd));
        Vector3_t qpd = qp.normalized();
        auto u = (qp - q).norm();

        auto sin_gamma = std::min(r_k/qp.norm(), (number_t)1.0f);
        auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);

        Vector3_t da = qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
        Vector3_t d_alpha;
        auto da_dot_h1 = da.dot(h1);
        auto da_dot_h2 = da.dot(h2);
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            d_alpha = da;
        }else if(da_dot_h1 <= da_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        Vector3_t db = - qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
        Vector3_t d_beta;
        auto db_dot_h1 = db.dot(h1);
        auto db_dot_h2 = db.dot(h2);
        if(db_dot_h1 >= 0 && db_dot_h2 >= 0)
        {
            d_beta = db;
        }else if(db_dot_h1 < db_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_beta = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_beta = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        auto va = (qp.cross(d_alpha) - k).norm();
        auto vb = (qp.cross(d_beta) - k).norm();
        auto v = std::min(va, vb);

        auto f = std::sqrt(u*u + v*v);

        // partial
        /*auto sin_theta_k = std::sin(theta_k);
        auto pow2_sin_theta_k = sin_theta_k * sin_theta_k;
        auto sin_2theta_k = std::sin(2*theta_k);

        auto cos_theta_k = std::cos(theta_k);
        auto cos_2theta_k = std::cos(2*theta_k);
        auto pow2_cos_theta_k = cos_theta_k * cos_theta_k;

        auto sin_phi_k = std::sin(phi_k);
        auto pow2_sin_phi_k = sin_phi_k * sin_phi_k;
        auto sin_2phi_k = std::sin(2*phi_k);

        auto cos_phi_k = std::cos(phi_k);
        auto cos_2phi_k = std::cos(2*phi_k);
        auto pow2_cos_phi_k = cos_phi_k * cos_phi_k;

        ///
        auto partial_k_phi_k = r_k * Vector3_t(sin_theta_k * (-sin_phi_k), sin_theta_k * cos_phi_k, cos_theta_k);
        auto partial_k_theta_k = r_k * Vector3_t(cos_theta_k * cos_phi_k, cos_theta_k * sin_phi_k, (-sin_theta_k));
        auto partial_k_r_k = Vector3_t(sin_theta_k * cos_phi_k, sin_theta_k * sin_phi_k, cos_theta_k);

        Matrix3_t partial_k;
        partial_k.row(0) = partial_k_phi_k;
        partial_k.row(1) = partial_k_theta_k;
        partial_k.row(2) = partial_k_r_k;
        ///

        auto k_norm = k.norm();
        Matrix3_t partial_kd;
        for(int i = 0; i < 3; ++i)
        {
            partial_kd.row(i) = (partial_k.row(i)/k_norm) - k.transpose() * ((-1/(std::pow(k_norm, 3))) * (k.x() * partial_k.row(i).x() + k.y() * partial_k.row(i).y() + k.z() * partial_k.row(i).z()));
        }

        ///
        Matrix3_t partial_qp;
        for(int i = 0; i < 3; ++i)
        {
            partial_qp.row(i).x() = q.x() * 2 * kd.x() * partial_kd.row(i).x();
            partial_qp.row(i).x() += q.y() * (partial_kd.row(i).x() * kd.y() + kd.x() * partial_kd.row(i).y());
            partial_qp.row(i).x() += q.z() * (partial_kd.row(i).x() * kd.z() + kd.x() * partial_kd.row(i).z());

            partial_qp.row(i).y() = q.x() * (partial_kd.row(i).y() * kd.x() + kd.y() * partial_kd.row(i).x());
            partial_qp.row(i).y() += q.y() * 2 * kd.y() * partial_kd.row(i).y();
            partial_qp.row(i).y() += q.z() * (partial_kd.row(i).y() * kd.z() + kd.y() * partial_kd.row(i).z());

            partial_qp.row(i).z() = q.x() * (partial_kd.row(i).z() * kd.x() + kd.z() * partial_kd.row(i).x());
            partial_qp.row(i).z() += q.y() * (partial_kd.row(i).z() * kd.y() + kd.z() * partial_kd.row(i).y());
            partial_qp.row(i).z() += q.z() * 2 * kd.z() * partial_kd.row(i).z();

            partial_qp.row(i) *= -1;
        }
        ///

        //auto partial_pow2_u_phi_k = 2*(qp.x() - q.x()) * partial_qp_phi_k.x() + 2*(qp.y() - q.y()) * partial_qp_phi_k.y() + 2*(qp.z() - q.z()) * partial_qp_phi_k.z();
        auto partial_pow2_u = 2 * (partial_qp * (qp - q));

        auto da_xy_norm = Eigen::Vector2f(da.x(), da.y()).norm();

        auto qp_norm = qp.norm();
        //double partial_delta_phi_k = (1/qp_norm) * (qp.x() * partial_qp_phi_k.x() + qp.y() * partial_qp_phi_k.y() + qp.z() * partial_qp_phi_k.z());
        Vector3_t partial_delta;
        for(int i = 0; i < 3; i++)
        {
            partial_delta[i] = (1/qp_norm) * (qp.x() * partial_qp.row(i).x() + qp.y() * partial_qp.row(i).y() + qp.z() * partial_qp.row(i).z());
        }

        Matrix3_t partial_qpd;
        for(int i = 0; i < 3; i++)
        {
            partial_qpd.row(i) = (partial_qp.row(i)/qp_norm) + qp.transpose() * (-1/qp.squaredNorm()) * partial_delta[i];
        }

        Vector3_t partial_sin_gamma(0, 0, 0);
        //double partial_sin_gamma_phi_k = 1;
        if(r_k/qp_norm > 1)
        {
            for(int i = 0; i < 3; i++)
            {
                partial_sin_gamma[i] = -(r_k/qp.squaredNorm()) * partial_delta[i]; //TODO: is this wrong for i=2?
                //partial_sin_gamma_phi_k = -(r_k/qp.squaredNorm()) * partial_delta_phi_k;
            }
        }

        Vector3_t partial_cos_gamma;
        for(int i = 0; i < 3; i++)
        {
            //TODO
            partial_cos_gamma[i] = (1/std::sqrt(1-sin_gamma*sin_gamma)) * (-sin_gamma * partial_sin_gamma[i]);
        }

        Matrix3_t partial_da = Matrix3_t::Zero();
        float sign = 1.0f;
        if(va >= vb)
        {
            sign = -1.0f; //partial_db
        }

        for(int i = 0; i < 3; i++)
        {
            partial_da.row(i).x() += sign * partial_qpd.row(i).x() * cos_gamma + qpd.x() * partial_cos_gamma[i];
            partial_da.row(i).y() += sign * partial_qpd.row(i).y() * cos_gamma + qpd.x() * partial_cos_gamma[i];
            partial_da.row(i).z() += sign * partial_qpd.row(i).z() * cos_gamma + qpd.z() * partial_cos_gamma[i];

            partial_da.row(i).x() += partial_kd.row(i).y() * qpd.z() * sin_gamma + kd.y() * partial_qpd.row(i).z() * sin_gamma + kd.y() * qpd.z() * partial_sin_gamma[i];
            partial_da.row(i).y() += partial_kd.row(i).x() * qpd.z() * sin_gamma + kd.x() * partial_qpd.row(i).z() * sin_gamma + kd.x() * qpd.z() * partial_sin_gamma[i];
            partial_da.row(i).z() += partial_kd.row(i).x() * qpd.y() * sin_gamma + kd.x() * partial_qpd.row(i).y() * sin_gamma + kd.x() * qpd.y() * partial_sin_gamma[i];

            partial_da.row(i).x() += partial_kd.row(i).z() * qpd.y() * sin_gamma + kd.z() * partial_qpd.row(i).y() * sin_gamma + kd.z() * qpd.y() * partial_sin_gamma[i];
            partial_da.row(i).y() += partial_kd.row(i).z() * qpd.x() * sin_gamma + kd.z() * partial_qpd.row(i).x() * sin_gamma + kd.z() * qpd.x() * partial_sin_gamma[i];
            partial_da.row(i).z() += partial_kd.row(i).y() * qpd.x() * sin_gamma + kd.y() * partial_qpd.row(i).x() * sin_gamma + kd.y() * qpd.x() * partial_sin_gamma[i];
        }

        Matrix3_t partial_d_alpha; //also partial_d_beta, depending on va >= vb
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            partial_d_alpha = partial_da;
        }else if(da_dot_h1 < da_dot_h2 || (da_dot_h1 == da_dot_h2 && va < vb))
        {
            for(int i = 0; i < 3; i++)
            {
                auto h1_cross_k_sqr_norm = h1.cross(k).squaredNorm();
                auto h1_cross_k_norm = std::sqrt(h1_cross_k_sqr_norm);

                auto partial_h1_cross_k_norm = (h1.y()*k.z() - h1.z()*k.y())*(h1.y()*partial_k.row(i).z() - h1.z()*partial_k.row(i).y());
                partial_h1_cross_k_norm += (h1.x()*k.z() - h1.z()*k.x())*(h1.x()*partial_k.row(i).z() - h1.z()*partial_k.row(i).x());
                partial_h1_cross_k_norm += (h1.x()*k.y() - h1.y()*k.x())*(h1.x()*partial_k.row(i).y() - h1.y()*partial_k.row(i).x());
                partial_h1_cross_k_norm *= (1.0/h1_cross_k_norm);

                auto fact = std::copysign(1.0, k.dot(h2.cross(h1))) / h1_cross_k_sqr_norm;

                partial_d_alpha.row(i).x() = h1_cross_k_norm * (h1.y() * partial_k.row(i).z() - h1.z() * partial_k.row(i).y());
                partial_d_alpha.row(i).x() -= partial_h1_cross_k_norm * (h1.y() * k.z() - h1.z() * k.y());

                partial_d_alpha.row(i).y() = h1_cross_k_norm * (h1.x() * partial_k.row(i).z() - h1.z() * partial_k.row(i).x());
                partial_d_alpha.row(i).y() -= partial_h1_cross_k_norm * (h1.x() * k.z() - h1.z() * k.x());

                partial_d_alpha.row(i).z() = h1_cross_k_norm * (h1.x() * partial_k.row(i).y() - h1.y() * partial_k.row(i).x());
                partial_d_alpha.row(i).z() -= partial_h1_cross_k_norm * (h1.x() * k.y() - h1.y() * k.x());

                partial_d_alpha.row(i) *= fact;
            }
        }else
        {
            for(int i = 0; i < 3; i++)
            {
                auto h2_cross_k_sqr_norm = h2.cross(k).squaredNorm();
                auto h2_cross_k_norm = std::sqrt(h2_cross_k_sqr_norm);

                auto partial_h2_cross_k_norm = (h1.y()*k.z() - h2.z()*k.y())*(h2.y()*partial_k.row(i).z() - h2.z()*partial_k.row(i).y());
                partial_h2_cross_k_norm += (h2.x()*k.z() - h2.z()*k.x())*(h2.x()*partial_k.row(i).z() - h2.z()*partial_k.row(i).x());
                partial_h2_cross_k_norm += (h2.x()*k.y() - h2.y()*k.x())*(h2.x()*partial_k.row(i).y() - h2.y()*partial_k.row(i).x());
                partial_h2_cross_k_norm *= (1.0/h2_cross_k_norm);

                auto fact = std::copysign(1.0, k.dot(h2.cross(h1))) / h2_cross_k_sqr_norm;

                partial_d_alpha.row(i).x() = h2_cross_k_norm * (h2.y() * partial_k.row(i).z() - h2.z() * partial_k.row(i).y());
                partial_d_alpha.row(i).x() -= partial_h2_cross_k_norm * (h2.y() * k.z() - h2.z() * k.y());

                partial_d_alpha.row(i).y() = h2_cross_k_norm * (h2.x() * partial_k.row(i).z() - h2.z() * partial_k.row(i).x());
                partial_d_alpha.row(i).y() -= partial_h2_cross_k_norm * (h2.x() * k.z() - h2.z() * k.x());

                partial_d_alpha.row(i).z() = h2_cross_k_norm * (h2.x() * partial_k.row(i).y() - h2.y() * partial_k.row(i).x());
                partial_d_alpha.row(i).z() -= partial_h2_cross_k_norm * (h2.x() * k.y() - h2.y() * k.x());

                partial_d_alpha.row(i) *= fact;
            }
        }

        ////

        auto c_alpha = qp.cross(da);
        Matrix3_t partial_c_alpha;
        for(int i = 0; i < 3; i++)
        {
            partial_c_alpha.row(i) = Vector3_t(
                    (partial_qp.row(i).y()*d_alpha.z() + qp.y() * partial_d_alpha.row(i).z()) - (partial_qp.row(i).z()*d_alpha.y() + qp.z()*partial_d_alpha.row(i).y()),
                    (partial_qp.row(i).x()*d_alpha.z() + qp.x() * partial_d_alpha.row(i).z()) - (partial_qp.row(i).z()*d_alpha.x() + qp.z()*partial_d_alpha.row(i).x()),
                    (partial_qp.row(i).x()*d_alpha.y() + qp.x() * partial_d_alpha.row(i).y()) - (partial_qp.row(i).y()*d_alpha.x() + qp.y()*partial_d_alpha.row(i).x())
            );
        }

        Vector3_t partial_v;
        for(int i = 0; i < 3; i++)
        {
            partial_v[i] = (1.0/va) * (
                    (c_alpha.x() - k.x()) * (partial_c_alpha.row(i).x() - partial_k.row(i).x()) +
                    (c_alpha.y() - k.y()) * (partial_c_alpha.row(i).y() - partial_k.row(i).y()) +
                    (c_alpha.z() - k.z()) * (partial_c_alpha.row(i).z() - partial_k.row(i).z())
            );
        }

        Vector3_t partial_pow2_v = 2*va*partial_v;

        grad = (1.0/(2.0*std::sqrt(u*u + v*v))) * (partial_pow2_u + partial_pow2_v);
        /*assert(!grad.hasNaN());
        assert(!std::isinf(grad[0]));
        assert(!std::isinf(grad[1]));
        assert(!std::isinf(grad[2]));*/

        return f;
    }
};

class FixedMomentMinHitDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t q_n;
    Vector3_t h1;
    Vector3_t h2;
public:
    FixedMomentMinHitDist(Vector3_t q, Vector3_t q_n, Vector3_t dirLowerBound, Vector3_t dirUpperBound)
            : q(std::move(q)), q_n(std::move(q_n)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)) {}

    double operator()(const Vector& x, Vector& grad)
    {
        // Careful, don't disturb the big pile of math!
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k.normalized();

        //Find intersection line of directionvector plane with query plane
        Vector3_t kd_cross_qn = kd.cross(q_n);
        auto kd_cross_qn_norm = kd_cross_qn.norm();
        if(kd_cross_qn_norm < 1e-6) // All lines are parallel to the query plane, assume all miss.
        {
            return std::numeric_limits<double>::infinity();
        }
        Vector3_t isect_d = kd_cross_qn / kd_cross_qn_norm;
        Vector3_t isect_k = kd * (q_n.dot(q)/kd_cross_qn_norm);
        Vector3_t isect_p = isect_d.cross(isect_k);
        Vector3_t qpl = isect_p + isect_d * isect_d.dot(q - isect_p);
        Vector3_t qpld = qpl.normalized();

        auto sin_gamma = std::min(r_k/qpl.norm(), (number_t)1.0f);
        auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);

        Vector3_t da = qpld * cos_gamma + kd.cross(qpld) * sin_gamma;
        Vector3_t d_alpha;
        auto da_dot_h1 = da.dot(h1);
        auto da_dot_h2 = da.dot(h2);
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            d_alpha = da;
        }else if(da_dot_h1 <= da_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        Vector3_t db = - qpld * cos_gamma + kd.cross(qpld) * sin_gamma;
        Vector3_t d_beta;
        auto db_dot_h1 = db.dot(h1);
        auto db_dot_h2 = db.dot(h2);
        if(db_dot_h1 >= 0 && db_dot_h2 >= 0)
        {
            d_beta = db;
        }else if(db_dot_h1 < db_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_beta = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_beta = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        //Calculate intersection point of isect line with (d;k)
        number_t va = std::numeric_limits<double>::infinity();
        if(1 - std::abs(d_alpha.dot(isect_d)) > 1e-3)
        {
            Vector3_t isect_d_cross_d_alpha = isect_d.cross(d_alpha);
            Vector3_t vp_a = (1.0/isect_d.cross(d_alpha).squaredNorm()) * (isect_d * (k.dot(isect_d_cross_d_alpha)) - d_alpha * (isect_k.dot(isect_d_cross_d_alpha)));
            va = (q - vp_a).norm();
        }

        number_t vb = std::numeric_limits<double>::infinity();
        if(1 - std::abs(d_beta.dot(isect_d)) > 1e-3)
        {
            Vector3_t isect_d_cross_d_beta = isect_d.cross(d_beta);
            Vector3_t vp_b = (1.0/isect_d.cross(d_beta).squaredNorm()) * (isect_d * (k.dot(isect_d_cross_d_beta)) - d_beta * (isect_k.dot(isect_d_cross_d_beta)));
            vb = (q - vp_b).norm();
        }

        auto v = std::min(va, vb);
        if(std::isinf(v))
        {
            return std::numeric_limits<double>::infinity();
        }

        return v;
    }
};

class FixedMomentMinSegmentDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t h1;
    Vector3_t h2;
    float t1;
    float t2;
public:
    FixedMomentMinSegmentDist(Vector3_t q, Vector3_t dirLowerBound, Vector3_t dirUpperBound, float t1, float t2)
            : q(std::move(q)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)), t1(t1), t2(t2) {}

    double operator()(const Vector& x, Vector& grad)
    {
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k / r_k;
        Vector3_t qp = q - kd*(q.dot(kd));
        auto qp_norm = qp.norm();
        Vector3_t qpd = qp / qp_norm;
        auto u = (qp - q).norm();

        auto calc_dist_for_gamma = [](const Vector3_t& qp, const Vector3_t& qpd, const Vector3_t& k, const Vector3_t& kd,
                const Vector3_t& h1, const Vector3_t& h2, number_t orientation, number_t sin_gamma, number_t t1, number_t t2)
        {
            auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);
            Vector3_t da = orientation * qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
            Vector3_t d_alpha;
            auto da_dot_h1 = da.dot(h1);
            auto da_dot_h2 = da.dot(h2);
            if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
            {
                d_alpha = da;
            }else if(da_dot_h1 <= da_dot_h2)
            {
                Vector h1_cross_k = h1.cross(k);
                d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
            }else
            {
                Vector h2_cross_k = h2.cross(k);
                d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
            }

            //auto v = (qp.cross(d_alpha) - k).norm();
            Vector3_t p = d_alpha.cross(k);
            auto t = std::min(std::max(d_alpha.dot(qp), t1), t2);
            auto v = ((p + t*d_alpha) - qp).norm();
            return v;
        };

        auto t1_k_hypo_sqr = t1*t1 + r_k*r_k;
        auto t2_k_hypo_sqr = t2*t2 + r_k*r_k;
        number_t v;
        if(t1 >= 0 && t2 >= 0)
        {
            number_t sin_gamma;
            if(qp_norm > t2_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else if(qp_norm < t1_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            v = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, 1, sin_gamma, t1, t2);
        }
        else if(t1 <= 0 && t2 <= 0)
        {
            number_t sin_gamma;
            if(qp_norm > t1_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else if(qp_norm < t2_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            v = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, -1, sin_gamma, t1, t2);
        }
        else
        {
            number_t sin_gamma_a;
            if(qp_norm > t2_k_hypo_sqr)
            {
                sin_gamma_a = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma_a = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            number_t va = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, 1, sin_gamma_a, t1, t2);

            number_t sin_gamma_b;
            if(qp_norm > t1_k_hypo_sqr)
            {
                sin_gamma_b = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma_b = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            number_t vb = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, -1, sin_gamma_b, t1, t2);

            v = std::min(va, vb);
        }

        auto f = std::sqrt(u*u + v*v);
        return f;
    }
};

namespace pluckertree
{
    double FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            Eigen::Vector3f& minimum
    )
    {
        auto minimize = [point,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound](Eigen::Vector3f& minimum){
            //nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX
            nlopt::opt opt(nlopt::LN_SBPLX, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinDist* fun = reinterpret_cast<FixedMomentMinDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;

        /*// Set up parameters
        LBFGSBParam<double> param;
        param.epsilon = 1e-6;
        param.max_iterations = 100;

        // Create solver and function object
        LBFGSBSolver<double> solver(param);  // New solver class
        FixedMomentMinDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

        // Initial guess
        Vector x = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();

        // x will be overwritten to be the best point found
        double dist;
        int nbIter = solver.minimize(fun, x, dist, momentLowerBound.cast<double>(), momentUpperBound.cast<double>());
        minimum = x.cast<float>();

        std::cout << nbIter << " iterations" << std::endl;
        std::cout << "x = \n" << x.transpose() << std::endl;
        std::cout << "f(x) = " << dist << std::endl;

        return dist;*/
    }

    double FindMinHitDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& point_normal,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            Eigen::Vector3f& minimum
    )
    {
        auto minimize = [point,point_normal,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound](Eigen::Vector3f& minimum){
            //nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX
            nlopt::opt opt(nlopt::LN_SBPLX, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinHitDist fun(point.cast<double>(), point_normal.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinHitDist* fun = reinterpret_cast<FixedMomentMinHitDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;
    }

    // Segments
    double segments::FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            float t1Min,
            float t2Max,
            Eigen::Vector3f& min
    )
    {
        auto minimize = [point,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound, t1Min, t2Max](Eigen::Vector3f& minimum){
            nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinSegmentDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>(), t1Min, t2Max);
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinSegmentDist* fun = reinterpret_cast<FixedMomentMinSegmentDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;
    }

    double FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& moment
    )
    {
        FixedMomentMinDist f(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

        Vector vect = moment.cast<double>();
        Vector grad;
        auto dist = f(vect, grad);

        return dist;
    }

///// POINTCLOUD EXPORT
thread_local unsigned int pluckertree::Diag::visited = 0;
thread_local unsigned int pluckertree::Diag::minimizations = 0;
std::optional<std::function<void(float, float, float, int)>> pluckertree::Diag::on_node_visited;
std::optional<std::function<void(float, float, int)>> pluckertree::Diag::on_node_enter;
std::optional<std::function<void(float, float, float, int)>> pluckertree::Diag::on_node_leave;
std::optional<std::function<void(float, float, float, float)>> pluckertree::Diag::on_build_variance_calculated;
bool pluckertree::Diag::force_visit_all = false;

std::random_device pluckertree::MyRand::rand_dev;

struct GridPoint
{
    Eigen::Vector3f pos;
    Eigen::Vector3f grad;
    float dist;
};

/*std::vector<GridPoint> CalculateGrid(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& query_point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        float x_start,
        float x_end,
        float y_start,
        float y_end,
        float z_start,
        float z_end,
        float resolution
)
{
    FixedMomentMinHitDist f(point.cast<double>(), query_point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    auto x_steps = int((x_end - x_start) / resolution);
    auto y_steps = int((y_end - y_start) / resolution);
    auto z_steps = int((z_end - z_start) / resolution);

    std::vector<GridPoint> points(x_steps * y_steps * z_steps);

    for(int x_i = 0; x_i < x_steps; x_i++)
    {
        std::cout << ((float)x_i/(float)x_steps)*100.0f << "%\r";
        std::cout.flush();
        auto x = x_start + resolution*x_i;
        for(int y_i = 0; y_i < y_steps; y_i++)
        {
            auto y = y_start + resolution*y_i;
            for(int z_i = 0; z_i < z_steps; z_i++)
            {
                auto z = z_start + resolution*z_i;

                Vector vect = cart2spherical(Eigen::Vector3d(x, y, z));
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * y_steps * z_steps) + (y_i * z_steps) + z_i];
                curPoint.pos = Eigen::Vector3f(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}*/

std::vector<GridPoint> CalculateBallSlice(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        const Eigen::Vector3f& mlb,
        const Eigen::Vector3f& mub,
        float resolution
)
{
    FixedMomentMinDist f(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    Eigen::Vector3f stepSize = (mub - mlb) / resolution;

    std::vector<GridPoint> points(resolution * resolution * resolution);

    for(int x_i = 0; x_i < resolution; x_i++)
    {
        std::cout << ((float)x_i/(float)resolution)*100.0f << "%\r";
        std::cout.flush();
        auto x = mlb.x() + stepSize.x()*x_i;
        for(int y_i = 0; y_i < resolution; y_i++)
        {
            auto y = mlb.y() + stepSize.y()*y_i;
            for(int z_i = 0; z_i < resolution; z_i++)
            {
                auto z = mlb.z() + stepSize.z()*z_i;

                Vector vect = Eigen::Vector3d(x, y, z);
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * resolution * resolution) + (y_i * resolution) + z_i];
                curPoint.pos = spherical2cart(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}

std::vector<GridPoint> CalculateBallSlice(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& query_point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        const Eigen::Vector3f& mlb,
        const Eigen::Vector3f& mub,
        float resolution
)
{
    FixedMomentMinHitDist f(point.cast<double>(), query_point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    Eigen::Vector3f stepSize = (mub - mlb) / resolution;

    std::vector<GridPoint> points(resolution * resolution * resolution);

    for(int x_i = 0; x_i < resolution; x_i++)
    {
        std::cout << ((float)x_i/(float)resolution)*100.0f << "%\r";
        std::cout.flush();
        auto x = mlb.x() + stepSize.x()*x_i;
        for(int y_i = 0; y_i < resolution; y_i++)
        {
            auto y = mlb.y() + stepSize.y()*y_i;
            for(int z_i = 0; z_i < resolution; z_i++)
            {
                auto z = mlb.z() + stepSize.z()*z_i;

                Vector vect = Eigen::Vector3d(x, y, z);
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * resolution * resolution) + (y_i * resolution) + z_i];
                curPoint.pos = spherical2cart(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}

void show_me_the_grid(std::string& file,
                      const Eigen::Vector3f& dlb,
                      const Eigen::Vector3f& dub,
                      const Eigen::Vector3f& mlb,
                      const Eigen::Vector3f& mub,
                      const Eigen::Vector3f& q)
{
    /*Eigen::Vector3f dlb = Eigen::Vector3f(0,0,-1);
    Eigen::Vector3f dub = Eigen::Vector3f(-std::sqrt(2)/2.0, std::sqrt(2)/2, 0);
    Eigen::Vector3f mlb(-M_PI, 0.785398185, 1);
    Eigen::Vector3f mub(-M_PI/2, 2.3561945, 80);
    Eigen::Vector3f q(25.216011, 86.2393799, 64.2581253);
    Eigen::Vector3f q_normal(0.742901862, 0.662636876, 0.0949169919);*/

    float resolution = 300;
    //auto data = CalculateBallSlice(q, q_normal, dlb, dub, mlb, mub, resolution);
    auto data = CalculateBallSlice(q, dlb, dub, mlb, mub, resolution);

    std::fstream myfile;
    myfile = std::fstream(file, std::ios::out | std::ios::binary);

    unsigned int size = data.size();
    myfile.write(reinterpret_cast<char*>(&size), sizeof(unsigned int));
    for(const auto& entry : data)
    {
        myfile.write(reinterpret_cast<const char*>(&entry.pos.x()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.pos.y()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.pos.z()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.x()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.y()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.z()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.dist), sizeof(float));
    }
    myfile.close();
}

/////

}