// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/LU>
using namespace std;

template<typename MatrixType>
typename MatrixType::RealScalar matrix_l1_norm(const MatrixType& m) {
  return m.cwiseAbs().colwise().sum().maxCoeff();
}

template<typename MatrixType> void lu_non_invertible()
{
  typedef typename MatrixType::RealScalar RealScalar;
  /* this test covers the following files:
     LU.h
  */
  Index rows, cols, cols2;
  if(MatrixType::RowsAtCompileTime==Dynamic)
  {
    rows = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE);
  }
  else
  {
    rows = MatrixType::RowsAtCompileTime;
  }
  if(MatrixType::ColsAtCompileTime==Dynamic)
  {
    cols = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE);
    cols2 = internal::random<int>(2,EIGEN_TEST_MAX_SIZE);
  }
  else
  {
    cols2 = cols = MatrixType::ColsAtCompileTime;
  }

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };
  typedef typename internal::kernel_retval_base<FullPivLU<MatrixType> >::ReturnType KernelMatrixType;
  typedef typename internal::image_retval_base<FullPivLU<MatrixType> >::ReturnType ImageMatrixType;
  typedef Matrix<typename MatrixType::Scalar, ColsAtCompileTime, ColsAtCompileTime>
          CMatrixType;
  typedef Matrix<typename MatrixType::Scalar, RowsAtCompileTime, RowsAtCompileTime>
          RMatrixType;

  Index rank = internal::random<Index>(1, (std::min)(rows, cols)-1);

  // The image of the zero matrix should consist of a single (zero) column vector
  VERIFY((MatrixType::Zero(rows,cols).fullPivLu().image(MatrixType::Zero(rows,cols)).cols() == 1));

  // The kernel of the zero matrix is the entire space, and thus is an invertible matrix of dimensions cols.
  KernelMatrixType kernel = MatrixType::Zero(rows,cols).fullPivLu().kernel();
  VERIFY((kernel.fullPivLu().isInvertible()));

  MatrixType m1(rows, cols), m3(rows, cols2);
  CMatrixType m2(cols, cols2);
  createRandomPIMatrixOfRank(rank, rows, cols, m1);

  FullPivLU<MatrixType> lu;

  // The special value 0.01 below works well in tests. Keep in mind that we're only computing the rank
  // of singular values are either 0 or 1.
  // So it's not clear at all that the epsilon should play any role there.
  lu.setThreshold(RealScalar(0.01));
  lu.compute(m1);

  MatrixType u(rows,cols);
  u = lu.matrixLU().template triangularView<Upper>();
  RMatrixType l = RMatrixType::Identity(rows,rows);
  l.block(0,0,rows,(std::min)(rows,cols)).template triangularView<StrictlyLower>()
    = lu.matrixLU().block(0,0,rows,(std::min)(rows,cols));

  VERIFY_IS_APPROX(lu.permutationP() * m1 * lu.permutationQ(), l*u);

  KernelMatrixType m1kernel = lu.kernel();
  ImageMatrixType m1image = lu.image(m1);

  VERIFY_IS_APPROX(m1, lu.reconstructedMatrix());
  VERIFY(rank == lu.rank());
  VERIFY(cols - lu.rank() == lu.dimensionOfKernel());
  VERIFY(!lu.isInjective());
  VERIFY(!lu.isInvertible());
  VERIFY(!lu.isSurjective());
  VERIFY_IS_MUCH_SMALLER_THAN((m1 * m1kernel), m1);
  VERIFY(m1image.fullPivLu().rank() == rank);
  VERIFY_IS_APPROX(m1 * m1.adjoint() * m1image, m1image);

  m2 = CMatrixType::Random(cols,cols2);
  m3 = m1*m2;
  m2 = CMatrixType::Random(cols,cols2);
  // test that the code, which does resize(), may be applied to an xpr
  m2.block(0,0,m2.rows(),m2.cols()) = lu.solve(m3);
  VERIFY_IS_APPROX(m3, m1*m2);

  // test solve with transposed
  m3 = MatrixType::Random(rows,cols2);
  m2 = m1.transpose()*m3;
  m3 = MatrixType::Random(rows,cols2);
  lu.template _solve_impl_transposed<false>(m2, m3);
  VERIFY_IS_APPROX(m2, m1.transpose()*m3);
  m3 = MatrixType::Random(rows,cols2);
  m3 = lu.transpose().solve(m2);
  VERIFY_IS_APPROX(m2, m1.transpose()*m3);

  // test solve with conjugate transposed
  m3 = MatrixType::Random(rows,cols2);
  m2 = m1.adjoint()*m3;
  m3 = MatrixType::Random(rows,cols2);
  lu.template _solve_impl_transposed<true>(m2, m3);
  VERIFY_IS_APPROX(m2, m1.adjoint()*m3);
  m3 = MatrixType::Random(rows,cols2);
  m3 = lu.adjoint().solve(m2);
  VERIFY_IS_APPROX(m2, m1.adjoint()*m3);
}

template<typename MatrixType> void lu_invertible()
{
  /* this test covers the following files:
     LU.h
  */
  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
  Index size = MatrixType::RowsAtCompileTime;
  if( size==Dynamic)
    size = internal::random<Index>(1,EIGEN_TEST_MAX_SIZE);

  MatrixType m1(size, size), m2(size, size), m3(size, size);
  FullPivLU<MatrixType> lu;
  lu.setThreshold(RealScalar(0.01));
  do {
    m1 = MatrixType::Random(size,size);
    lu.compute(m1);
  } while(!lu.isInvertible());

  VERIFY_IS_APPROX(m1, lu.reconstructedMatrix());
  VERIFY(0 == lu.dimensionOfKernel());
  VERIFY(lu.kernel().cols() == 1); // the kernel() should consist of a single (zero) column vector
  VERIFY(size == lu.rank());
  VERIFY(lu.isInjective());
  VERIFY(lu.isSurjective());
  VERIFY(lu.isInvertible());
  VERIFY(lu.image(m1).fullPivLu().isInvertible());
  m3 = MatrixType::Random(size,size);
  m2 = lu.solve(m3);
  VERIFY_IS_APPROX(m3, m1*m2);
  MatrixType m1_inverse = lu.inverse();
  VERIFY_IS_APPROX(m2, m1_inverse*m3);

  RealScalar rcond = (RealScalar(1) / matrix_l1_norm(m1)) / matrix_l1_norm(m1_inverse);
  const RealScalar rcond_est = lu.rcond();
  // Verify that the estimated condition number is within a factor of 10 of the
  // truth.
  VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);

  // test solve with transposed
  lu.template _solve_impl_transposed<false>(m3, m2);
  VERIFY_IS_APPROX(m3, m1.transpose()*m2);
  m3 = MatrixType::Random(size,size);
  m3 = lu.transpose().solve(m2);
  VERIFY_IS_APPROX(m2, m1.transpose()*m3);

  // test solve with conjugate transposed
  lu.template _solve_impl_transposed<true>(m3, m2);
  VERIFY_IS_APPROX(m3, m1.adjoint()*m2);
  m3 = MatrixType::Random(size,size);
  m3 = lu.adjoint().solve(m2);
  VERIFY_IS_APPROX(m2, m1.adjoint()*m3);

  // Regression test for Bug 302
  MatrixType m4 = MatrixType::Random(size,size);
  VERIFY_IS_APPROX(lu.solve(m3*m4), lu.solve(m3)*m4);
}

template<typename MatrixType> void lu_partial_piv()
{
  /* this test covers the following files:
     PartialPivLU.h
  */
  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
  Index size = internal::random<Index>(1,4);

  MatrixType m1(size, size), m2(size, size), m3(size, size);
  m1.setRandom();
  PartialPivLU<MatrixType> plu(m1);

  VERIFY_IS_APPROX(m1, plu.reconstructedMatrix());

  m3 = MatrixType::Random(size,size);
  m2 = plu.solve(m3);
  VERIFY_IS_APPROX(m3, m1*m2);
  MatrixType m1_inverse = plu.inverse();
  VERIFY_IS_APPROX(m2, m1_inverse*m3);

  RealScalar rcond = (RealScalar(1) / matrix_l1_norm(m1)) / matrix_l1_norm(m1_inverse);
  const RealScalar rcond_est = plu.rcond();
  // Verify that the estimate is within a factor of 10 of the truth.
  VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);

  // test solve with transposed
  plu.template _solve_impl_transposed<false>(m3, m2);
  VERIFY_IS_APPROX(m3, m1.transpose()*m2);
  m3 = MatrixType::Random(size,size);
  m3 = plu.transpose().solve(m2);
  VERIFY_IS_APPROX(m2, m1.transpose()*m3);

  // test solve with conjugate transposed
  plu.template _solve_impl_transposed<true>(m3, m2);
  VERIFY_IS_APPROX(m3, m1.adjoint()*m2);
  m3 = MatrixType::Random(size,size);
  m3 = plu.adjoint().solve(m2);
  VERIFY_IS_APPROX(m2, m1.adjoint()*m3);
}

template<typename MatrixType> void lu_verify_assert()
{
  MatrixType tmp;

  FullPivLU<MatrixType> lu;
  VERIFY_RAISES_ASSERT(lu.matrixLU())
  VERIFY_RAISES_ASSERT(lu.permutationP())
  VERIFY_RAISES_ASSERT(lu.permutationQ())
  VERIFY_RAISES_ASSERT(lu.kernel())
  VERIFY_RAISES_ASSERT(lu.image(tmp))
  VERIFY_RAISES_ASSERT(lu.solve(tmp))
  VERIFY_RAISES_ASSERT(lu.determinant())
  VERIFY_RAISES_ASSERT(lu.rank())
  VERIFY_RAISES_ASSERT(lu.dimensionOfKernel())
  VERIFY_RAISES_ASSERT(lu.isInjective())
  VERIFY_RAISES_ASSERT(lu.isSurjective())
  VERIFY_RAISES_ASSERT(lu.isInvertible())
  VERIFY_RAISES_ASSERT(lu.inverse())

  PartialPivLU<MatrixType> plu;
  VERIFY_RAISES_ASSERT(plu.matrixLU())
  VERIFY_RAISES_ASSERT(plu.permutationP())
  VERIFY_RAISES_ASSERT(plu.solve(tmp))
  VERIFY_RAISES_ASSERT(plu.determinant())
  VERIFY_RAISES_ASSERT(plu.inverse())
}

void test_lu()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( lu_non_invertible<Matrix3f>() );
    CALL_SUBTEST_1( lu_invertible<Matrix3f>() );
    CALL_SUBTEST_1( lu_verify_assert<Matrix3f>() );

    CALL_SUBTEST_2( (lu_non_invertible<Matrix<double, 4, 6> >()) );
    CALL_SUBTEST_2( (lu_verify_assert<Matrix<double, 4, 6> >()) );

    CALL_SUBTEST_3( lu_non_invertible<MatrixXf>() );
    CALL_SUBTEST_3( lu_invertible<MatrixXf>() );
    CALL_SUBTEST_3( lu_verify_assert<MatrixXf>() );

    CALL_SUBTEST_4( lu_non_invertible<MatrixXd>() );
    CALL_SUBTEST_4( lu_invertible<MatrixXd>() );
    CALL_SUBTEST_4( lu_partial_piv<MatrixXd>() );
    CALL_SUBTEST_4( lu_verify_assert<MatrixXd>() );

    CALL_SUBTEST_5( lu_non_invertible<MatrixXcf>() );
    CALL_SUBTEST_5( lu_invertible<MatrixXcf>() );
    CALL_SUBTEST_5( lu_verify_assert<MatrixXcf>() );

    CALL_SUBTEST_6( lu_non_invertible<MatrixXcd>() );
    CALL_SUBTEST_6( lu_invertible<MatrixXcd>() );
    CALL_SUBTEST_6( lu_partial_piv<MatrixXcd>() );
    CALL_SUBTEST_6( lu_verify_assert<MatrixXcd>() );

    CALL_SUBTEST_7(( lu_non_invertible<Matrix<float,Dynamic,16> >() ));

    // Test problem size constructors
    CALL_SUBTEST_9( PartialPivLU<MatrixXf>(10) );
    CALL_SUBTEST_9( FullPivLU<MatrixXf>(10, 20); );
  }
}


#pragma once

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "../utils.hpp"
#include "Operator.hpp"

namespace yavque
{

class DenseHermitianMatrix
{
private:
	const Eigen::MatrixXcd ham_;

	mutable bool diagonalized_;
	mutable Eigen::VectorXd evals_;
	mutable Eigen::MatrixXcd evecs_;

	void diagonalize() const
	{
		// Add mutex (future)
		if(diagonalized_)
		{
			return;
		}

		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> es(ham_);
		evals_ = es.eigenvalues();
		evecs_ = es.eigenvectors();
		diagonalized_ = true;
	}

public:
	explicit DenseHermitianMatrix(Eigen::MatrixXcd ham) : ham_{std::move(ham)}
	{
		assert(ham_.rows() == ham_.cols()); // check diagonal
		diagonalized_ = false;
	}

	[[nodiscard]] uint32_t dim() const { return ham_.rows(); }

	[[nodiscard]] const Eigen::MatrixXcd& get_ham() const& { return ham_; }

	[[nodiscard]] Eigen::MatrixXcd get_ham() && { return ham_; }

	[[nodiscard]] const Eigen::MatrixXcd& evecs() const&
	{
		if(!diagonalized_)
		{
			diagonalize();
		}
		return evecs_;
	}

	[[nodiscard]] Eigen::MatrixXcd evecs() &&
	{
		if(!diagonalized_)
		{
			diagonalize();
		}
		return evecs_;
	}

	[[nodiscard]] const Eigen::VectorXd& evals() const&
	{
		if(!diagonalized_)
		{
			diagonalize();
		}
		return evals_;
	}

	[[nodiscard]] Eigen::VectorXd evals() &&
	{
		if(!diagonalized_)
		{
			diagonalize();
		}
		return evals_;
	}

	[[nodiscard]] Eigen::MatrixXcd ham_exp(cx_double x) const
	{
		if(!diagonalized_)
		{
			diagonalize();
		}

		Eigen::VectorXcd v = exp(x * evals_.array());
		return evecs_ * v.asDiagonal() * evecs_.adjoint();
	}
};

} // namespace yavque


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

// Contains Quickbook snippets used by boost/libs/multiprecision/doc/multiprecision.qbk,
// section Literal Types and constexpr Support.

#include <iostream>
#include <boost/config.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

//[constexpr_circle
#include <boost/math/constants/constants.hpp> // For constant pi with full precision of type T.
// using  boost::math::constants::pi;

template <class T>
inline constexpr T circumference(T radius)
{
   return 2 * boost::math::constants::pi<T>() * radius;
}

template <class T>
inline constexpr T area(T radius)
{
   return boost::math::constants::pi<T>() * radius * radius;
}
//] [/constexpr_circle]

template <class T, unsigned Order>
struct const_polynomial
{
 public:
   T data[Order + 1];

 public:
   constexpr const_polynomial(T val = 0) : data{val} {}
   constexpr const_polynomial(const std::initializer_list<T>& init) : data{}
   {
      if (init.size() > Order + 1)
         throw std::range_error("Too many initializers in list!");
      for (unsigned i = 0; i < init.size(); ++i)
         data[i] = init.begin()[i];
   }
   constexpr T& operator[](std::size_t N)
   {
      return data[N];
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return data[N];
   }
   template <class U>
   constexpr T operator()(U val)const
   {
      T result = data[Order];
      for (unsigned i = Order; i > 0; --i)
      {
         result *= val;
         result += data[i - 1];
      }
      return result;
   }
   constexpr const_polynomial<T, Order - 1> derivative() const
   {
      const_polynomial<T, Order - 1> result;
      for (unsigned i = 1; i <= Order; ++i)
      {
         result[i - 1] = (*this)[i] * i;
      }
      return result;
   }
   constexpr const_polynomial operator-()
   {
      const_polynomial t(*this);
      for (unsigned i = 0; i <= Order; ++i)
         t[i] = -t[i];
      return t;
   }
   template <class U>
   constexpr const_polynomial& operator*=(U val)
   {
      for (unsigned i = 0; i <= Order; ++i)
         data[i] = data[i] * val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator/=(U val)
   {
      for (unsigned i = 0; i <= Order; ++i)
         data[i] = data[i] / val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator+=(U val)
   {
      data[0] += val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator-=(U val)
   {
      data[0] -= val;
      return *this;
   }
};

template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, (Order1 > Order2 ? Order1 : Order2)> operator+(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   if
      constexpr(Order1 > Order2)
      {
         const_polynomial<T, Order1> result(a);
         for (unsigned i = 0; i <= Order2; ++i)
            result[i] += b[i];
         return result;
      }
   else
   {
      const_polynomial<T, Order2> result(b);
      for (unsigned i = 0; i <= Order1; ++i)
         result[i] += a[i];
      return result;
   }
}
template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, (Order1 > Order2 ? Order1 : Order2)> operator-(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   if
      constexpr(Order1 > Order2)
      {
         const_polynomial<T, Order1> result(a);
         for (unsigned i = 0; i <= Order2; ++i)
            result[i] -= b[i];
         return result;
      }
   else
   {
      const_polynomial<T, Order2> result(b);
      for (unsigned i = 0; i <= Order1; ++i)
         result[i] = a[i] - b[i];
      return result;
   }
}
template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, Order1 + Order2> operator*(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   const_polynomial<T, Order1 + Order2> result;
   for (unsigned i = 0; i <= Order1; ++i)
   {
      for (unsigned j = 0; j <= Order2; ++j)
      {
         result[i + j] += a[i] * b[j];
      }
   }
   return result;
}
template <class T, unsigned Order, class U>
inline constexpr const_polynomial<T, Order> operator*(const const_polynomial<T, Order>& a, const U& b)
{
   const_polynomial<T, Order> result(a);
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] *= b;
   }
   return result;
}
template <class U, class T, unsigned Order>
inline constexpr const_polynomial<T, Order> operator*(const U& b, const const_polynomial<T, Order>& a)
{
   const_polynomial<T, Order> result(a);
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] *= b;
   }
   return result;
}
template <class T, unsigned Order, class U>
inline constexpr const_polynomial<T, Order> operator/(const const_polynomial<T, Order>& a, const U& b)
{
   const_polynomial<T, Order> result;
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] /= b;
   }
   return result;
}

//[hermite_example
template <class T, unsigned Order>
class hermite_polynomial
{
   const_polynomial<T, Order> m_data;

 public:
   constexpr hermite_polynomial() : m_data(hermite_polynomial<T, Order - 1>().data() * const_polynomial<T, 1>{0, 2} - hermite_polynomial<T, Order - 1>().data().derivative())
   {
   }
   constexpr const const_polynomial<T, Order>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N)const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)const
   {
      return m_data(val);
   }
};
//] [/hermite_example]

//[hermite_example2
template <class T>
class hermite_polynomial<T, 0>
{
   const_polynomial<T, 0> m_data;

 public:
   constexpr hermite_polynomial() : m_data{1} {}
   constexpr const const_polynomial<T, 0>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)
   {
      return m_data(val);
   }
};

template <class T>
class hermite_polynomial<T, 1>
{
   const_polynomial<T, 1> m_data;

 public:
   constexpr hermite_polynomial() : m_data{0, 2} {}
   constexpr const const_polynomial<T, 1>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)
   {
      return m_data(val);
   }
};
//] [/hermite_example2]

void test_double()
{
   constexpr double radius = 2.25;
   constexpr double c      = circumference(radius);
   constexpr double a      = area(radius);

   std::cout << "Circumference = " << c << std::endl;
   std::cout << "Area = " << a << std::endl;

   constexpr const_polynomial<double, 2> pa = {3, 4};
   constexpr const_polynomial<double, 2> pb = {5, 6};
   static_assert(pa[0] == 3);
   static_assert(pa[1] == 4);
   constexpr auto pc = pa * 2;
   static_assert(pc[0] == 6);
   static_assert(pc[1] == 8);
   constexpr auto pd = 3 * pa;
   static_assert(pd[0] == 3 * 3);
   static_assert(pd[1] == 4 * 3);
   constexpr auto pe = pa + pb;
   static_assert(pe[0] == 3 + 5);
   static_assert(pe[1] == 4 + 6);
   constexpr auto pf = pa - pb;
   static_assert(pf[0] == 3 - 5);
   static_assert(pf[1] == 4 - 6);
   constexpr auto pg = pa * pb;
   static_assert(pg[0] == 15);
   static_assert(pg[1] == 38);
   static_assert(pg[2] == 24);

   #if defined(__clang__) && (__clang_major__ < 6)
   #else
   constexpr hermite_polynomial<double, 2> h1;
   static_assert(h1[0] == -2);
   static_assert(h1[1] == 0);
   static_assert(h1[2] == 4);

   constexpr hermite_polynomial<double, 3> h3;
   static_assert(h3[0] == 0);
   static_assert(h3[1] == -12);
   static_assert(h3[2] == 0);
   static_assert(h3[3] == 8);

   constexpr hermite_polynomial<double, 9> h9;
   static_assert(h9[0] == 0);
   static_assert(h9[1] == 30240);
   static_assert(h9[2] == 0);
   static_assert(h9[3] == -80640);
   static_assert(h9[4] == 0);
   static_assert(h9[5] == 48384);
   static_assert(h9[6] == 0);
   static_assert(h9[7] == -9216);
   static_assert(h9[8] == 0);
   static_assert(h9[9] == 512);

   static_assert(h9(0.5) == 6481);
   #endif
}

void test_float128()
{
#ifdef BOOST_HAS_FLOAT128
//[constexpr_circle_usage

   using boost::multiprecision::float128;

   constexpr float128 radius = 2.25;
   constexpr float128 c      = circumference(radius);
   constexpr float128 a      = area(radius);

   std::cout << "Circumference = " << c << std::endl;
   std::cout << "Area = " << a << std::endl;

 //]   [/constexpr_circle_usage]

   constexpr hermite_polynomial<float128, 2> h1;
   static_assert(h1[0] == -2);
   static_assert(h1[1] == 0);
   static_assert(h1[2] == 4);

   constexpr hermite_polynomial<float128, 3> h3;
   static_assert(h3[0] == 0);
   static_assert(h3[1] == -12);
   static_assert(h3[2] == 0);
   static_assert(h3[3] == 8);

   //[hermite_example3
   constexpr hermite_polynomial<float128, 9> h9;
   //
   // Verify that the polynomial's coefficients match the known values:
   //
   static_assert(h9[0] == 0);
   static_assert(h9[1] == 30240);
   static_assert(h9[2] == 0);
   static_assert(h9[3] == -80640);
   static_assert(h9[4] == 0);
   static_assert(h9[5] == 48384);
   static_assert(h9[6] == 0);
   static_assert(h9[7] == -9216);
   static_assert(h9[8] == 0);
   static_assert(h9[9] == 512);
   //
   // Define an abscissa value to evaluate at:
   constexpr float128 abscissa(0.5);
   //
   // Evaluate H_9(0.5) using all constexpr arithmetic, and check that it has the expected result:
   static_assert(h9(abscissa) == 6481);
   //]
#endif
}

int main()
{
   test_double();
   test_float128();
   std::cout << "Done!" << std::endl;
}


#ifndef SPHEP_GENERATOR_HH
#define SPHEP_GENERATOR_HH

#include <Eigen/Dense>

namespace sphep
{
  // compute force from b to a
  template <class V>
  V computeForce(const V& a, const V& b)
  {
    // compute direction of force a-b
    auto diff = a - b;
    // scale force by norm(diff)^3
    typename V::Scalar diffsq = diff.squaredNorm();
    return diff / (diffsq * std::sqrt(diffsq));
  }

  // update the veloctiy in an explicit euler fashion
  template <class V>
  void updateVelocity(const std::vector<V>& points, typename V::Scalar dt, std::vector<V>& velocity)
  {
    assert(points.size() == velocity.size());

    for (std::size_t i = 0; i < points.size(); ++i) {
      for (std::size_t j = i + 1; j < points.size(); ++j) {
        // compute force from j to i
        V diff = computeForce(points[i], points[j]);
        // update velocity of i and j with dt*diff (using action = reaction)
        velocity[i] += dt * diff;
        velocity[j] -= dt * diff;
      }
    }
  }

  // redistribute the given points on the unit sphere, such that they are
  // more evenly distributed
  template <class V>
  void optimizePointsOnUnitSphere(std::vector<V>& points, unsigned int iterations,
                                  typename V::Scalar dist, typename V::Scalar dt)
  {
    const std::size_t N = points.size();
    std::vector<V> velocity(points.size(), V::Zero());
    for (unsigned int iter = 0; iter < iterations; ++iter) {
      // update velocity (explicit euler)
      updateVelocity(points, dt, velocity);
      // update points (explicit euler)
      typename V::Scalar globalDist = 0.0;
      for (std::size_t i = 0; i < N; ++i) {
        V oldPoint(points[i]);
        // update point
        points[i] += dt * velocity[i];
        // fetch p_i back onto sphere
        points[i] /= points[i].norm();
        // compute distance between old and newpoint
        globalDist += (oldPoint - points[i]).norm();
      }
      // stop iteration if global update is less than threshold
      if (globalDist < dist) {
        break;
      }
    }
  }

  // non-specialized struct for generating random points on the unit sphere
  template <class Vector>
  struct PointOnUnitSphereGenerator;

  // three dimensional spcialization of the random points on unit sphere
  // generator. Uses sphere coordinates.
  template <class T>
  struct PointOnUnitSphereGenerator<Eigen::Matrix<T, 3, 1> >
  {
    typedef Eigen::Matrix<T, 3, 1> Vector;

    Vector operator()() const
    {
      // generate random sphere coordinates between [0,2*PI]
      T theta = 2.0 * M_PI * static_cast<T>(std::rand()) / RAND_MAX;
      T phi = 2.0 * M_PI * static_cast<T>(std::rand()) / RAND_MAX;
      // transform sphere coordinates to cartesian
      Vector p;
      p << std::sin(theta) * std::cos(phi), std::sin(theta) * std::sin(phi), std::cos(theta);
      return p;
    }
  };

  // perform the operation axpy on a vector
  template <class Vector>
  struct ScaleAndTranslateFunctor
  {
    typedef typename Vector::Scalar Scalar;

    ScaleAndTranslateFunctor(const Scalar& factor_ = Scalar(1.0),
                             const Vector& offset_ = Vector::Zero())
        : factor(factor_)
        , offset(offset_)
    {
    }

    void operator()(Vector& vector) const { vector = vector * factor + offset; }

    Scalar factor;
    Vector offset;
  };

  // generator more or less evenly distributed points on a sphere
  template <class Vector>
  void generatePoints(unsigned int N, std::vector<Vector>& points,
                      const Vector& center = Vector::Zero(), typename Vector::Scalar radius = 1.0,
                      unsigned int iterations = 500, typename Vector::Scalar dist = 1e-3,
                      typename Vector::Scalar dt = 0.1)
  {
    typedef typename Vector::Scalar Scalar;
    // construct vector
    std::vector<Vector> newPoints;
    newPoints.reserve(N);

    // generate N points on unit sphere
    std::generate_n(std::back_inserter(newPoints), N, PointOnUnitSphereGenerator<Vector>());
    // optimize these points on unit sphere
    optimizePointsOnUnitSphere(newPoints, iterations, dist, dt);

    // map from unit sphere to radius-sphere at center
    std::for_each(newPoints.begin(), newPoints.end(),
                  ScaleAndTranslateFunctor<Vector>(radius, center));

    // copy the new vectors into the output
    std::copy(newPoints.begin(), newPoints.end(), std::back_inserter(points));
  }
}

#endif // SPHEP_GENERATOR_HH


#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cassert>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "aux/eigen2hdf.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/h2n_1d.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"

//#include <deal.II/base/exceptions.h>

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;

  string fname = "spectral_basis.desc";
  cout << "reading `" << fname << "` from disk.\n";
  SpectralBasisFactoryKS::create(polar_basis, fname);

  int K = spectral::get_max_k(polar_basis) + 1;
  int N = polar_basis.n_dofs();

  // create basis files
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  cout << "Hermite basis size: " << hermite_basis.n_dofs() << endl;
  cout << "Polar basis size: " << polar_basis.n_dofs() << endl;

  // polar 2 hermite
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);

  typedef Eigen::MatrixXd mat_t;
  // hermite to nodal
  Hermite2Nodal<hermite_basis_t> h2n(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K); });

  // debug
  auto& M = h2n.get_h2n();
  cout << "M := H2N-matrix\n";
  cout << "M.T * M\n";
  Eigen::MatrixXd O = M.transpose() * M;

  double MtM_diff = (O - Eigen::MatrixXd::Identity(O.rows(), O.cols())).cwiseAbs().sum();
  if (MtM_diff > 1e-13 * O.rows() * O.cols()) {
    cout << "diff: " << MtM_diff << "\n";
    O = (O.array().cwiseAbs() > 1e-15).select(O, Eigen::ArrayXXd::Zero(O.rows(), O.cols()));
    cout << O << endl;
  }

  cout << "---------- Test: P->H->N->H->P  ----------"
       << "\n";
  fname = "coefficients.h5";
  vec_t cp(N);  // polar coefficients
  if (!boost::filesystem::exists(fname)) {
    cp.setOnes();
  } else {
    hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(h5_init, "coeffs", cp);
    H5Fclose(h5_init);
  }

  assert(polar_basis.n_dofs() == cp.size());

  // Transform Polar to Hermite basis
  vec_t ch = cp;
  P2H.to_hermite(ch, cp);

  // Transform Hermite to Nodal basis
  Eigen::MatrixXd cn;
  h2n.to_nodal(cn, ch);
  // Transform Nodal to Hermite basis
  h2n.to_hermite(ch.data(), cn);
  // Tranform Hermite to Polar basis
  vec_t cp2(N);
  P2H.to_polar(cp2, ch);

  /* hid_t h5f = H5Fcreate("out.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT); */
  /* eigen2hdf::save(h5f, "coeffs_nodal", cn); */
  /* eigen2hdf::save(h5f, "coeffs_hermite", ch); */
  /* eigen2hdf::save(h5f, "coeffs", cp2); */
  /* H5Fclose(h5f); */

  vec_t diff = (cp2 - cp).rowwise().squaredNorm();
  if (diff.sum() > 1e-10) {
    cout << "diff\n";
    cout << diff << endl;
  } else {
    cout << "sum(diff) " << diff.sum() << endl;
  }

  // int i,j;
  // cout << "max diff: " << diff.maxCoeff(&i, &j) << " at " << i << ", " << j << endl;

  return 0;
}


// This is C++ code for discretizing AR(1) process by using Rouwenhorst method (1995)
// written on 2017/10/17
// z' = rho*z+e e~N(0,(sig_e)^2)
//
// INPUTS (through Terminal)
// N:     Number of grids
// rho:   Coeffecient of AR(1) process z'=mew+rho*z+e, e~N(0,sigmasq)
// sig_e: Std of innovation, e, in AR(1) process
//
// OUTPUT
// vZ:    grids
// mPI:   transition prob. matrix

#include <iostream>
#include <iomanip>
#include <cmath>
#include <random>
#include <armadillo>  // YOU SHOULD INSTALL "armadillo" BEFORE USING THIS FILE

//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//                          MAIN PART
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

int main(int argc, char* argv[])
{
  ///////////////////////////////////////////////////////////
  // 1. Basic Parameters
  ///////////////////////////////////////////////////////////

  // Ex. Standard quarterly calibration for pruductivity
  // process of the US economy

  int     N = 5;          // Number of grids, N
  double  rho = 0.95;     // AR(1) coefficient
  double  sig_e = 0.007;  // std of innovation

  ///////////////////////////////////////////////////////////
  // 2. Generating grids && Parameterization
  ///////////////////////////////////////////////////////////

  double  sig_z = sqrt(pow(sig_e,2)/(1-pow(rho,2)));  // std of AR(1) process r.v.
  double  z_max = sig_z*sqrt(N-1);                    // Max grid
  double  z_min = -z_max;                             // Min grid
  double  p = (1+rho)/2, q = p;                       // prob. parameters

  arma::vec vZ = arma::linspace<arma::vec>(z_min,z_max,N); // Grid vector 1*N

  ////////////////////////////////////////////////////////////
  // 3. Transition matrix
  ////////////////////////////////////////////////////////////

  arma::mat mPI_old = {{p,1-p},{1-q,q}}; // Initial PI

  // for recursive computation
  arma::mat mPI_new(3,3);
  arma::mat mPI_new1(3,3);
  arma::mat mPI_new2(3,3);
  arma::mat mPI_new3(3,3);
  arma::mat mPI_new4(3,3);

  for (int i=3;i<N+1; i++)
  {

    mPI_new.zeros(i,i);
    mPI_new1.zeros(i,i);
    mPI_new2.zeros(i,i);
    mPI_new3.zeros(i,i);
    mPI_new4.zeros(i,i);

    mPI_new1.submat(0,0,i-2,i-2) = mPI_old;
    mPI_new2.submat(0,1,i-2,i-1) = mPI_old;
    mPI_new3.submat(1,0,i-1,i-2) = mPI_old;
    mPI_new4.submat(1,1,i-1,i-1) = mPI_old;

    mPI_new = p*mPI_new1 + (1-p)*mPI_new2 + (1-q)*mPI_new3 + q*mPI_new4;
    mPI_new.submat(1,0,i-2,i-1) = 0.5 * mPI_new.submat(1,0,i-2,i-1); // divide by 2

    mPI_old.resize(i,i);
    mPI_old = mPI_new;

  }

  ////////////////////////////////////////////////////////////
  // 4. Display result with 4 decimals
  ////////////////////////////////////////////////////////////

  std::cout << "Grids are\n";
  for (int i=0; i<N; i++)
  {
    std::cout << std::fixed << std::setprecision(4) << vZ(i) << " ";
  }

  std::cout << "\n \nTransition Prob Matrix is \n";
  for (int i = 0; i < N; i++)
  {
    for (int j = 0; j < N; j++)
    {
      if (j == N-1)
      {
        std::cout << std::fixed << std::setprecision(4) << mPI_new(i,j) << '\n';
      }
      else
      {
        std::cout << std::fixed << std::setprecision(4) << mPI_new(i,j) << ' ';
      }
    }
  }
  return 0;
}


#include <iostream>
#include <string>
#include <armadillo>
#include <time.h>
#include "ga.h"
#include "simpleNN.h"

using namespace std;
using namespace arma;

//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec sigmoid(vec inp){
    return 1/(1 + exp(-inp));
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec myTanh(vec inp){
    return tanh(inp);
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
rowvec sigmoidR(rowvec inp){
    return 1/(1 + exp(-inp));
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
rowvec myTanhR(rowvec inp){
    return tanh(inp);
}
//------------------------------------------------------------------------------------------------------------------------
mat simpleNNmat(mat X, vec inp, mat inpWeights , cube weights, mat outWeights ,vec inpBiases, mat biases, vec outBiases){

    vec buffVec;
    buffVec = sigmoid(inpWeights*inp + inpBiases);

    int largeI = 0;

    mat Y(X.n_rows, outBiases.n_elem);

// Adding Data from values from x to each type of weights and biases.
    for(int i = 0; i < X.n_rows; i++){

        for(int dummyX = 0; dummyX < inpWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < inpWeights.n_cols; dummyY++){
                inpWeights.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < weights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < weights.n_cols; dummyY++){
                for(int dummyZ = 0; dummyZ < weights.n_slices; dummyZ++){
                    weights.at(dummyX,dummyY,dummyZ) = X.at(i,largeI);
                    largeI++;
                }
            }
        }

        for(int dummyX = 0; dummyX < outWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < outWeights.n_cols; dummyY++){
                outWeights.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < inpBiases.n_rows; dummyX++){
            inpBiases.at(dummyX) = X.at(i,largeI);
            largeI++;
        }

        for(int dummyX = 0; dummyX < biases.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < biases.n_cols; dummyY++){
                biases.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < outBiases.n_rows; dummyX++){
            outBiases.at(dummyX) = X.at(i,largeI);
            largeI++;
        }

// Feed Forward into hidden.
        buffVec = myTanh(inpWeights*inp + inpBiases);
// Feed Forward each hidden Layer.
        for(int l = 0; l < weights.n_slices; l++){
            buffVec = myTanh(weights.slice(l)*buffVec + biases.col(l));
          }
// Feed Forward each hidden Layer.
          Y.row(i) = sigmoidR((outWeights*buffVec).t() + outBiases.t());
   largeI = 0;
   }

      return Y;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec simpleNNvec(vec x, vec inp, mat inpWeights , cube weights, mat outWeights ,vec inpBiases, mat biases, vec outBiases){

    vec buffVec;
    buffVec = sigmoid(inpWeights*inp + inpBiases);

    int largeI = 0;

    vec y(outBiases.n_elem);

// Adding Data from values from x to each type of weights and biases.
        for(int dummyX = 0; dummyX < inpWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < inpWeights.n_cols; dummyY++){
                inpWeights.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < weights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < weights.n_cols; dummyY++){
                for(int dummyZ = 0; dummyZ < weights.n_slices; dummyZ++){

                    weights.at(dummyX,dummyY,dummyZ) = x.at(largeI);
                    largeI++;
                }
            }
        }

        for(int dummyX = 0; dummyX < outWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < outWeights.n_cols; dummyY++){
                outWeights.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < inpBiases.n_rows; dummyX++){
            inpBiases.at(dummyX) = x.at(largeI);
            largeI++;
        }

        for(int dummyX = 0; dummyX < biases.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < biases.n_cols; dummyY++){
                biases.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < outBiases.n_rows; dummyX++){
            outBiases.at(dummyX) = x.at(largeI);
            largeI++;
        }

// Feed Forward into hidden.
        buffVec = myTanh(inpWeights*inp + inpBiases);
// Feed Forward each hidden Layer.
        for(int l = 0; l < weights.n_slices; l++){
            buffVec = myTanh(weights.slice(l)*buffVec + biases.col(l));
          }
// Feed Forward  final Layer.
          y = sigmoid((outWeights*buffVec)  + outBiases);
      return y;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------


/*
 * Copyright 2017 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   CoordinateTransformTest.cpp
 * Author: glm,jordan
 *
 */

#include "catch.hpp"
#include "../src/math/CoordinateTransform.hpp"
#include <Eigen/Dense>
#include "../src/Position.hpp"
#include <math.h>

#define POSITION_PRECISION 0.00000001

/**Test if the coordinate transform works*/
TEST_CASE("Coordinate Transform Test") {

    // prime meridian, equator, on ellipsoid
    Position primeMeridianEquator(0, 0, 0, 0);

    Eigen::Vector3d testPositionECEF;
    CoordinateTransform::getPositionECEF(testPositionECEF,primeMeridianEquator);

    Eigen::Vector3d expectedPositionECEF;
    expectedPositionECEF << 6378137.0 , 0, 0;

    REQUIRE( testPositionECEF.isApprox(expectedPositionECEF, POSITION_PRECISION) );

    // North Pole on ellipsoid
    Position northPole(0, 90, 0, 0);
    Eigen::Vector3d northPoleECEF;
    CoordinateTransform::getPositionECEF(northPoleECEF,northPole);

    Eigen::Vector3d expectedNorthPoleECEF;
    expectedNorthPoleECEF << 0, 0, 6356752.3142; // WGS 84 polar semi-minor axis

    double northPolePrecision = 0.00000001;
    REQUIRE( northPoleECEF.isApprox(expectedNorthPoleECEF, northPolePrecision) );

}

/**test if the conversion between ECEF and longitude latitude height works*/
TEST_CASE("Conversions between ECEF and Longitude Latitude Height") {
    Position testPosition(0, 48, -68, 10);

    Eigen::Vector3d testPositionECEF;
    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    Position expectedPosition(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, 60, 79, -100);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, -60, -79, 100);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, -70, 48, 90);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, 70, 60, 80);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, 0, 0, 0);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, -80, -40, -50);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);

    testPosition = Position(0, 87, -43, -59);

    CoordinateTransform::getPositionECEF(testPositionECEF,testPosition);

    expectedPosition = Position(0,0,0,0);
    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(testPositionECEF, expectedPosition);

    std::cout << "expectedPosition:" << std::endl;
    std::cout << expectedPosition << std::endl;

    REQUIRE(abs((roundf(expectedPosition.getLatitude()*100)/100)-testPosition.getLatitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getLongitude()*100)/100)-testPosition.getLongitude())<1e-10);
    REQUIRE(abs((roundf(expectedPosition.getEllipsoidalHeight()*100)/100)-testPosition.getEllipsoidalHeight())<1e-10);
}

/**Test if the conversion between spherical to cartesian works*/
TEST_CASE("Conversion between spherical to cartesian"){
    Eigen::Vector3d v;

    CoordinateTransform::spherical2cartesian(v,45,45,1);

    REQUIRE(abs(v(0) - 0.5) < 0.000000001);
    REQUIRE(abs(v(1) - 0.5) < 0.000000001);
    REQUIRE(abs(v(2) - 0.707107) < 0.000001);
}

/**Test if the conversion between sonar to cartesian works*/
TEST_CASE("Conversion between sonar to cartesian"){
    Eigen::Vector3d v;
    CoordinateTransform::sonar2cartesian(v,90,0,1);
    REQUIRE(std::abs(v(0)-1)<1e-10);
    REQUIRE(std::abs(v(1)-0)<1e-10);
    REQUIRE(std::abs(v(2)-0)<1e-10);
    CoordinateTransform::sonar2cartesian(v,0,90,1);
    REQUIRE(std::abs(v(0)-0)<1e-10);
    REQUIRE(std::abs(v(1)-1)<1e-10);
    REQUIRE(std::abs(v(2)-0)<1e-10);
    CoordinateTransform::sonar2cartesian(v,0,0,1);
    REQUIRE(std::abs(v(0)-0)<1e-10);
    REQUIRE(std::abs(v(1)-0)<1e-10);
    REQUIRE(std::abs(v(2)-1)<1e-10);
}

/**Test if the conversion between NED to ECEF position works*/
TEST_CASE("Conversion between NED to ECEF position")
{
    Position nedPos(0,0,0,0);
    Eigen::Matrix3d mECEF;
    CoordinateTransform::ned2ecef(mECEF,nedPos);
    Eigen::Matrix3d matrixSearch;
    matrixSearch << 0,0,-1,
                    0,1,0,
                    1,0,0;
    REQUIRE(mECEF == matrixSearch);

    nedPos = Position(0,16,19,0);
    CoordinateTransform::ned2ecef(mECEF,nedPos);
    matrixSearch << -0.260620240054051,-0.325568154457157,-0.908890789521783,
                    -0.0897387452327910,0.945518575599317,-0.312956196296995,
                    0.961261695938319,0,-0.275637355816999;
    REQUIRE(abs(mECEF(0,0)-matrixSearch(0,0))< 1e-10);
    REQUIRE(abs(mECEF(0,1)-matrixSearch(0,1))< 1e-10);
    REQUIRE(abs(mECEF(0,2)-matrixSearch(0,2))< 1e-10);
    REQUIRE(abs(mECEF(1,0)-matrixSearch(1,0))< 1e-10);
    REQUIRE(abs(mECEF(1,1)-matrixSearch(1,1))< 1e-10);
    REQUIRE(abs(mECEF(1,2)-matrixSearch(1,2))< 1e-10);
    REQUIRE(abs(mECEF(2,0)-matrixSearch(2,0))< 1e-10);
    REQUIRE(abs(mECEF(2,1)-matrixSearch(2,1))< 1e-10);
    REQUIRE(abs(mECEF(2,2)-matrixSearch(2,2))< 1e-10);
}

/**Test if the conversion between Terrestial to Geodetic works*/
TEST_CASE("Conversion between Terrestial to Geodetic")
{
    Position pos(0,0,0,0);
    Eigen::Matrix3d mGeo;
    CoordinateTransform::getTerrestialToLocalGeodeticReferenceFrameMatrix(mGeo,pos);
    Eigen::Matrix3d matrixSearch;
    matrixSearch << 0,0,1,
                    0,1,0,
                    -1,0,0;
    REQUIRE(mGeo == matrixSearch);

    pos = Position(0,45,56,0);
    CoordinateTransform::getTerrestialToLocalGeodeticReferenceFrameMatrix(mGeo,pos);
    matrixSearch << -0.395409094035560,-0.586218089412104,0.707106781186548,
                    -0.829037572555042,0.559192903470747,0,
                    -0.395409094035560,-0.586218089412104,-0.707106781186548;
    REQUIRE(abs(mGeo(0,0)-matrixSearch(0,0))< 1e-10);
    REQUIRE(abs(mGeo(0,1)-matrixSearch(0,1))< 1e-10);
    REQUIRE(abs(mGeo(0,2)-matrixSearch(0,2))< 1e-10);
    REQUIRE(abs(mGeo(1,0)-matrixSearch(1,0))< 1e-10);
    REQUIRE(abs(mGeo(1,1)-matrixSearch(1,1))< 1e-10);
    REQUIRE(abs(mGeo(1,2)-matrixSearch(1,2))< 1e-10);
    REQUIRE(abs(mGeo(2,0)-matrixSearch(2,0))< 1e-10);
    REQUIRE(abs(mGeo(2,1)-matrixSearch(2,1))< 1e-10);
    REQUIRE(abs(mGeo(2,2)-matrixSearch(2,2))< 1e-10);
}

/**Test if CoordinateTransform get a valid DCM*/
TEST_CASE("getDCM Test") {

    Eigen::Vector3d testVector = Eigen::Vector3d::Ones();

    Attitude attitudeZero(0, 0, 0, 0);
    Eigen::Matrix3d dcmAttitudeZero;
    CoordinateTransform::getDCM(dcmAttitudeZero,attitudeZero);
    REQUIRE(dcmAttitudeZero.isIdentity());

    // roll 180 degrees
    Attitude attitudeRoll(0, 180, 0, 0);
    Eigen::Matrix3d dcmFromAttitudeRollTest;
    CoordinateTransform::getDCM(dcmFromAttitudeRollTest,attitudeRoll);
    Eigen::Vector3d testRollVector = dcmFromAttitudeRollTest * testVector;

    double testRollPrecision = 0.000000001;
    REQUIRE(abs(testRollVector(0) - 1.0) < testRollPrecision);
    REQUIRE(abs(testRollVector(1) - -1.0) < testRollPrecision);
    REQUIRE(abs(testRollVector(2) - -1.0) < testRollPrecision);

    // pitch 180 degrees
    Attitude attitudePitch(0, 0, 180, 0);
    Eigen::Matrix3d dcmFromAttitudePitchTest;
    CoordinateTransform::getDCM(dcmFromAttitudePitchTest,attitudePitch);
    Eigen::Vector3d testPitchVector = dcmFromAttitudePitchTest * testVector;

    double testPitchPrecision = 0.000000001;
    REQUIRE(abs(testPitchVector(0) - -1.0) < testPitchPrecision);
    REQUIRE(abs(testPitchVector(1) - 1.0) < testPitchPrecision);
    REQUIRE(abs(testPitchVector(2) - -1.0) < testPitchPrecision);

    // heading 180 degrees
    Attitude attitudeHeading(0, 0, 0, 180);
    Eigen::Matrix3d dcmFromAttitudeHeadingTest;
    CoordinateTransform::getDCM(dcmFromAttitudeHeadingTest,attitudeHeading);
    Eigen::Vector3d testHeadingVector = dcmFromAttitudeHeadingTest * testVector;

    double testHeadingPrecision = 0.000000001;
    REQUIRE(abs(testHeadingVector(0) - -1.0) < testHeadingPrecision);
    REQUIRE(abs(testHeadingVector(1) - -1.0) < testHeadingPrecision);
    REQUIRE(abs(testHeadingVector(2) - 1.0) < testHeadingPrecision);

    // random attitude
    double roll = 24.357;
    double pitch = 36.54;
    double heading = 157.43;
    Attitude attitudeRandom(0,roll, pitch, heading);
    Eigen::Matrix3d dcmFromAttitudeRandomTest;
    CoordinateTransform::getDCM(dcmFromAttitudeRandomTest,attitudeRandom);

    Eigen::Matrix3d dcmFromEulerAngles;
    dcmFromEulerAngles = Eigen::AngleAxisd(heading*D2R, Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(pitch*D2R, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(roll*D2R, Eigen::Vector3d::UnitX());

    double randomPrecision = 0.000000001;
    Eigen::Matrix3d dcmDifferences = dcmFromAttitudeRandomTest - dcmFromEulerAngles;
    dcmDifferences = dcmDifferences.cwiseAbs();
    REQUIRE(dcmDifferences.maxCoeff() < randomPrecision);
};

TEST_CASE("Test getPositionInNavigationFrame function")
{
    Eigen::Vector3d posNav;
    Position pos(0,0,0,0);
    Eigen::Matrix3d dcm;
    dcm << 0,0,0,
           0,0,0,
           0,0,0;
    Eigen::Vector3d origin(0,0,0);
    CoordinateTransform::getPositionInNavigationFrame(posNav,pos,dcm,origin);
    REQUIRE(posNav(0) == 0);
    REQUIRE(posNav(1) == 0);
    REQUIRE(posNav(2) == 0);

    pos = Position(0,180,180,0);
    dcm << 1,0,0,
           0,1,0,
           0,0,1;
    origin(0) = 0;
    origin(1) = 0;
    origin(2) = 0;
    CoordinateTransform::getPositionInNavigationFrame(posNav,pos,dcm,origin);
    Eigen::Vector3d vectorSearch(6378137,-0.0000000008,0.0000000008);
    REQUIRE(abs(posNav(0) - vectorSearch(0))< 1e-10);
    REQUIRE(abs(posNav(1) - vectorSearch(1))< 1e-10);
    REQUIRE(abs(posNav(2) - vectorSearch(2))< 1e-10);
}


// system includes ------------------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/MPRealSupport>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <iomanip>
#include <iostream>
//#include <quadmath.h>

// own includes ---------------------------------------------------------------
#include "base/numbers.hpp"
#include "maxwell_quadrature.hpp"
#include "spectral/mpfr/import_std_math.hpp"

namespace mp = boost::multiprecision;  // Reduce the typing a bit later...

using namespace mpfr;

typedef mp::mpfr_float_backend<100000> mfloat_t;
typedef mp::number<mfloat_t> mpfr_float_t;

using namespace std;

const mpfr_float_t PI = boost::math::constants::pi<mpfr_float_t>();
const mpfr_float_t rPI = boost::math::constants::root_pi<mpfr_float_t>();

template <typename T>
class Beta
{
 public:
  Beta(int p = 1)
      : p_(p)
      , beta_n(1)
  {
    beta_n[0] = 0;
    navail = 0;
  }

  template <typename A>
  const T& get(unsigned int n, A& alpha);

 private:
  int p_;
  std::vector<T> beta_n;
  unsigned int navail;
};

// ----------------------------------------------------------------------
template <typename T>
template <typename A>
const T& Beta<T>::get(unsigned int n, A& alpha)
{
  if (n < beta_n.size())
    return beta_n[n];
  else {
    // make sure all previous n's exist
    for (unsigned int i = navail + 1; i < n; ++i) {
      this->get(i, alpha);
    }
    // compute
    T am = alpha.get(n - 1, *this);
    T an = alpha.get(n, *this);
    T bm = this->get(n - 1, alpha);
    T denom = 2 + 4 * am * (am + an) + 4 * bm;
    T nom = n * (n + p_);
    T res = nom / denom;
    beta_n.push_back(res);
    navail++;
    assert(navail == n);
    assert(beta_n[n] == res);
    return beta_n[n];
  }
}

// ----------------------------------------------------------------------
template <typename T>
class Alpha
{
 public:
  Alpha(int p = 1)
      : p_(p)
      , alpha_n(2)
  {
    mpfr_float_t one(1);
    mpfr_float_t two(2);
    mpfr_float_t three(3);
    mpfr_float_t four(4);
    mpfr_float_t eight(8);

    if (p == 0) {
      alpha_n[0] = one / rPI;
      alpha_n[1] = two / rPI / (PI - two);
    } else if (p == 1) {
      alpha_n[0] = rPI / two;
      alpha_n[1] = rPI * (PI - two) / two / (four - PI);
    } else if (p == 2) {
      alpha_n[0] = two / rPI;
      alpha_n[1] = four * (four - PI) / rPI / (three * PI - eight);
    } else {
      throw std::runtime_error("invalid p");
    }

    navail = 1;
  }
  template <typename B>
  const T& get(unsigned int n, B& beta);

 private:
  int p_;
  std::vector<T> alpha_n;
  unsigned int navail;
};

// ----------------------------------------------------------------------
template <typename T>
template <typename B>
const T& Alpha<T>::get(unsigned int n, B& beta)
{
  if (n < alpha_n.size())
    return alpha_n[n];
  else {
    // make sure all previous n's exist
    for (unsigned int i = navail + 1; i < n; ++i) {
      this->get(i, beta);
    }
    // use recursion formula (15)
    // compute
    T sum = 0;
    T am = this->get(n - 1, beta);
    T am2 = am * am;
    for (unsigned int k = 0; k < n; ++k) {
      sum += this->get(k, beta);
    }
    sum /= (2 * n - 1 + p_ - 2 * am2 - 2 * beta.get(n - 1, *this));
    sum -= am;
    alpha_n.push_back(sum);
    navail++;
    assert(navail == n);
    assert(alpha_n[n] == sum);
    return alpha_n[n];
  }
}

void MaxwellQuadrature::init(int ndigits)
{
  // recurrence relation using mpfr_float_t
  typedef Alpha<mpfr_float_t> alpha_t;
  typedef Beta<mpfr_float_t> beta_t;

  alpha_t alpha(p);
  beta_t beta(p);

  // eigen with mpreal
  mpreal::set_default_prec(ndigits);
  typedef mpreal mfloat_t;
  //  typedef double mfloat_t;
  typedef Eigen::Matrix<mfloat_t, Eigen::Dynamic, Eigen::Dynamic> MatrixXmp;
  MatrixXmp A(N, N);
  A.fill(0);
  for (int i = 0; i < N; ++i) {
    A(i, i) = mfloat_t(alpha.get(i, beta).backend().data()).setPrecision(ndigits);
    const auto sqrt_beta1 = ::math::sqrt(beta.get(i + 1, alpha));
    const auto sqrt_beta0 = ::math::sqrt(beta.get(i, alpha));

    if (i + 1 < N) A(i, i + 1) = mfloat_t(sqrt_beta1.backend().data()).setPrecision(ndigits);
    if (i > 0) A(i, i - 1) = mfloat_t(sqrt_beta0.backend().data()).setPrecision(ndigits);
  }

  Eigen::SelfAdjointEigenSolver<MatrixXmp> eigensolver;
  eigensolver.compute(A, Eigen::ComputeEigenvectors);

  // compute weights
  const auto V = eigensolver.eigenvectors();
  const auto w = eigensolver.eigenvalues();

  if (V.rows() == 0) {
    cerr << "abort";
    exit(-1);
  }

  const double pi = boltzmann::numbers::PI;
  std::array<double, 3> mp = {std::sqrt(pi) / 2, 0.5, std::sqrt(pi) / 4};

  std::vector<double> weights(N);
  for (int i = 0; i < N; ++i) {
    // eigenvectors are normalized by eigen
    auto v = V(0, i);
    weights[i] = mp[p] * (v * v).toDouble();
  }

  std::vector<double> points(N);
  for (int i = 0; i < N; ++i) {
    points[i] = w(i).toDouble();
  }

  std::vector<std::pair<double, double> > pairs(N);
  for (int i = 0; i < N; ++i) {
    pairs[i] = std::make_pair(points[i], weights[i]);
  }

  std::sort(pairs.begin(), pairs.end());

  for (int i = 0; i < N; ++i) {
    pts_[i] = pairs[i].first;
    wts_[i] = pairs[i].second;
  }
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

// BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

int main(int argc, char **argv) {
  if (argc != 5) {
    cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  // 建立3D点
  Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
  Mat R;
  cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }

  cout << "calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

  cout << "calling bundle adjustment by g2o" << endl;
  Sophus::SE3d pose_g2o;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
  return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }

  cout << "pose by g-n: \n" << pose.matrix() << endl;
}

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }

  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate();
}


#include <cassert>
#include <cmath>
#include <numeric> // accumulate
#include <ostream>
#include <utility>

#include <boost/variant/static_visitor.hpp>

#include "Geometry.h"

using namespace std;

namespace geometry {

Scalar size(Vector const &vec) { return std::hypot(vec.x, vec.y); }

// Dot product
Scalar dot(Vector const &v1, Vector const &v2) {
  return v1.x * v2.x + v1.y * v2.y;
}

Scalar cos(Vector const &a, Vector const &b) {
  return dot(a, b) / (size(a) * size(b));
}

Vector projection(Vector const &v, Vector const &s) {
  return ((v * s) / (s * s)) * s;
}

Scalar projection(Point const &p, Point const &A, Point const &B) {
  // http://stackoverflow.com/a/15187473/6209703
  Vector const direction = B - A;
  Scalar const t = ((p.x - A.x) * (B.x - A.x) + (p.y - A.y) * (B.y - A.y)) /
                   (direction.x * direction.x + direction.y * direction.y);
  // t=[(Cx-Ax)(Bx-Ax)+(Cy-Ay)(By-Ay)]/[(Bx-Ax)^2+(By-Ay)^2]
  return t;
}

Vector unit(Vector const &v) { return v / size(v); }

Scalar distance(Point const &p1, Point const &p2) {
  auto d = p2 - p1;
  return Scalar(std::hypot(d.x, d.y));
}

std::ostream &operator<<(std::ostream &o, Point const &point) {
  o << "[" << point.x << "," << point.y << "]";
  return o;
}

std::ostream &operator<<(std::ostream &o, Vector const &point) {
  o << "(" << point.x << "," << point.y << ")";
  return o;
}

std::ostream &operator<<(std::ostream &o, CircleShape const &shape) {
  o << "{ radius: " << shape.radius << " }";
  return o;
}

std::ostream &operator<<(std::ostream &o, Circle const &c) {
  o << "{ circ at " << c.center << ", r: " << c.radius << " }";
  return o;
}

PolygonalShape &operator+=(PolygonalShape &shape, Vector const &vec) {
  for (auto &v : shape)
    v += vec;
  return shape;
}

PolygonalShape &operator*=(PolygonalShape &shape, Scalar scale) {
  for (auto &v : shape)
    v *= scale;
  return shape;
}

PolygonalShape &operator-=(PolygonalShape &shape, Vector const &vec) {
  for (auto &v : shape)
    v -= vec;
  return shape;
}

std::ostream &operator<<(std::ostream &o, PolygonalShape const &shape) {
  o << "[";
  for (auto const &vec : shape)
    o << " " << vec;
  o << " ]";
  return o;
}

std::ostream &operator<<(std::ostream &o, Polygon const &p) {
  o << "[";
  for (auto const &pt : p)
    o << " " << pt;
  o << " ]";
  return o;
}

std::ostream &operator<<(std::ostream &o, RectangularArea const &area) {
  o << "{ area from " << area.topLeft() << " to " << area.bottomRight() << " }";
  return o;
}

bool in_triangle(Point const &p, Point const &p1, Point const &p2,
                 Point const &p3) {
  auto alpha = ((p2.y - p3.y) * (p.x - p3.x) + (p3.x - p2.x) * (p.y - p3.y)) /
               ((p2.y - p3.y) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.y - p3.y));
  auto beta = ((p3.y - p1.y) * (p.x - p3.x) + (p1.x - p3.x) * (p.y - p3.y)) /
              ((p2.y - p3.y) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.y - p3.y));
  auto gamma = 1.0f - alpha - beta;

  return alpha >= 0 && beta >= 0 && gamma >= 0;
}

Vector perpendicularDistance(Point const &p, Point const &A, Point const &B) {
  Scalar const t = projection(p, A, B);
  Point const projectedCenter = A + t * (B - A);
  return p - projectedCenter;
}

bool onLeftOf(Point const &point, Point const &A, Point const &B) {
  return ((A.x - point.x) * (B.y - point.y) -
          (A.y - point.y) * (B.y - point.x)) > 0;
}

// http://stackoverflow.com/a/21096006/6209703
bool in_polygon(Point const &point, Polygon const &polygon_old) {
  Polygon polygon = polygon_old;

  assert(polygon.size() >= 3);

  while (polygon.size() > 3) {
    // TODO detect automatically polygons rotation/order of verteces
    Polygon newPolygon;
    if (onLeftOf(point, polygon[0], polygon[polygon.size() / 2])) {
      for (size_t i = 0; i <= polygon.size() / 2; i++)
        newPolygon.push_back(polygon[i]);
    } else {
      for (size_t i = polygon.size() / 2; i < polygon.size(); i++)
        newPolygon.push_back(polygon[i]);
      newPolygon.push_back(polygon[0]);
    }

    assert(newPolygon.size() < polygon.size());
    using std::swap;
    swap(newPolygon, polygon);
  }

  return in_triangle(point, polygon[0], polygon[1], polygon[2]);
}

bool in_polygon(Circle const &circle, Polygon const &polygon) {
  for (Point const &p : polygon) {
    Vector const direction = unit(p - circle.center);
    Point const outer = circle.center + circle.radius * direction;
    if (!in_polygon(outer, polygon))
      return false;
  }
  return true;
}

bool in_polygon(Polygon const &polygon1, Polygon const &polygon2) {
  for (Point const &p : polygon1) {
    if (!in_polygon(p, polygon2))
      return false;
  }
  return true;
}

bool in_circle(Point const &point, Circle const &circle) {
  auto vec = point - circle.center;
  return std::hypot(vec.x, vec.y) <= circle.radius;
}

bool in_circle(Circle const & /*circle1*/, Circle const & /*circle2*/) {
  return false;
}

bool in_circle(Polygon const &polygon, Circle const &circle) {
  for (Point const &p : polygon) {
    if (!in_circle(p, circle))
      return false;
  }
  return true;
}

bool in(Point const &point, RectangularArea const &area) {
  if (point.x <= area.topLeft().x)
    return false;
  if (point.x >= area.bottomRight().x)
    return false;
  if (point.y <= area.topLeft().y)
    return false;
  if (point.y >= area.bottomRight().y)
    return false;
  return true;
}

bool in(Circle const &circle, RectangularArea const &area) {
  return in(circle.center, CircleShape{circle.radius}, area);
}

bool in(Point const &point, CircleShape const &shape,
        RectangularArea const &area) {
  if (point.x - shape.radius <= area.topLeft().x)
    return false;
  if (point.x + shape.radius >= area.bottomRight().x)
    return false;
  if (point.y - shape.radius <= area.topLeft().y)
    return false;
  if (point.y + shape.radius >= area.bottomRight().y)
    return false;
  return true;
}

bool in(Polygon const &polygon, RectangularArea const &area) {
  // TODO use accumulate with ranges
  for (auto const &p : polygon) {
    if (!in(p, area))
      return false;
  }

  return true;
}

namespace {

template <typename Object1>
class InVisitor1 : public boost::static_visitor<bool> {
  Object1 const &object1;

public:
  explicit InVisitor1(Object1 const &object1) : object1(object1) {}

  bool operator()(Polygon const &object2) {
    return in_polygon(object1, object2);
  }

  bool operator()(Circle const &object2) { return in_circle(object1, object2); }
};

class InVisitor2 : public boost::static_visitor<bool> {
  Object2D const &object2;

public:
  explicit InVisitor2(Object2D const &object2) : object2(object2) {}

  bool operator()(Polygon const &object1) {
    InVisitor1<Polygon> visitor{object1};
    return boost::apply_visitor(visitor, object2);
  }

  bool operator()(Circle const &object1) {
    InVisitor1<Circle> visitor{object1};
    return boost::apply_visitor(visitor, object2);
  }
};

} // anonymous namespace

bool in(Object2D const &object1, Object2D const &object2) {
  InVisitor2 visitor{object2};
  return boost::apply_visitor(visitor, object1);
}

bool in(Object2D const &object, RectangularArea const &area) {
  class Visitor : public boost::static_visitor<bool> {
  private:
    RectangularArea const &area;

  public:
    Visitor(RectangularArea const &area) : area(area) {}

    bool operator()(Polygon const &polygon) { return in(polygon, area); }

    bool operator()(Circle const &circle) { return in(circle, area); }
  };

  Visitor visitor{area};
  return boost::apply_visitor(visitor, object);
}

Scalar deg2rad(Scalar degrees) { return degrees * 3.141592653589793 / 180.0; }

Vector rotate(Vector const &vec, Angle angle) {
  auto const dangle = deg2rad(angle);
  auto const cs = Scalar(std::cos(dangle));
  auto const sn = Scalar(std::sin(dangle));

  return vec.x * Vector{cs, sn} + vec.y * Vector{Scalar(-sn), cs};
}

Point rotate_around(Point const &center, Point const &point, Angle angle) {
  return center + rotate(point - center, angle);
}

} // namespace geometry


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

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

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <standards/simpson.hpp>
#include <lattice/graph.hpp>
#include "common.hpp"

namespace ising {
namespace free_energy {
namespace square {

namespace {

template<typename T, typename FVAR>
struct functor {
  typedef T real_t;
  typedef FVAR fvar_t;
  functor(real_t Jx, real_t Jy, fvar_t beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<real_t>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  fvar_t operator()(real_t t) const {
    using std::cos; using std::log; using std::sqrt;
    return c_ * log(chab_ + sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_);
  }
  real_t c_;
  fvar_t chab_, k_;
};

template<typename T, std::size_t Order>
struct functor<T, boost::math::differentiation::detail::fvar<T, Order>> {
  typedef T real_t;
  typedef boost::math::differentiation::detail::fvar<real_t, Order> fvar_t;
  functor(real_t Jx, real_t Jy, fvar_t beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<real_t>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  fvar_t operator()(real_t t) const {
    using std::abs; using std::cos; using std::log; using std::sqrt;
    auto r = sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_;
    if (abs(r.derivative(1)) > std::numeric_limits<real_t>::max()) {
      auto x = boost::math::differentiation::make_fvar<real_t, 2>(real_t(0));
      r = fvar_t(r.derivative(0)) + std::numeric_limits<real_t>::max() * x * x;
    }
    return c_ * log(chab_ + r);
  }
  real_t c_;
  fvar_t chab_, k_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Jx, T Jy, FVAR beta) { return functor<T, FVAR>(Jx, Jy, beta); }

}

template<typename T, typename U>
inline U infinite(T Jx, T Jy, U beta) {
  const unsigned long max_n = 1 << 16;
  typedef T real_t;
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0)
    throw(std::invalid_argument("beta should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  boost::math::quadrature::tanh_sinh<real_t> integrator;
  auto logZ = log(real_t(2)) / 2 + 2 * integrator.integrate(func(Jx, Jy, beta), 0, pi / 2);
  return - logZ / beta;
}

template<typename I, typename T, typename U>
inline U finite(I Lx, I Ly, T Jx, T Jy, U beta) {
  typedef I int_t;
  typedef T real_t;
  typedef U value_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  auto a = beta * Jx;
  auto b = beta * Jy;
  value_t lp0(0), lp1(0), lp2(0), lp3(0);
  auto gamma0 = log((1 + cosh(2*a)) / sinh(2*a)) - 2*b;
  for (int_t k = 0; k < 2 * Ly; ++k) {
    auto cosh_g = (cosh(2*a) * cosh(2*b) - cos(pi * k / Ly) * sinh(2*b)) / sinh(2*a);
    auto gamma = (k == 0) ? abs(gamma0) : (log(cosh_g + sqrt(cosh_g * cosh_g - 1)));
    if ((k & 1) == 1) {
      lp0 += (Lx * gamma/2) + log(1 + exp(-(Lx * gamma)));
      lp1 += (Lx * gamma/2) + log(1 - exp(-(Lx * gamma)));
    } else {
      lp2 += (Lx * gamma/2) + log(1 + exp(-(Lx * gamma)));
      lp3 += (Lx * gamma/2) + log(1 - exp(-(Lx * gamma)));
    }
  }
  auto logZ = -log(real_t(2)) / (Lx * Ly) + a + real_t(1)/2 * log(1 - exp(-4*a));
  if (gamma0 > 0) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) - exp(lp3-lp0))) / (Lx * Ly);
  } else if (gamma0 < 0) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) + exp(lp3-lp0))) / (Lx * Ly);
  } else {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0))) / (Lx * Ly);
  }
  return - logZ / beta;
}

template<typename T, typename I>
inline T finite_tc(I Lx, I Ly) {
  typedef I int_t;
  typedef T real_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  real_t beta = log(sqrt(real_t(2)) + 1)/2;
  real_t lp0(0), lp1(0);
  for (int_t k = 1; k < 2 * Ly; k += 2) {
    real_t c = (2 - cos(pi * k / Ly));
    real_t cosh_g = c;
    real_t gamma_abs = log(cosh_g + sqrt(cosh_g * cosh_g - 1));
    lp0 += (Lx * gamma_abs/2) + log(1 + exp(-(Lx * gamma_abs)));
    lp1 += (Lx * gamma_abs/2) + log(1 - exp(-(Lx * gamma_abs)));
  }
  real_t logZ = -log(real_t(2)) / (Lx * Ly) + beta + real_t(1)/2 * log(1 - exp(-4*beta))
    + (lp0 + 2 * lp1) / (Lx * Ly)
    + (lp0 + log(1 + 2 * exp(lp1-lp0))) / (Lx * Ly);
  return - logZ / beta;
}

template<typename I, typename T, typename U, typename W>
inline boost::math::differentiation::promote<U, W> finite_count(I Lx, I Ly, T Jx, T Jy, U beta, W h) {
  typedef I int_t;
  typedef T real_t;
  typedef boost::math::differentiation::promote<U, W> value_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));

  auto basis = lattice::basis::simple(2);
  auto unitcell = lattice::unitcell(2);
  unitcell.add_site(lattice::coordinate(0, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(1, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(0, 1), 1);
  auto graph = lattice::graph(basis, unitcell, lattice::extent(Lx, Ly));
  if (graph.num_sites() >= 32) throw(std::invalid_argument("too large system size"));

  std::vector<real_t> J(2);
  J[0] = Jx;
  J[1] = Jy;
  real_t gs_energy = -(Jx + Jy) * (Lx * Ly);
  int_t num_states = 1 << graph.num_sites();
  value_t Z(0);
  for (int_t c = 0; c < num_states; ++c) {
    real_t energy = 0;
    for (int_t b = 0; b < graph.num_bonds(); ++b) {
      real_t ci = real_t(2) * ((c >> graph.source(b)) & 1) - 1;
      real_t cj = real_t(2) * ((c >> graph.target(b)) & 1) - 1;
      energy -= J[graph.bond_type(b)] * ci * cj;
    }
    real_t mag = 0;
    for (int_t s = 0; s < graph.num_sites(); ++s) {
      mag += real_t(2) * ((c >> s) & 1) - 1;
    }
    Z += exp((gs_energy - energy - h * mag) * beta);
  }
  return (-log(Z)/beta + gs_energy) / (Lx * Ly);
}

} // end namespace square
} // end namespace free_energy
} // end namespace ising


#include <vector>
#include <armadillo>
#include "ConvectionDiffusionEulerImplicit.h"

using namespace std;

void ConvectionDiffusionEulerImplicit::initialize() {
	// Initial time conditions
	current_time = 0.0; // time 0 = option expiration - more pos = farther in the past
	current_t_index = 0;

	// Proccessing step
	solution.resize(T);
    for (unsigned long t = 0; t < T; t++) { solution[t].resize(N,0.0); }
	x_values.resize(N,0.0);

	// Initialize the solution set at all x values
	double x;
	for (unsigned long n = 0; n < N; n++) {
		x = static_cast<double>(n) * h;
		solution[0][n] = pde->initial_condition(x);
		x_values[n] = x;
	}
}

// Compute PDE's boundary conditions
void ConvectionDiffusionEulerImplicit::calculate_boundary() {
	// double time = current_time - k;
	solution[current_t_index][0] = pde->left_boundary(current_time, x_values, solution[current_t_index-1],k,h);
	solution[current_t_index][N-1] = pde->right_boundary(current_time, x_values, solution[current_t_index-1],k,h);
}

// Apply FEM to solve system of PDEs with given initial and boundary conditions
void ConvectionDiffusionEulerImplicit::calculate_inner_mesh() {
	double right, center, left, source, x, prev_time = current_time - k;
	double sigma = k / (h * h);
	double lambda = k / h;

	// INIT TRANSITION MATRIX
	arma::mat A = arma::zeros<arma::mat>(N-2,N-2);
	for (unsigned long n = 1; n < N-1; n++) {
		x = x_values[n];

		A(n-1,n-1) = 1.0
		+ (2.0 * sigma * pde->diffusion_param(current_time,x))
		- (k * pde->solution_param(current_time,x));

		if (n < N-2) {
			A(n-1,n) =  - (sigma * pde->diffusion_param(current_time,x))
            - (0.5 * lambda * pde->convection_param(current_time,x));
		}
        if (n > 1) {
			A(n-1,n-2) = - (sigma * pde->diffusion_param(current_time,x))
			+ (0.5 * lambda * pde->convection_param(current_time,x));
		}
	}

	// INIT BOUNDARY VECTOR
	arma::vec b = arma::zeros<arma::vec>(N-2);

	left = - (sigma * pde->diffusion_param(current_time,x_values[0])) + (0.5 * lambda * pde->convection_param(current_time,x_values[0]));
    b(0) = left * pde->left_boundary(current_time, x_values,solution[current_t_index-1],k,h);

	right = - (sigma * pde->diffusion_param(current_time,x_values[N-1])) - (0.5 * lambda * pde->convection_param(current_time,x_values[N-1]));
    b(N-3) = right * pde->right_boundary(current_time, x_values,solution[current_t_index-1],k,h);

	// INIT SOURCE VECTOR
	arma::vec S = arma::zeros<arma::vec>(N-2);
	for (unsigned long n = 1; n < N-1; n++) {
		S(n-1) = k * pde->source_param(current_time,x_values[n]);
	}

	// LAST SOLUTION
	arma::vec u = arma::zeros<arma::vec>(N-2);
	for (unsigned long n = 1; n < N-1; n++) {
        u(n-1) = solution[current_t_index-1][n];
	}

	// SOLVE
    arma::vec sol = arma::solve(A, u + b + S);

	// STORE
	for (unsigned long n = 1; n < N-1; n++) {
		solution[current_t_index][n] = sol(n-1);
	}
}

// Define the solve function for a single PDE
void ConvectionDiffusionEulerImplicit::solve() {
	while (current_time < t_bound) {
		current_time += k;
		current_t_index++;
		calculate_boundary();
		calculate_inner_mesh();
	}
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2016.
//  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)
//

// Example program showing a bare metal real-time performance measurement
// of a shift-and-add variation of sqrt(negatable), 32-bit.

#include <cstdint>
#include <limits>

#include <mcal_benchmark.h>
#include <mcal_cpu.h>
#include <mcal_irq.h>

#define BOOST_FIXED_POINT_DISABLE_IOSTREAM
#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

#include <boost/fixed_point/fixed_point.hpp>

typedef mcal::benchmark::benchmark_port_type port_type;

typedef boost::fixed_point::negatable<7, -24> fixed_point_type;

extern const fixed_point_type x;
extern       fixed_point_type y;

namespace shift
{
  fixed_point_type sqrt(const fixed_point_type& x);

  // Computes the fixed-point square root using a shift-and-add algorithm.
  // Adapted from sqrt found here: http://codepad.org/CqvPx5u0
  // And this has beem modified from Ken Turkowski's algorithm.
  // Despite the somewhat confusing description of the result in the white paper,
  // the algorithm builds a M.F fixed-point square root.
  // @see http://www.realitypixels.com/turk/computergraphics/FixedSqrt.pdf
  fixed_point_type sqrt(const fixed_point_type& x)
  {
    typedef fixed_point_type::unsigned_small_type local_unsigned_small_type;
    typedef fixed_point_type::nothing             local_nothing;

    constexpr std::uint_fast8_t total_bits      = static_cast<std::uint_fast8_t>(fixed_point_type::all_bits);
    constexpr std::uint_fast8_t fractional_bits = static_cast<std::uint_fast8_t>(fixed_point_type::radix_split);

    local_unsigned_small_type root  = static_cast<local_unsigned_small_type>(0U);                  // Clear the root.
    local_unsigned_small_type remHi = static_cast<local_unsigned_small_type>(0U);                  // Clear the high part of the partial remainder.
    local_unsigned_small_type remLo = static_cast<local_unsigned_small_type>(x.crepresentation()); // Get the argument into the low part of the partial remainder.

    for(std::uint_fast8_t count = (total_bits >> 1) + (fractional_bits >> 1); count > 0U; --count)
    {
      // Get 2 bits of the argument.
      remHi = (remHi << 2) | (remLo >> (total_bits - 2));
      remLo <<= 2;

      // Get ready for the next bit in the root.
      root <<= 1;

      // Test the radical.
      const local_unsigned_small_type testDiv = (root << 1) + 1;

      if(remHi >= testDiv)
      {
        remHi = remHi - testDiv;

        ++root;
      }
    }

    return fixed_point_type(local_nothing(), root);
  }

} // namespace shift

namespace app
{
  namespace benchmark
  {
    void task_init();
    void task_func();
  }
}

void app::benchmark::task_init()
{
  port_type::set_direction_output();
}

void app::benchmark::task_func()
{
  mcal::irq::disable_all();
  port_type::set_pin_high();

  // Use either one of the following two lines (but not both)
  // to benchmark variations of square root algorithms.

  y = shift::sqrt(x);
  //y = boost::fixed_point::sqrt(x);

  port_type::set_pin_low();

  mcal::irq::enable_all();
}

const fixed_point_type x(fixed_point_type(6U) /  10U);
      fixed_point_type y;


#define BOOST_TEST_MODULE "test_vector_matrix"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../LinearAlgebra.hpp"
using Vector3d = ax::Vector<double, 3>;
using Vector4d = ax::Vector<double, 4>;
using Matrix3  = ax::Matrix3d;
using Matrix4  = ax::Matrix4d;

#include "test_Defs.hpp"
using ax::test::tolerance;
using ax::test::seed;

#include <random>

BOOST_AUTO_TEST_CASE(vector3d_matrix3x3_multiplication)
{
    const Vector3d vec1(1e0, 2e0, 3e0);
    const Matrix3  mat1(1e0);
    const Vector3d vec2 = mat1 * vec1;

    BOOST_CHECK_EQUAL(vec2[0], vec1[0]);
    BOOST_CHECK_EQUAL(vec2[1], vec1[1]);
    BOOST_CHECK_EQUAL(vec2[2], vec1[2]);

    const Matrix3  mat2(2e0);
    const Vector3d vec3 = mat2 * vec1;

    BOOST_CHECK_EQUAL(vec3[0], 2e0 * vec1[0]);
    BOOST_CHECK_EQUAL(vec3[1], 2e0 * vec1[1]);
    BOOST_CHECK_EQUAL(vec3[2], 2e0 * vec1[2]);

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    Matrix3  mat3;
    std::array<std::array<double, 3>, 3> random1;
    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            random1[i][j] = randreal(mt);

    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            mat3(i, j) = random1[i][j];

    const Vector3d ones(1e0, 1e0, 1e0);
    const Vector3d vec4 = mat3 * ones;
    for(std::size_t i = 0; i < 3; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 3; ++j)
            value += random1[i][j];
        BOOST_CHECK_EQUAL(vec4[i], value);
    }
}

BOOST_AUTO_TEST_CASE(matrix3x3_vector3d_multiplication)
{
    const Vector3d vec1(1e0, 2e0, 3e0);
    const Matrix3  mat1(1e0);
    const Vector3d vec2 = vec1 * mat1;

    BOOST_CHECK_EQUAL(vec2[0], vec1[0]);
    BOOST_CHECK_EQUAL(vec2[1], vec1[1]);
    BOOST_CHECK_EQUAL(vec2[2], vec1[2]);

    const Matrix3  mat2(2e0);
    const Vector3d vec3 = vec1 * mat2;

    BOOST_CHECK_EQUAL(vec3[0], 2e0 * vec1[0]);
    BOOST_CHECK_EQUAL(vec3[1], 2e0 * vec1[1]);
    BOOST_CHECK_EQUAL(vec3[2], 2e0 * vec1[2]);

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    Matrix3  mat3;
    std::array<std::array<double, 3>, 3> random1;
    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            random1[i][j] = randreal(mt);

    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            mat3(i, j) = random1[i][j];

    const Vector3d ones(1e0, 1e0, 1e0);
    const Vector3d vec4 = ones * mat3;
    for(std::size_t i = 0; i < 3; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 3; ++j)
            value += random1[j][i];
        BOOST_CHECK_EQUAL(vec4[i], value);
    }
}

BOOST_AUTO_TEST_CASE(vector4d_matrix4x4_multiplication)
{
    const Vector4d vec1(2e0);
    const Matrix4  mat1(1e0);
    const Vector4d vec2 = mat1 * vec1;

    for(std::size_t i = 0; i<4; ++i)
        BOOST_CHECK_EQUAL(vec2[i], vec1[i]);

    const Matrix4 mat2(2e0);
    const Vector4d vec3 = mat2 * vec1;

    for(std::size_t i = 0; i<4; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 2e0 * vec1[i]);

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    Matrix4  mat3;
    std::array<std::array<double, 4>, 4> random1;
    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            random1[i][j] = randreal(mt);

    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            mat3(i, j) = random1[i][j];

    const Vector4d ones(1e0);
    const Vector4d vec4 = mat3 * ones;
    for(std::size_t i = 0; i < 4; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 4; ++j)
            value += random1[i][j];
        BOOST_CHECK_EQUAL(vec4[i], value);
    }
}

BOOST_AUTO_TEST_CASE(matrix4x4_vector4d_multiplication)
{
    const Vector4d vec1(1e0);
    const Matrix4  mat1(1e0);
    const Vector4d vec2 = vec1 * mat1;

    for(std::size_t i=0; i<4; ++i)
        BOOST_CHECK_EQUAL(vec2[i], vec1[i]);

    const Matrix4  mat2(2e0);
    const Vector4d vec3 = vec1 * mat2;

    for(std::size_t i=0; i<4; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 2e0 * vec1[i]);

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    Matrix4 mat3;
    std::array<std::array<double, 4>, 4> random1;
    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            random1.at(i).at(j) = randreal(mt);

    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            mat3(i, j) = random1.at(i).at(j);

    const Vector4d ones(1e0);
    const Vector4d vec4 = ones * mat3;
    for(std::size_t i = 0; i < 4; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 4; ++j)
            value += random1[j][i];
        BOOST_CHECK_EQUAL(vec4[i], value);
    }
}

BOOST_AUTO_TEST_CASE(matrix4x3_vector3d_multiplication)
{
    const Vector3d        vec1(1e0, 1e0, 1e0);
    ax::Matrix<double, 4, 3>  mat1;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::array<std::array<double, 3>, 4> random1;
    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            random1[i][j] = randreal(mt);

    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            mat1(i, j) = random1[i][j];

    const Vector4d   vec2 = mat1 * vec1;
    for(std::size_t i = 0; i < 4; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 3; ++j)
            value += random1[i][j];
        BOOST_CHECK_EQUAL(vec2[i], value);
    }
}

BOOST_AUTO_TEST_CASE(matrix3x4_vector4d_multiplication)
{
    const Vector4d vec1(1e0);
    for(std::size_t i=0; i<4;++i)
        assert(vec1[i] == 1e0);
    ax::Matrix<double, 3, 4>    mat1;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::array<std::array<double, 4>, 3> random1;
    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            random1.at(i).at(j) = randreal(mt);

    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            mat1(i, j) = random1.at(i).at(j);

    const Vector3d vec2 = mat1 * vec1;
    for(std::size_t i = 0; i < 3; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 4; ++j)
            value += random1[i][j];
        BOOST_CHECK_EQUAL(vec2[i], value);
    }
}

BOOST_AUTO_TEST_CASE(vector4d_matrix4x3_multiplication)
{
    const Vector4d vec1(1e0);
    for(std::size_t i = 0; i<4; ++i)
        assert(vec1[i] == 1e0);

    ax::Matrix<double, 4, 3>    mat1;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::array<std::array<double, 3>, 4> random1;
    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            random1[i][j] = randreal(mt);

    for(std::size_t i = 0; i < 4; ++i)
        for(std::size_t j = 0; j < 3; ++j)
            mat1(i, j) = random1[i][j];

    const Vector3d vec2 = vec1 * mat1;
    for(std::size_t i = 0; i < 3; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 4; ++j)
            value += random1[j][i];
        BOOST_CHECK_EQUAL(vec2[i], value);
    }
}

BOOST_AUTO_TEST_CASE(vector3d_matrix3x4_multiplication)
{
    const Vector3d vec1(1e0, 1e0, 1e0);
    for(std::size_t i = 0; i<3; ++i)
        assert(vec1[i] == 1e0);

    ax::Matrix<double, 3, 4> mat1;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::array<std::array<double, 4>, 3> random1;
    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            random1.at(i).at(j) = randreal(mt);

    for(std::size_t i = 0; i < 3; ++i)
        for(std::size_t j = 0; j < 4; ++j)
            mat1(i, j) = random1.at(i).at(j);

    const Vector4d vec2 = vec1 * mat1;
    for(std::size_t i = 0; i < 4; ++i)
    {
        double value = 0e0;
        for(std::size_t j = 0; j < 3; ++j)
            value += random1[j][i];
        BOOST_CHECK_EQUAL(vec2[i], value);
    }
}