//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/collision_ray_aabb.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

BOOST_AUTO_TEST_SUITE(opentissue_collision_ray_aabb);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;

  vector3_type min_coord( -1.0, -1.0, -1.0 );
  vector3_type max_coord(  1.0,  1.0,  1.0 );
  vector3_type r;
  vector3_type p;
  bool collision = false;

  // x-faces are separating planes

  r = vector3_type(1.0, 0.0, 0.0 ) ;
  p = vector3_type(2.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );

  r = vector3_type(-1.0, 0.0, 0.0 ) ;
  p = vector3_type(2.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(1.0, 0.0, 0.0 ) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(-1.0, 0.0, 0.0 ) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  // y-faces are separating planes

  r = vector3_type(0.0, 1.0, 0.0 ) ;
  p = vector3_type(0.0, 2.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );

  r = vector3_type(0.0, -1.0, 0.0 ) ;
  p = vector3_type(0.0,  2.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(0.0, 1.0, 0.0 ) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(0.0, -1.0, 0.0 ) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  // z-faces are separating planes

  r = vector3_type(0.0, 0.0, 1.0 ) ;
  p = vector3_type(0.0, 0.0, 2.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );

  r = vector3_type(0.0, 0.0, -1.0 ) ;
  p = vector3_type(0.0, 0.0,  2.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(0.0, 0.0, 1.0) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  r = vector3_type(0.0, 0.0, -1.0) ;
  p = vector3_type(0.0, 0.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( collision );

  // cross-produces are separating planes
  r = vector3_type(0.01, 0.01,   1.0) ;
  p = vector3_type(-2.0, -2.0, 0.0 ) ;
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );

  r = vector3_type(0.01, 1.0, 0.01);
  p = vector3_type(-2.0, 0.0, -2.0);
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );

  r = vector3_type(1.0, 0.01, 0.01);
  p = vector3_type(0.0, -2.0, -2.0);
  collision = OpenTissue::collision::ray_aabb(p,r,min_coord,max_coord);
  BOOST_CHECK( !collision );
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include <deal.II/base/function.h>
#include <deal.II/base/tensor_function.h>

#include "aux/tensor_helpers.hpp"
#include "var_form/polar_var_form_rhs.hpp"

namespace boltzmann {

template <int dimX, typename APP, typename SOURCE = typename APP::source_function_t>
class RhsVarForm : public PolarXVarFormRhs<dimX, SOURCE::rank>
{
 private:
  typedef PolarXVarFormRhs<dimX, SOURCE::rank> base_class;
  typedef SOURCE source_xv_t;
  typedef typename source_xv_t::SX_t source_function_t;

 public:
  // is S(x,v) = s(x) . s(v) ?s(x),s(v) scalar or vector valued?
  static const int rank = source_function_t::rank;

 public:
  template <typename FE>
  RhsVarForm(const FE& fe)
      : base_class(fe)
  { /* empty */
  }

  template <typename cell_iterator>
  void calc(const cell_iterator& cell);

 private:
  //@{
  /// functions
  source_function_t source_;
  //@}
  //@{
  /// working storage
  std::vector<typename base_class::Sx_t> source_values_;
  //@}
};

/**
 * @brief Weighted least squares var form in X-domain
 \f[
 \int_D s^x(x) \otimes \nabla \alpha(x) \epsilon(x) \mathrm{d} x
 + \int_D s^x \times \alpha(x) \sigma(x) \epsilon(x) \mathrm{d} x
 \f]
 Note that \f$ s^x (x) \f$ can be \f$ \mathbb{R}^2 \rightarrow \mathbb{R} \f$
 or \f$ \mathbb{R}^2 \rightarrow \mathbb{R}^2 \f$
 *
 *
 * @param cell
 *
 * @return
 */
template <int dimX, typename APP, typename SOURCE>
template <typename cell_iterator>
void
RhsVarForm<dimX, APP, SOURCE>::calc(const cell_iterator& cell)
{
  // update fevalues
  this->init_cell(cell);
  // clear
  base_class::clear_storage();
  const int nqpoints = this->quad.size();
  const int dofs_per_cell = this->fe_values.dofs_per_cell;
  // resize storage
  source_.value_list(this->fe_values.get_quadrature_points(), source_values_);

  for (int ix = 0; ix < dofs_per_cell; ++ix) {
    for (int q = 0; q < nqpoints; ++q) {
      auto shape = this->fe_values.shape_value(ix, q);
      auto grad = this->fe_values.shape_grad(ix, q);
      double weight = this->fe_values.JxW(q);
      /// mass part
      /// stabilization part i.e multiplied with grad
      typename base_class::Tx_t Tx_tmp;
      outer_product(Tx_tmp, source_values_[q], grad);

      this->Tx_vec[ix] += Tx_tmp * weight;
    }
  }
}

}  // end namespace boltzmann


#ifndef COMPILER_QUBIT_TAPERING_HPP_
#define COMPILER_QUBIT_TAPERING_HPP_

#include "IRTransformation.hpp"
#include "PauliOperator.hpp"
#include "OptionsProvider.hpp"
#include <Eigen/Dense>

using namespace xacc::quantum;

namespace xacc {
namespace vqe {

/**
 * QubitTapering is an IRTransformation that implements
 * the Hamiltonian reduction scheme for discrete
 * Z2 symmetries as described in https://arxiv.org/pdf/1701.08213.pdf.
 */
class QubitTapering : public IRTransformation, public OptionsProvider {

public:
  QubitTapering() {}

  virtual std::shared_ptr<IR> transform(std::shared_ptr<IR> ir);

  virtual const std::string name() const { return "qubit-tapering"; }
  virtual const std::string description() const {
    return "Reduce number of qubits required by exploiting Z2 symmetries.";
  }

  virtual OptionPairs getOptions() {
    OptionPairs desc {{"phase-sector",
                        "Provide the +-1 vector."},{
        "qubit-tapering-show",
        "Create and display reduced hamiltonian, but then exit."}};
    return desc;
  }

private:
  Eigen::MatrixXi computeTableaux(PauliOperator &H, const int nQubits);
  std::vector<std::vector<int>> generateCombinations(
      const int nQubits, std::function<void(std::vector<int> &)> &&f =
                             [](std::vector<int> &tmp) { return; });
  int binaryVectorInnerProduct(std::vector<int> &bv1, std::vector<int> &bv2);
  const double computeGroundStateEnergy(PauliOperator &op, const int n);

  Eigen::MatrixXi gauss(Eigen::MatrixXi& A, std::vector<int> &pivotCols) {
     int n = A.rows();
     int m = A.cols();

     int sc = 0;

     for (int i = 0; i < m; i++) {
         bool found_row = false;
         int ip = i-sc;
         for (int k = ip; k < n; k++) {
             if (A(k,i) == 1) {
                 found_row = true;

                 if (k > ip) {
                     for (int j = i; j < m; j++) {
                         auto tmp = A(k,j);
                         A(k,j) = A(ip,j);
                         A(ip,j) = tmp;
                     }
                 }

                 for (int l = ip+1; l < n; l++) {
                     if (A(l,i) != 0) {
                         for (int j = ip; j < m; j++) {
                             A(l,j) = (A(l,j)+A(ip,j)) %2;
                         }
                     }
                 }
                 break;
             }
         }
         if (!found_row) sc += 1;
     }

    unsigned row2 = 0;
    for (unsigned col = 0; col < A.cols() && row2 < A.rows(); col++) {
      if (std::fabs(A(row2, col)) < 1e-12)
        continue;

      pivotCols.push_back(col);
      row2++;
    }

     return A;

  }
};
} // namespace vqe
} // namespace xacc

#endif


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout;  using std::endl;

//[policy_ref_snip2

#include <boost/math/distributions/normal.hpp>
using boost::math::normal_distribution;

using namespace boost::math::policies;

// Define a specific policy:
typedef policy<
      overflow_error<ignore_error>
      > my_policy;

// Define the distribution, using my_policy:
typedef normal_distribution<double, my_policy> my_norm;

// Construct a my_norm distribution, using default mean and standard deviation,
// and get a 0.05 or 5% quantile:
double q = quantile(my_norm(), 0.05); // = -1.64485

//] //[/policy_ref_snip2]

int main()
{
  my_norm n; // Construct a my_norm distribution,
  // using default mean zero and standard deviation unity.
  double q = quantile(n, 0.05); // and get a quantile.
  cout << "quantile(my_norm(), 0.05) = " << q << endl;
}

/*

Output:

  quantile(my_norm(), 0.05) = -1.64485
*/


#pragma once

#include <Eigen/Core>

#include <memory>
#include <utility>
#include <vector>

namespace eq {
    const int INIT_TIME       = 0;
    const double MAX_MIN_DIFF = 10e-10;
    const double DEFAULT_STEP = 0.1;

    const double TASK_DIFF_STEP = 0.001;
    const double MAX_TIME_RUNGE = 0.2;
    const double MAX_TIME_DP    = 10;

    struct task_answer {
        double answer_max_diff_runge{-1};
        double answer_step_runge{-1};
        double answer_min_step_dp{-1};
        int answer_total_steps_dp{-1};
    };

    class EquationSolver;

    class Solver {
    public:
        virtual ~Solver() = default;
        explicit Solver(EquationSolver &eq_solver, double step) : _solver(eq_solver), _step(step) {}

        Solver() = delete;
        Solver(const Solver&) = delete;
        void operator=(const Solver&) = delete;

        virtual void Calculate() = 0;
        virtual void Solver_set_matrix() = 0;

        Eigen::MatrixXd Generate_mat();

        double Get_step() const { return _step; }
        void Set_step(double step) {_step = step; }

    protected:
        EquationSolver &_solver;
        double _step;
    };

    class RungeSolver : public Solver {
    public:
        explicit RungeSolver(EquationSolver &eq_solver, double step = DEFAULT_STEP) : Solver(eq_solver, step) {}

        RungeSolver() = delete;
        RungeSolver(const RungeSolver&) = delete;
        void operator=(const RungeSolver&) = delete;

        void Calculate() override;
        void Solver_set_matrix() override;

    private:
        void calc_step();
    };

    class DPSolver : public Solver {
    public:
        explicit DPSolver(EquationSolver &eq_solver, double max_diff = MAX_MIN_DIFF, double min_diff = MAX_MIN_DIFF,
                          double step = DEFAULT_STEP) :
                          Solver(eq_solver, step), _max_diff(max_diff), _min_diff(min_diff) {}

        DPSolver() = delete;
        DPSolver(const DPSolver&) = delete;
        void operator=(const DPSolver&) = delete;

        void Calculate() override;
        void Solver_set_matrix() override;

    private:
        void calc_step();

    private:
        double _max_diff;
        double _min_diff;
    };

    class EquationSolver {
    public:
        explicit EquationSolver(std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> recalc_function,
                        Eigen::VectorXd start_vals, double start_time, std::function<Eigen::VectorXd(double)> result_function) :
                        _recalc_function(std::move(recalc_function)), _values(std::move(start_vals)), _time(start_time),
                        _result_function(std::move(result_function)) {}

        EquationSolver() = delete;
        EquationSolver(const EquationSolver&) = delete;
        void operator=(const EquationSolver&) = delete;

        void Set_solver(std::unique_ptr<Solver>&& solver);
        void Reload_values(const Eigen::VectorXd &init_vals, double init_time);

        void Solve();
        task_answer Get_answer() const;

        double Get_time() const { return _time; }
        Eigen::VectorXd& Get_values() { return _values; }

    public:
        friend Solver;
        friend RungeSolver;
        friend DPSolver;

    private:
        std::unique_ptr<Solver> _solver;

        std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> _recalc_function;
        Eigen::VectorXd _values;
        Eigen::VectorXd _steps;
        std::function<Eigen::VectorXd(double)> _result_function;

        Eigen::MatrixXd _a;
        std::vector<Eigen::MatrixXd> _b;

        double _time{0};

    private:
        double _answer_max_diff_runge{-1};
        double _answer_step_runge{-1};
        double _answer_min_step_dp{-1};
        int _answer_total_steps_dp{-1};
    };

    task_answer Calculate(std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> src_function,
                   std::function<Eigen::VectorXd(double)> result_function, const Eigen::VectorXd& init_val);
}


#include <iostream>
#include <iomanip>
#include <algorithm>
#include <functional>
#include <limits>
#include <ctime>
#include <cmath>
#include <cassert>

#include <boost/timer.hpp>
#include <boost/random.hpp>

#include "StochasticFunctionMinimization.h"

namespace Grante {

double StochasticFunctionMinimization::StochasticSubgradientMethodMinimize(
	StochasticFunctionMinimizationProblem& prob, std::vector<double>& x_opt,
	double conv_tol, unsigned int max_epochs, bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	// Random instance generator
	size_t N = prob.NumberOfElements();
	assert(N > 0);
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_int<unsigned int> rdestd(0, static_cast<unsigned int>(N-1));
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_n(rgen, rdestd);

	// Optimize a given number of epochs
	boost::timer total_timer;
	std::vector<double> avg_grad(dim, 0.0);
	double lambda = 1.0;	// (should be lambda=1/C)
	double avg_obj = 0.0;
	for (unsigned int epoch = 0; max_epochs == 0 || epoch < max_epochs;
		++epoch) {
		avg_obj = 0.0;
		std::fill(avg_grad.begin(), avg_grad.end(), 0.0);

		// Choose epoch-wide step size
		double alpha = 1.0 / (static_cast<double>(epoch + 1) * lambda);

		// Optimize by sampling instances
		for (size_t n = 0; n < N; ++n) {
			unsigned int id = rand_n();
			// Update average objective and averaged gradient of this epoch
			avg_obj += prob.Eval(id, x, grad);
			std::transform(grad.begin(), grad.end(), avg_grad.begin(),
				avg_grad.begin(), std::plus<double>());

			// Perform incremental subgradient update
			for (unsigned int d = 0; d < dim; ++d)
				x[d] -= alpha * grad[d];
		}
		// Compute mean gradient and estimated objective
		double avg_grad_norm = 0.0;
		for (unsigned int d = 0; d < dim; ++d)
			avg_grad_norm += avg_grad[d]*avg_grad[d];
		avg_grad_norm = std::sqrt(avg_grad_norm);

		// Output statistics
		if (verbose && (epoch % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time        avg_obj  |avg_grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << epoch << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << avg_obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << avg_grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Convergence check
		if (avg_grad_norm < conv_tol)
			break;
	}

	x_opt = x;
	return (avg_obj);	// This is not exact, but stochastic anyway
}

}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <eve/detail/diff_div.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of sph_bessel_yn"
              , eve::test::simd::ieee_reals
              )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::sph_bessel_yn(T(), T())  ,  T);
  TTS_EXPR_IS( eve::sph_bessel_yn(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::sph_bessel_yn(i_t(),T()),   T);
  TTS_EXPR_IS( eve::sph_bessel_yn(I_t(),T()),   T);
  TTS_EXPR_IS( eve::sph_bessel_yn(i_t(),v_t()), v_t);
//  TTS_EXPR_IS( eve::sph_bessel_yn(I_t(),v_t()), T);
 };

//==================================================================================================
//== integral orders
//==================================================================================================
EVE_TEST( "Check behavior of sph_bessel_yn on wide with integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0), eve::test::randoms(0.0, 20000.0))
        )
  <typename T>(T n, T a0)
{
  using v_t = eve::element_type_t<T>;
  auto eve__sph_bessel_yn =  [](auto n, auto x) { return eve::sph_bessel_yn(n, x); };
  auto std__sph_bessel_yn =  [](auto n, auto x)->v_t { return boost::math::sph_neumann(unsigned(n), double(x)); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_yn(2, eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_yn(3, eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }

  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, v_t(1500)), std__sph_bessel_yn(3u, v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, v_t(500)), std__sph_bessel_yn(2u, v_t(500)), 2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, v_t(10)), std__sph_bessel_yn(2u, v_t(10))  , 5.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, v_t(5)),  std__sph_bessel_yn(3u, v_t(5))   , 40.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, v_t(0.1)), std__sph_bessel_yn(2u, v_t(0.1))  , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, v_t(0.2)),  std__sph_bessel_yn(3u, v_t(0.2))   , 2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(10, v_t(8)), std__sph_bessel_yn(10u, v_t(8))  , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(10, v_t(8)),  std__sph_bessel_yn(10u, v_t(8))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_yn(2, eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_yn(3, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }

  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, T(1500)),  T(std__sph_bessel_yn(3u, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, T(500)),   T(std__sph_bessel_yn(2u, v_t(500))),   2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, T(10)),    T(std__sph_bessel_yn(2u, v_t(10)))   , 5.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, T(5)),     T(std__sph_bessel_yn(3u, v_t(5)))    , 40.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(2, T(0.1)),   T(std__sph_bessel_yn(2u, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(3, T(0.2)),   T(std__sph_bessel_yn(3u, v_t(0.2)))  , 2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(10, T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(10, T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_yn(T(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_yn(T(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }

  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(3), T(1500)),  T(std__sph_bessel_yn(3u, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(2), T(500)),   T(std__sph_bessel_yn(2u, v_t(500))),   2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(2), T(10)),    T(std__sph_bessel_yn(2u, v_t(10)))   , 5.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(3), T(5)),     T(std__sph_bessel_yn(3u, v_t(5)))    , 40.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(2), T(0.1)),   T(std__sph_bessel_yn(2u, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(3), T(0.2)),   T(std__sph_bessel_yn(3u, v_t(0.2)))  , 2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(10), T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(T(10), T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);

  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }

  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(3), T(1500)),  T(std__sph_bessel_yn(3u, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(2), T(500)),   T(std__sph_bessel_yn(2u, v_t(500))),   2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(2), T(10)),    T(std__sph_bessel_yn(2u, v_t(10)))   , 5.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(3), T(5)),     T(std__sph_bessel_yn(3u, v_t(5)))    , 40.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(2), T(0.1)),   T(std__sph_bessel_yn(2u, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(3), T(0.2)),   T(std__sph_bessel_yn(3u, v_t(0.2)))  , 2.0);

  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(10), T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__sph_bessel_yn(I_t(10), T(8)),   T(std__sph_bessel_yn(10u, v_t(8)))   , 2.0);

  TTS_RELATIVE_EQUAL(eve__sph_bessel_yn(n, a0),   map(std__sph_bessel_yn, n, a0)   , 0.0025);
};

EVE_TEST( "Check behavior of diff(sph_bessel_j1) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(1.0, 10.0))
        )
  <typename T>(T a0 )
{
  auto eve__diff_bessel_yn =  [](auto n, auto x) { return eve::diff(eve::sph_bessel_yn)(n, x); };
  auto y3 = [](auto x){ return eve::sph_bessel_yn(3, x); };
  auto df = [y3](auto x){return eve::detail::centered_diffdiv(y3, x); };

  TTS_RELATIVE_EQUAL(eve__diff_bessel_yn(3, a0),   df(a0), 5.0e-2);
  TTS_RELATIVE_EQUAL(eve__diff_bessel_yn(1, a0),   eve::diff(eve::sph_bessel_y1)(a0), 5.0e-2);
};


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is 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)

//[touches_one_geometry
//` Checks if a geometry has at least one touching point (self-tangency)

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Checks if the geometry has self-tangency.
    bg::model::polygon<bg::model::d2::point_xy<double> > poly1;
    bg::read_wkt("POLYGON((0 0,0 3,2 3,2 2,1 2,1 1,2 1,2 2,3 2,3 0,0 0))", poly1);
    bool check_touches = bg::touches(poly1);
    if (check_touches) {
         std::cout << "Touches: Yes" << std::endl;
    } else {
        std::cout << "Touches: No" << std::endl;
    }

    bg::model::polygon<bg::model::d2::point_xy<double> > poly2;
    bg::read_wkt("POLYGON((0 0,0 4,4 4,4 0,2 3,0 0))", poly2);
    check_touches = bg::touches(poly2);
    if (check_touches) {
         std::cout << "Touches: Yes" << std::endl;
    } else {
        std::cout << "Touches: No" << std::endl;
    }

    return 0;
}

//]

//[touches_one_geometry_output
/*`
Output:
[pre
Touches: Yes

[$img/algorithms/touches_one_geometry.png]

Touches: No
]
*/
//]


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/acoth.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/i.hpp>

#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/atanh.hpp>
#include <boost/math/complex/atanh.hpp>

NT2_TEST_CASE_TPL ( acoth_real, NT2_REAL_TYPES)
{
  using nt2::acoth;
  using nt2::tag::acoth_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<acoth_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(nt2::One<T>(), nt2::Zero<T>())),  cT(nt2::Inf<T>(), nt2::Zero<T>()), 0.75);
#endif
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(nt2::Zero<T>(), nt2::Zero<T>())),  cT(nt2::Zero<T>(), nt2::Pio_2<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(nt2::Mzero<T>(), nt2::Zero<T>())), cT(nt2::Zero<T>(), nt2::Pio_2<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(1, 1)),   boost::math::atanh(nt2::rec(cT(1, 1))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(1, 10)),  boost::math::atanh(nt2::rec(cT(1, 10))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(10, 10)), boost::math::atanh(nt2::rec(cT(10, 10))), 1);
  NT2_TEST_ULP_EQUAL(nt2::acoth(cT(10, 1)),  boost::math::atanh(nt2::rec(cT(10, 1))), 1);
}


#include "fieldtrack/AdaptiveCovarianceEstimator.h"
#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/utils/MatrixUtils.h"
#include <boost/foreach.hpp>
#include <sstream>
#include <Eigen/SVD>

using namespace argus_msgs;
using namespace argus;

namespace argus
{
AdaptiveTransCovEstimator::AdaptiveTransCovEstimator() {}

void AdaptiveTransCovEstimator::Initialize( ros::NodeHandle& ph )
{
	GetParamRequired( ph, "max_window_samples", _maxSamples );

	double dur;
	GetParamRequired( ph, "max_sample_age", dur );
	_maxAge = ros::Duration( dur );

	unsigned int dim;
	GetParamRequired( ph, "dim", dim );
	_priorCov = MatrixType( dim, dim );
	GetParam( ph, "prior_cov", _priorCov, 1E-3 * MatrixType::Identity( dim, dim ) );
	GetParam( ph, "prior_age", _priorAge, 1.0 );
	GetParam( ph, "prior_dt", _priorDt, 1.0 );

	GetParam( ph, "use_diag", _useDiag, true );
	GetParam( ph, "decay_rate", _decayRate, 1.0 );
	_decayRate = std::log( _decayRate );
}

unsigned int AdaptiveTransCovEstimator::NumSamples() const
{
	return _innoProds.size();
}

MatrixType AdaptiveTransCovEstimator::GetQ( const ros::Time& time )
{
	CheckBuffer( time );

	double wAcc = std::exp( _decayRate * _priorAge );
	MatrixType Qacc = _priorCov * wAcc;
	for( unsigned int i = 0; i < _innoProds.size(); ++i )
	{
		double t = ( time - _innoProds[i].first ).toSec();
		double w = std::exp( _decayRate * t );
		Qacc += _innoProds[i].second * w;
		wAcc += w;
	}

	// Version using single adjusted state deltas
	// MatrixType adaptQ = Qacc / wAcc + _currSpost - _lastFSpostFT;

	// Version using innovations or pre-adjusted state deltas
	MatrixType adaptQ = Qacc / wAcc;

	double timeSpan;
	if( _innoProds.size() < 2 )
	{
		timeSpan = 0;
	}
	else
	{
		timeSpan = ( _innoProds.front().first - _innoProds.back().first ).toSec();
	}
	timeSpan += _priorDt;
	double averageDt = timeSpan / ( NumSamples() + 1 );
	MatrixType adaptQRate = adaptQ / averageDt;

	// Check for diagonal
	if( _useDiag )
	{
		adaptQRate = Eigen::DiagonalMatrix<double, Eigen::Dynamic>( adaptQRate.diagonal() );
	}
	return adaptQRate;
}

void AdaptiveTransCovEstimator::Update( const ros::Time& time,
                                                    const PredictInfo& predict,
                                                    const UpdateInfo& update )
{
	// Initialization catch
	if( _lastFSpostFT.size() == 0 )
	{
		unsigned int dim = update.state_delta.size();
		_currSpost = MatrixType::Zero( dim, dim );
	}

	// Version using state deltas
	// MatrixType op = update.state_delta * update.state_delta.transpose();

	// Version incorporating estimate covariance adjustment
	// MatrixType op = update.state_delta * update.state_delta.transpose() + _currSpost - _lastFSpostFT;

	// Version using innovations
	VectorType Kv = update.kalman_gain * update.prior_obs_error;
	MatrixType op = Kv * Kv.transpose();

	_innoProds.emplace_front( time, op );
	_lastFSpostFT = predict.trans_jacobian * _currSpost *
	                predict.trans_jacobian.transpose();
	_currSpost = update.post_state_cov;
}

void AdaptiveTransCovEstimator::Reset()
{
	_lastFSpostFT = MatrixType();
	_currSpost = MatrixType();
	_innoProds.clear();
}

void AdaptiveTransCovEstimator::CheckBuffer( const ros::Time& now )
{
	while( !_innoProds.empty() &&
	       ( NumSamples() > _maxSamples ||
	         ( now - _innoProds.back().first ) > _maxAge ) )
	{
		_innoProds.pop_back();
	}
}

AdaptiveObsCovEstimator::AdaptiveObsCovEstimator() {}

void AdaptiveObsCovEstimator::Initialize( unsigned int dim,
                                                         ros::NodeHandle& ph )
{
	GetParamRequired( ph, "max_window_samples", _maxSamples );
	GetParam( ph, "min_window_samples", _minSamples, (unsigned int) 0 );

	double dur;
	GetParamRequired( ph, "max_sample_age", dur );
	_maxAge = ros::Duration( dur );

	_dim = dim;
	_priorCov = MatrixType( _dim, _dim );
	GetParam( ph, "prior_cov", _priorCov, 1E-3 * MatrixType::Identity( _dim, _dim ) );
	GetParam( ph, "prior_age", _priorAge, 1.0 );
	GetParam( ph, "use_diag", _useDiag, true );
	GetParam( ph, "decay_rate", _decayRate, 1.0 );
	_decayRate = std::log( _decayRate );
}

unsigned int AdaptiveObsCovEstimator::NumSamples() const
{
	return _innoProds.size();
}

MatrixType AdaptiveObsCovEstimator::GetR( const ros::Time& time )
{
	CheckBuffer( time );

	MatrixType adaptR;
	if( _innoProds.size() >= _minSamples )
	{
		double priorW = std::exp( _decayRate * _priorAge );
		double wAcc = priorW;
		MatrixType Racc = _priorCov * priorW;
		for( unsigned int i = 0; i < _innoProds.size(); ++i )
		{
			const InnoStamped& data = _innoProds[i];
			const ros::Time& stamp = data.first;
			const MatrixType& Rhat = data.second;

			double dt = ( time - stamp ).toSec();
			double w = std::exp( _decayRate * dt );
			Racc += Rhat * w;
			wAcc += w;
		}
		adaptR = Racc / wAcc;
	}
	else
	{
		adaptR = _priorCov;
	}

	// Check for diagonal
	if( _useDiag )
	{
		adaptR = Eigen::DiagonalMatrix<double, Eigen::Dynamic>( adaptR.diagonal() );
	}

	//ROS_INFO_STREAM( "R: " << adaptR.diagonal().transpose() );

	return adaptR;
}

const MatrixType& AdaptiveObsCovEstimator::GetPriorCov() const
{
	return _priorCov;
}

// MatrixType AdaptiveObsCovEstimator::GetR( const ros::Time& time,
//                                                          const UpdateInfo& preview )
// {
//  CheckBuffer( time );

//  double wAcc = std::exp( _decayRate * _priorAge );
//  MatrixType Racc = _priorCov * wAcc;
//  for( unsigned int i = 0; i < _innoProds.size(); ++i )
//  {
//      const InnoStamped& data = _innoProds[i];
//      const ros::Time& stamp = data.first;
//      const MatrixType& Rhat = data.second;

//      double dt = ( time - stamp ).toSec();
//      double w = std::exp( _decayRate * dt );
//      Racc += Rhat * w;
//      wAcc += w;
//  }

//  // Add current preview in
//  // Update is R = Cv- + H * P- * H^T
//  MatrixType HPHT = update.obs_jacobian * update.pre_state_cov *
//                    update.obs_jacobian.transpose();
//  MatrixType op = update.pre_obs_error * update.pre_obs_error.transpose();
//  wAcc += 1.0;
//  Racc += op - HPHT;

//  MatrixType adaptR = Racc / wAcc;

//  // Check for diagonal
//  if( _useDiag )
//  {
//      adaptR = Eigen::DiagonalMatrix<double, Eigen::Dynamic>( adaptR.diagonal() );
//  }
//  return adaptR;
// }

void AdaptiveObsCovEstimator::Update( const UpdateInfo& update )
{
	// Update is R = Cv+ + H * P+ * H^T
	MatrixType HPHT = update.obs_jacobian * update.post_state_cov *
	                  update.obs_jacobian.transpose();
	MatrixType op = update.post_obs_error * update.post_obs_error.transpose();
        MatrixType Rest = HPHT + op;

        // Update is R = Cv- - H * P- * H^T
        //MatrixType HPHT = update.obs_jacobian * update.prior_state_cov *
        //         	  update.obs_jacobian.transpose();
        //MatrixType op = update.prior_obs_error * update.prior_obs_error.transpose();
        //MatrixType Rest = Eigen::DiagonalMatrix<double, Eigen::Dynamic>( ( op - HPHT ).diagonal() );
        //for( unsigned int i =0; i < Rest.rows(); ++i )
	//  {
	//    if( Rest(i,i) < 0.0 ) { Rest(i,i) = 0.0; }
	//  }
	_innoProds.emplace_front( update.time, Rest );
}

void AdaptiveObsCovEstimator::Reset()
{
	_innoProds.clear();
}

void AdaptiveObsCovEstimator::CheckBuffer( const ros::Time& now )
{
	while( !_innoProds.empty() &&
	       ( NumSamples() > _maxSamples ||
	         ( now - _innoProds.back().first ) > _maxAge ) )
	{
		_innoProds.pop_back();
	}
}
}


/*
 * Copyright (C) 2021, unclearness
 * All rights reserved.
 */

#include "ugu/optimizer/optimizer.h"

#include <Eigen/LU>
#include <iostream>

namespace {

using UpdateFunc = std::function<void(const ugu::OptimizerInput& in,
                                      ugu::OptimizerOutput& out)>;

double LineSearchBacktracking(const ugu::GradVec& update_direc,
                              const ugu::OptimizerInput& in,
                              ugu::OptimizerOutput& out) {
  // BACK_TRACKING
  double a = in.line_search_max;
  double r = 0.5;
  while (true) {
    // Armijo rule
    double c1 = 1e-4;
    auto new_param = out.best_param + a * update_direc;
    double new_val = in.loss_func(new_param);

    double constraint = out.best + c1 * a * out.best_grad.dot(update_direc);

    if (new_val <= constraint) {
      return a;
    }

    a *= r;
  }
}

double LineSearch(const ugu::GradVec& update_direc,
                  const ugu::OptimizerInput& in, ugu::OptimizerOutput& out) {
  if (in.line_search_method == ugu::LineSearchMethod::BACK_TRACKING) {
    return LineSearchBacktracking(update_direc, in, out);
  }

  throw std::invalid_argument("This type is not implemented");

  //return 0.0;
}

void LoopBody(const ugu::OptimizerInput& input, ugu::OptimizerOutput& output,
              UpdateFunc update_func) {
  output.Clear();
  output.best_param = input.init_param;
  output.best = input.loss_func(output.best_param);
  output.best_grad = input.grad_func(output.best_param);

  output.val_history.push_back(output.best);
  output.param_history.push_back(output.best_param);
  output.grad_history.push_back(output.best_grad);

  double prev = output.best;
  double diff = std::numeric_limits<double>::max();
  size_t max_size = static_cast<size_t>(input.LBFGS_memoery_num);
  while (!input.terminate_criteria.isTerminated(output.best_iter, diff)) {
    prev = output.best;

    update_func(input, output);

    output.best = input.loss_func(output.best_param);
    output.best_grad = input.grad_func(output.best_param);

    output.val_history.push_back(output.best);
    output.param_history.push_back(output.best_param);
    output.grad_history.push_back(output.best_grad);
    while (max_size < output.val_history.size()) {
      output.val_history.erase(output.val_history.begin());
      output.param_history.erase(output.param_history.begin());
      output.grad_history.erase(output.grad_history.begin());
    }

    // output.best = input.loss_func(output.best_param);
    diff = prev - output.best;
    output.best_iter++;
    // std::cout << prev << std::endl;
  }
}

void GradientDescentUdpateFunc(const ugu::OptimizerInput& in,
                               ugu::OptimizerOutput& out) {
  // Eval grad
  auto grad = in.grad_func(out.best_param);

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(-grad, in, out);
  }

  out.best_param += (lr * -grad);
}

// http://www.dais.is.tohoku.ac.jp/~shioura/teaching/mp13/mp13-13.pdf
void NewtonUpdateFunc(const ugu::OptimizerInput& in,
                      ugu::OptimizerOutput& out) {
  ugu::GradVec grad = in.grad_func(out.best_param);
  // out.best_grad = grad;
  ugu::Hessian hessian = in.hessian_func(out.best_param);
  ugu::OptParams delta = (-hessian.inverse() * grad);

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(delta, in, out);
  }

  out.best_param += (lr * delta);
}

// https://en.wikipedia.org/wiki/Limited-memory_BFGS
// https://abicky.net/2010/06/22/114613/
void LBFGSUpdateFunc(const ugu::OptimizerInput& in, ugu::OptimizerOutput& out) {
  ugu::GradVec grad = in.grad_func(out.best_param);
  // out.best_grad = grad;
  ugu::GradVec q = grad;

  if (out.grad_history.size() < 2) {
    // Initialize by GradientDescent
    GradientDescentUdpateFunc(in, out);
    return;
  }

  ugu::OptIndex k = static_cast<ugu::OptIndex>(out.grad_history.size() - 1);
  std::vector<ugu::GradVec> y_list;
  std::vector<ugu::GradVec> s_list;
  std::vector<double> rho_list;

  for (ugu::OptIndex i = k; i >= 1; i--) {
    ugu::GradVec y = out.grad_history[i] - out.grad_history[i - 1];
    ugu::GradVec s = out.param_history[i] - out.param_history[i - 1];
    double dot = y.dot(s);

#if 0
    // Guard
    if (std::abs(dot) < 1e-10) {
      if (dot > 0) {
        dot = 1e-10;
      } else {
        dot = -1e-10;
      }
    }
    if (std::abs(dot) < 1e10) {
      if (dot > 0) {
        dot = 1e10;
      } else {
        dot = -1e10;
      }
    }
#endif

    double rho = 1.0 / dot;
#if 0
    if (std::isnan(rho)) {
        rho = 0;
    }
    std::cout << y << std::endl;
    std::cout << s << std::endl;
    std::cout << rho << std::endl;
    std::cout << std::endl;

    if (std::abs(rho) < 1e-10) {
      if (rho > 0) {
        rho = 1e-10;
      } else {
        rho = -1e-10;
      }
    }
#endif
    y_list.push_back(y);
    s_list.push_back(s);
    rho_list.push_back(rho);
  }

  std::vector<double> alpha_list;

  for (ugu::OptIndex i = 0; i < k; i++) {
    double alpha = rho_list[i] * s_list[i].dot(q);
    q = q - alpha * y_list[i];
    alpha_list.push_back(alpha);
  }

  double gamma = s_list[0].dot(y_list[0]) / y_list[0].dot(y_list[0]);
  Eigen::SparseMatrix<double> I(in.init_param.rows(), in.init_param.rows());
  I.setIdentity();
  Eigen::SparseMatrix<double> H = gamma * I;

  // std::cout << H << std::endl;

  ugu::GradVec z = H * q;

  for (ugu::OptIndex i = k - 1; i >= 0; i--) {
    double beta = rho_list[i] * y_list[i].dot(z);
    z = z + s_list[i] * (alpha_list[i] - beta);
  }

  z = -z;

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(z, in, out);
  }

  out.best_param += (lr * z);
}

}  // namespace

namespace ugu {

GradFunc GenNumericalGrad(LossFunc loss_func, double h) {
  return [loss_func, h](const OptParams& params) {
    GradVec grad(params.size());
    const double inv_2h = 1.0 / (2 * h);
    for (OptIndex i = 0; i < params.size(); i++) {
      auto params_plus = params;
      params_plus[i] += h;
      auto params_minus = params;
      params_minus[i] -= h;
      grad[i] = (loss_func(params_plus) - loss_func(params_minus)) * inv_2h;
    }
    return grad;
  };
}

HessianFunc GenNumericalHessian(LossFunc loss_func, double h) {
  return [loss_func, h](const OptParams& params) {
    Hessian hessian(params.rows(), params.rows());
    const double inv_2h = 1.0 / (2 * h);
    for (OptIndex i = 0; i < params.size(); i++) {
      auto params_i_p = params;
      params_i_p[i] += h;
      auto params_i_n = params;
      params_i_n[i] -= h;
      for (OptIndex j = 0; j < params.size(); j++) {
        auto params_i_p_j_p = params_i_p;
        params_i_p_j_p[j] += h;
        auto params_i_p_j_n = params_i_p;
        params_i_p_j_n[j] -= h;

        auto grad_ij_p =
            (loss_func(params_i_p_j_p) - loss_func(params_i_p_j_n)) * inv_2h;

        auto params_i_n_j_p = params_i_n;
        params_i_n_j_p[j] += h;
        auto params_i_n_j_n = params_i_n;
        params_i_n_j_n[j] -= h;

        auto grad_ij_n =
            (loss_func(params_i_n_j_p) - loss_func(params_i_n_j_n)) * inv_2h;

        hessian(i, j) = (grad_ij_p - grad_ij_n) * inv_2h;
        // std::cout << grad_ij_p << " " << grad_ij_n << " " << hessian(i, j)
        //          << std::endl;
      }
    }
    return hessian;
  };
}

void GradientDescent(const OptimizerInput& input, OptimizerOutput& output) {
  LoopBody(input, output, GradientDescentUdpateFunc);
}

void Newton(const OptimizerInput& input, OptimizerOutput& output) {
  LoopBody(input, output, NewtonUpdateFunc);
}

void QuasiNewton(const OptimizerInput& input, OptimizerOutput& output,
                 QuasiNewtonMethod method) {
  if (method == QuasiNewtonMethod::LBFGS) {
    LoopBody(input, output, LBFGSUpdateFunc);
  } else {
    throw std::invalid_argument("This method is not supported");
  }
}

}  // namespace ugu


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

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/lagrangespace.hh"
#include "fem/hierarchicspace.hh"
#include "linalg/direct.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "utilities/enums.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare, createUnitCube
#include "io/vtk.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"

#include "mg/pcg.hh"
#include "mg/apcg.hh"

#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "poisson.hh"

bool compareAbs(const double x1, const double  x2)
{
  return fabs(x1)>fabs(x2);
}

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start heat transfer tutorial program using HB error estimation (SpaceDimension=3)"
            << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int refinements   = getParameter(pt, "refinements", 2),
      order         = getParameter(pt, "order", 1),
      maxAdaptSteps = getParameter(pt, "maxAdaptSteps", 2),
      verbosity     = getParameter(pt, "verbosity", 1);
  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "max. adaptive refine steps     : " << maxAdaptSteps << std::endl;
  std::cout << "output level (verbosity)       : " << verbosity << std::endl << std::endl;

  int  direct, onlyLowerTriangle = false;

  DirectType directType;
  IterateType iterateType = IterateType::PCG;
  PrecondType precondType = PrecondType::NONE;
  MatrixProperties property;
  std::string empty, geoFile("cube.am");

  geoFile = getParameter(pt, "file", geoFile);

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  std::cout << "Solver type is " << (direct ? "direct" : "iterative") << std::endl;

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 2));

  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || ( (precondType == PrecondType::ICC) && !direct ) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl << std::endl;
  }

  //   three-dimensional space: dim=3
  constexpr int dim=3;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = PoissonFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using H1ExSpace = FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,LeafView> >;
  using H1ExSpaces = boost::fusion::vector<H1Space const*,H1ExSpace const*>;
  using ExVariableDescriptions = boost::fusion::vector<VariableDescription<1,1,0> >;
  using ExVariableSet = VariableSetDescription<H1ExSpaces,ExVariableDescriptions>;
  using ErrorEstimator = HierarchicErrorEstimator<LinearizationAt<Functional>,ExVariableSet>;
  using EstAssembler = VariationalFunctionalAssembler<ErrorEstimator>;
  using ExCoefficientVectors = ExVariableSet::CoefficientVectorRepresentation<0,1>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using AssOperator = AssembledGalerkinOperator<Assembler,0,1,0,1>;
  using AssEstOperator = AssembledGalerkinOperator<EstAssembler>;

  GridManager<Grid> gridManager( createUnitCube<Grid>(0.5) );

  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " tetrahedra, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim-1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim) << " points" << std::endl;
  // a gridmanager is constructed
  // as connector between geometric and algebraic information
  gridManager.setVerbosity(verbosity);

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[1] = { "T" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;

  Assembler assembler(gridManager,spaces);

  H1ExSpace spaceEx(gridManager,gridManager.grid().leafGridView(), order+1);

  H1ExSpaces exSpaces(&temperatureSpace,&spaceEx);

  std::string exVarNames[2] = { "l", "e"};
  ExVariableSet exVariableSet(exSpaces, exVarNames);

  EstAssembler estAssembler(gridManager,exSpaces);

  auto const& is = gridManager.grid().leafIndexSet();

  VariableSet::VariableSet x(variableSet), dx(variableSet), tmp(variableSet);

  double rTolX = 2.0e-3, aTolX = 2.0e-3, minRefine = 0.2;

  std::vector<std::pair<double,double> > tolX(variableSet.noOfVariables);
  std::vector<std::pair<double,double> > tolXC(variableSet.noOfVariables);
  for (int i=0; i<tolX.size(); ++i)
  {
    tolX[i] = std::make_pair(aTolX,rTolX);
    tolXC[i] = std::make_pair(aTolX/100,rTolX/100);
  }
  std::cout << "Accuracy: rTol = " << rTolX << ", aTol = " << aTolX << std::endl;
  std::cout << "Minimal number of cells to refine in an adaptive step: " << minRefine *100
            << " % of all" << std::endl << std::endl;

  int refSteps = -1;
  bool accurate = false;
  int iteSteps = getParameter(pt, "solver.iteMax", 1000);
  double iteEps = getParameter(pt, "solver.iteEps", 1.0e-6);
  if (!direct)
  {
    std::cout << "max. number of iterations in iterative solver = " << iteSteps << std::endl;
    std::cout << "requested accuracy in iterate solver = " << iteEps << std::endl << std::endl;
  };
  Dune::InverseOperatorResult res;
  double errNorm = 0;

  size_t size = variableSet.degreesOfFreedom(0,1);
  double gamma = 1.0, d = dim;
  double beta = 1.0/sqrt(d*gamma);
  double alpha = (d*gamma-1.0)/(d*(1.0+gamma));
  double qk = 1.0, dNk = size, zk = 0.0;
  double requested = sqrt(1-beta*beta)*tolX[0].first;
  double safety = 1.0;
  double yk = pow(dNk,alpha);
  //printf("gamma=%e, d=%e, beta=%e, alpha=%e, requested=%e, safety=%e\n",
  //        gamma,d,beta,alpha,requested,safety);

  do
  {
    refSteps++;

    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    solution = 0;
    CoefficientVectors hilfe(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    hilfe = 0;
    assembler.assemble(linearization(F,x));
    CoefficientVectors rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);

    if (direct)
    {
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    }
    else
    {
      switch (iterateType)
      {
      case IterateType::CG:
      {
        if ( verbosity>0 ) std::cout << "preconditioned cg solver is used" << std::endl;
        JacobiPreconditioner<AssOperator> jacobi(A,1.0);
        const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
        StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
        int lookAhead = getParameter(pt, "solver.lookAhead", 3);
        termination.setLookAhead(lookAhead);
        CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity-1);
        cg.apply(hilfe,rhs,res);
      }
      break;
      case IterateType::PCG:
      {
        if ( verbosity>0) std::cout << "   preconditioned cascadic multigrid solver I is used" << std::endl;
        JacobiPreconditioner<AssOperator> jacobi(A,1.0);
        NMIIIPCGSolver<LinearSpace> pcg(A,jacobi,iteEps,iteSteps,verbosity-1);
        pcg.apply(hilfe,rhs,res);
      }
      break;
      case IterateType::APCG:
      {
        if ( verbosity>0) std::cout << "   preconditioned cascadic multigrid solver II is used" << std::endl;
        int addedIterations = getParameter(pt, "solver.APCG.addedIterations", 10);
        JacobiPreconditioner<AssOperator> jacobiPCG(A,1.0);
        NMIIIAPCGSolver<LinearSpace> apcg(A,jacobiPCG,iteEps,iteSteps,verbosity-1,addedIterations);
        apcg.apply(hilfe,rhs,res);
      }
      break;
      default:
        std::cout << "Solver not available" << std::endl;
        throw -111;
      }
      solution.axpy(-1,hilfe);
    }
    dx.data = solution.data;

    // Do hierarchical error estimation. Remember to provide the very same underlying problem to the
    // error estimator functional as has been used to compute dx (do not modify x!).
    std::vector<double> errorDistribution(is.size(0),0.0);
    double maxErr = 0.0;
    double errLevel = 0.0;

    if (!tolX.empty())
    {
      tmp *= 0 ;
      estAssembler.assemble(ErrorEstimator(LinearizationAt<Functional>(F,x),dx));
      constexpr int estNvars = ErrorEstimator::AnsatzVars::noOfVariables;
      constexpr int estNeq = ErrorEstimator::TestVars::noOfVariables;
      size_t  estNnz = estAssembler.nnz(0,estNeq,0,estNvars,false);
      size_t  estSize = exVariableSet.degreesOfFreedom(0,estNvars);

      std::vector<int> estRidx(estNnz), estCidx(estNnz);
      std::vector<double> estData(estNnz), estRhs(estSize), estSolVec(estSize);
      estAssembler.toSequence(0,estNeq,estRhs.begin());

      // iterative solution of error estimator

      AssEstOperator agro(estAssembler);
      Dune::InverseOperatorResult estRes;
      ExCoefficientVectors estRhside(estAssembler.rhs());
      ExCoefficientVectors estSol(ExVariableSet::CoefficientVectorRepresentation<0,1>::init(exSpaces));
      estSol = 1.0 ;
      JacobiPreconditioner<AssEstOperator> jprec(agro, 1.0);
      jprec.apply(estSol,estRhside); //single Jacobi iteration
      estSol.write(estSolVec.begin());

      // Transfer error indicators to cells.
      for (auto ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
      {
        double err = 0;
        auto gix = spaceEx.mapper().globalIndices(*ci);
        for (auto j=gix.begin(); j!=gix.end(); ++j)
          err += fabs(component<0>(estSol)[*j]);
        errorDistribution[is.index(*ci)] = err;
        if (fabs(err)>maxErr) maxErr = fabs(err);
      }

      errLevel = 0.5*maxErr;
      if (minRefine>0.0)
      {
        std::vector<double> eSort(errorDistribution);
        std::sort(eSort.begin(),eSort.end(),compareAbs);
        int minRefineIndex = minRefine*(eSort.size()-1);
        double minErrLevel = fabs(eSort[minRefineIndex])+1.0e-15;
        if (minErrLevel<errLevel)
          errLevel = minErrLevel;
      }

      errNorm = 0 ;
      for (int k=0; k < estRhs.size() ; k++ )
        errNorm +=  estRhs[k]*estSolVec[k] ;
    }
    errNorm = sqrt(errNorm) ;
    // apply the Newton correction here
    x += dx;

    if ( verbosity>0)
            std::cout << "step = " << refSteps << ": " << size << " points,   ||estim. err|| = " << std::scientific << std::setprecision(3) << errNorm << std::resetiosflags( ::std::ios::scientific ) << std::setprecision(6) << std::endl;

	// graphical output of solution, mesh is not yet refined
    std::ostringstream fn;
    fn << "graph3d/peak-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(x,fn.str(),IoOptions().setOrder(order).setPrecision(7));
    std::cout << "   output written to file " << fn.str() << std::endl;

    if (refSteps>maxAdaptSteps)
    {
      std::cout << "max. number of refinement steps is reached" << std::endl;
      break;
    }

	// Evaluation of (global/local) error estimator information
    if (!tolX.empty())
    {
      if (errNorm<requested)
      {
        accurate = true ;
        std::cout << "||estim. error|| is smaller than requested" << std::endl;
      }
      else
      {
        // Refine mesh.

        int noToRefine = 0;
        double alphaSave = 1.0 ;
        std::vector<bool> toRefine( is.size(0), false ) ; //for adaptivity in compression
        std::vector< std::vector<bool> > refinements;
        for (auto ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
          if (fabs(errorDistribution[is.index(*ci)]) >= alphaSave*errLevel)
          {
            noToRefine++;
            toRefine[is.index(*ci)] = true ;
            gridManager.mark(1,*ci);
          }

        refinements.push_back(toRefine);
        accurate = !gridManager.adaptAtOnce();
      }
    }

    size = variableSet.degreesOfFreedom(0,1);
    qk = size/dNk;
    dNk = size;
    yk += pow(dNk,alpha);
    zk = pow(dNk,alpha)*(pow(errNorm/requested,d*alpha)-pow(qk,alpha))/(pow(qk,alpha)-1.0);
    iteEps = safety*beta*errNorm*yk/(yk+zk);

  } while (!accurate);

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End peak source problem" << std::endl;
}


#include <matplotlibcpp.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>

namespace plt = matplotlibcpp;
using namespace Eigen;

int main(){
    using std::cout;
    using std::endl;
    using std::vector;

    VectorXd x = Eigen::VectorXd::LinSpaced(200, 0, 6);
    VectorXd y;

    y = x.array().sin().exp().matrix();

    vector<double> x_vec(200), y_vec(200);

    // 直接EigenのVectorを扱えないので、STLのコンテナに移し替えて使う
    Map<VectorXd>(&x_vec[0], 200) = x;
    Map<VectorXd>(&y_vec[0], 200) = y;

    plt::named_plot("sample", x_vec, y_vec, "--b");
    plt::grid(true);
    plt::legend();
    plt::save("./ch3/sample_eigen.png");

    return 0;
}


#include "Circle2D.h"
#include "Line2D.h"
#include "Pose2D.h"
#include "Geometry.h"

#include <cmath>
#include <boost/concept_check.hpp>

using namespace A2O;
using namespace Eigen;

Circle2D::Circle2D()
      : _origin(0, 0), _radius(1)
{
}

Circle2D::Circle2D(const double& xOrigin,
	       const double& yOrigin,
	       const double& radius)
      : _origin(xOrigin, yOrigin), _radius(radius >= 0 ? radius : -radius)
{
}

Circle2D::Circle2D(const Vector2d& origin,
	       const double& radius)
      : Circle2D(origin(0), origin(1), radius)
{
}

Circle2D::Circle2D(const Vector2d& origin,
		   const Vector2d& point)
      : _origin(origin), _radius(Geometry::getDistance(origin, point))
{
}

Circle2D::Circle2D(const double& xOrigin,
	       const double& yOrigin,
	       const double& px,
	       const double& py)
      : _origin(xOrigin, yOrigin), _radius(Geometry::getDistance(xOrigin, yOrigin, px, py))
{
}

Circle2D::Circle2D(const Pose2D tangent, Vector2d point)
{
  // transform point in to an tangent touching point local coordinate system (tangent angle = x-axis + pi)
  Eigen::Vector2d tmpVector = point - tangent.getPosition();
  Eigen::Vector2d rotatedVector(0,0);
  Angle::rotate(tmpVector, - tangent.getAngle().rad(), rotatedVector);
  // calculate y-coordinate of the circle origin
  double my = (- pow(rotatedVector(0), 2) - pow(rotatedVector(1), 2) ) / (-2 * rotatedVector(1));
  // transform back to world coordinate system
  Eigen::Vector2d shiftedCircleOrigin(0,0);
  Angle::rotate(Eigen::Vector2d(0, my), tangent.getAngle().rad(), shiftedCircleOrigin);
  _origin = shiftedCircleOrigin + tangent.getPosition();
  _radius = Geometry::getDistance(_origin, tangent.getPosition());
}

Circle2D::~Circle2D()
{
}

const Vector2d& Circle2D::origin() const
{
  return _origin;
}

const double& Circle2D::radius() const
{
  return _radius;
}

const bool Circle2D::checkInnerTouching(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return distance > 0
      && (_radius == distance + other._radius || other._radius == distance + _radius);
}

const bool Circle2D::checkOuterTouching(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return _radius + other._radius == distance;
}

const bool Circle2D::checkIntersect(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return distance > 0
      && distance < _radius + other._radius
      && _radius < distance + other._radius
      && other._radius < distance + _radius;
}

const bool Circle2D::checkInnerCircle(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return _radius > distance + other._radius || other._radius > distance + _radius;
}

const double Circle2D::getDistanceTo(const Circle2D& other) const
{
  return Geometry::getDistance(_origin, other._origin);
}

const Vector2d Circle2D::getPointOnCircle(const Angle& angle) const
{
  return Vector2d(_origin(0) + (std::cos(angle.rad()) * _radius),
		  _origin(1) + (std::sin(angle.rad()) * _radius));
}

const bool Circle2D::isOnCircle(const Vector2d& point) const
{
  double distance = Geometry::getDistance(_origin, point);
  double diff = std::abs(distance - _radius);
  return (diff < 0.000001);
}

const Angle Circle2D::getAngle(const Vector2d& point)
{
  Vector2d shiftedPoint = point - _origin;
  // point is equal to origin
  if(std::fabs(point(0)- _origin(0)) < 0.001 && std::fabs(point(1)- _origin(1)) < 0.001)
    return Angle(0);
  // threshold x = 0
  if(std::fabs<double>(shiftedPoint(0)) < 0.0001)
  {
    if(shiftedPoint(1) > 0)
      return Angle(M_PI * 0.5);
    else
      return Angle(-M_PI * 0.5);
  }

  double angle = atan2(shiftedPoint(1), shiftedPoint(0));
  return Angle(angle);
}

const Angle Circle2D::getAngle(const double& segmentLength)
{
  return Angle(segmentLength / _radius);
}

Pose2D Circle2D::getClosestPose(const Vector2d& point)
{
 Angle angle = Angle::to(point - _origin);
 Vector2d closestPoint = getPointOnCircle(angle);
 return Pose2D(closestPoint, slope(closestPoint));
}

Angle Circle2D::slope(const Vector2d& point)
{
  // calculate slope on upper semicircle
  double slope = -0.5 * sqrt(1 / (pow(_radius, 2) - pow(point(0) - _origin(0), 2))) * (2 * point(0) - 2 * _origin(0));
  // check if point is on lower semicircle
  if(_origin(1) > point(1))
  {
    slope *= -1;
  }
  return Angle(atan(slope));
}

short Circle2D::getDirectionOfRotation(const Vector2d& point, const Angle& angle)
{
  Angle angle2 = getAngle(point);
  angle2 += Angle(M_PI * 0.5);
  double angleResult  = std::fabs<double>(angle2.rad() - angle.rad()) + M_PI * 0.5;

  if(angleResult < M_PI || angleResult > 2 * M_PI)
    return 1;
  else
    return -1;
}

short int Circle2D::getDirectionOfRotation(const Pose2D pose)
{
  return getDirectionOfRotation(pose.getPosition(), pose.getAngle());
}

std::vector< Vector2d > Circle2D::getTrail(const Vector2d& start, const Vector2d& end)
{
  Angle startAngle = getAngle(start);
  Angle endAngle = getAngle(end);
  Angle diff = endAngle - startAngle;
  Angle delta =  Angle(diff.rad() / 10);
  Vector2d point;
  std::vector<Vector2d> points;
  int i = 0;
  do{
    startAngle += delta;
    point = getPointOnCircle(startAngle);
    points.push_back(point);
  } while(++i < 10);
  return points;
}

bool Circle2D::operator==(const Circle2D& other) const
{
  return _origin == other._origin && _radius == other._radius;
}

bool Circle2D::operator!=(const Circle2D& other) const
{
  return !(*this == other);
}

const Circle2D Circle2D::avg(const Circle2D& c1,
			     const Circle2D& c2,
			     const double& weight1,
			     const double& weight2)
{
  Vector2d avgOrigin = Geometry::avg(c1._origin, c2._origin, weight1, weight2);

  return Circle2D(avgOrigin, c1._radius * weight1 + c2._radius * weight2);
}

const Circle2D Circle2D::avg(const std::vector<Circle2D>& circles)
{
  size_t size = circles.size();
  if (size == 0) {
    return Circle2D(0, 0, 1);
  } else if (size == 1) {
    return circles[0];
  }

  double x = 0;
  double y = 0;
  double radius = 0;

  for (Circle2D circle : circles) {
    x += circle._origin(0);
    y += circle._origin(1);
    radius += circle._radius;
  }

  return Circle2D(x / size, y / size, radius / size);
}


#include <benchmark/benchmark.h>

#include <boost/numeric/ublas/matrix.hpp>

#include <mitrax/dim.hpp>

#include <random>
#include <string>

#include "../../../include/get_binaryop.hpp"

using namespace mitrax;
using namespace mitrax::literals;

template < typename T, typename Op >
[[gnu::noinline]]
void bm(benchmark::State& state, Op op, rt_dim_pair_t d1, rt_dim_pair_t d2){
	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_int_distribution< T > dis(
		std::numeric_limits< T >::min(),
		std::numeric_limits< T >::max()
	);

	boost::numeric::ublas::matrix< T > m1(size_t(d1.cols()), size_t(d1.rows()));
	for(size_t y = 0; y < m1.size2(); ++y){
		for(size_t x = 0; x < m1.size1(); ++x){
			m1(x, y) = dis(gen);
		}
	}

	boost::numeric::ublas::matrix< T > m2(size_t(d2.cols()), size_t(d2.rows()));
	for(size_t y = 0; y < m2.size2(); ++y){
		for(size_t x = 0; x < m2.size1(); ++x){
			m2(x, y) = dis(gen);
		}
	}

	while(state.KeepRunning()){
		boost::numeric::ublas::matrix< T > res = op(m1, m2);
		benchmark::DoNotOptimize(res);
	}
}

#include <boost/hana/tuple.hpp>
#include <boost/hana/for_each.hpp>

namespace init{

	constexpr auto dimensions = boost::hana::make_tuple(
			dim_pair(2_CS, 2_RS),
			dim_pair(4_CS, 2_RS),
			dim_pair(8_CS, 2_RS),
			dim_pair(8_CS, 4_RS),
			dim_pair(8_CS, 8_RS),
			dim_pair(8_CS, 16_RS),
			dim_pair(8_CS, 32_RS),
			dim_pair(8_CS, 64_RS),
			dim_pair(16_CS, 64_RS),
			dim_pair(32_CS, 64_RS),
			dim_pair(64_CS, 64_RS),
			dim_pair(128_CS, 64_RS),
			dim_pair(256_CS, 64_RS),
			dim_pair(256_CS, 128_RS),
			dim_pair(256_CS, 256_RS)
		);

	using plus = std::plus<>;
	struct multiplies{
		template < typename T, typename U>
		constexpr decltype(auto) operator()(T&& lhs, U&& rhs)const{
			return boost::numeric::ublas::prod(
				static_cast< T&& >(lhs),
				static_cast< U&& >(rhs)
			);
		}
	};

}

#include "main.hpp"


#include "clear/Hungarian.h"
#include "clear/blockSVD.hpp"
#include "clear/MultiwayMatcher.hpp"

#include <cassert>
#include <fstream>
#include <ros/console.h>
#include <Eigen/SVD>
#include <Eigen/SparseLU>
#include <ctime>
#include <ratio>
#include <chrono>

using std::vector;
using namespace std::chrono;
using Eigen::MatrixXf;

MultiwayMatcher::MultiwayMatcher(){}

MultiwayMatcher::~MultiwayMatcher(){}

void MultiwayMatcher::initialize(Eigen::MatrixXf A, vector<unsigned> numSmp){
	high_resolution_clock::time_point t1,t2;
	duration<double, std::milli> elapsed_time;

	t1 = high_resolution_clock::now();
	A_sp = A.sparseView();

	// A_ = A;
	numSmp_ = numSmp;

	// compute cumsum_
	cumSum_.resize(numSmp_.size(), 0);
	std::partial_sum(numSmp_.begin(), numSmp_.end(), cumSum_.begin());
	assert(cumSum_.back() == A.rows());

	// compute other matrices needed
	construct_D();
	construct_L();
	construct_Lnrm();
	t2 = high_resolution_clock::now();
	elapsed_time = t2-t1;
	if(verbose_) ROS_INFO_STREAM("Initialization time: " << (float) elapsed_time.count() / 1000.0 << " seconds.");

}

void MultiwayMatcher::set_verbose(bool verbose){
	verbose_ = verbose;
}

void MultiwayMatcher::estimate_universe_size(){

	// singular value decomposition on Lnrm_
	// Eigen::JacobiSVD<MatrixXf> svd(Lnrm_, Eigen::ComputeFullU | Eigen::ComputeFullV);
	// Vl_ = svd.matrixV();
	// sl_ = svd.singularValues(); // a column vector of singular values in decreasing order

	blockSVD(MatrixXf(A_sp), Lnrm_, Vl_, sl_);

	m_ = 0;
	for(unsigned i = 0; i < sl_.rows(); ++i){
		if (sl_(i) < thresh_) m_++;
	}

	vector<unsigned>::iterator max_iterator;
	max_iterator = std::max_element(numSmp_.begin(), numSmp_.end());
	// Minimum # of objects must not be less than max # of samples
	m_ = std::max(*max_iterator, m_);

	if (verbose_) ROS_INFO_STREAM("Estimated size of universe: " << m_);

	fused_counts_.resize(m_, 0);

}

void MultiwayMatcher::CLEAR(){
	// runtime analysis
	high_resolution_clock::time_point t1,t2;
	duration<double, std::milli> elapsed_time;

	t1 = high_resolution_clock::now();
	estimate_universe_size();
	t2 = high_resolution_clock::now();
	elapsed_time = t2-t1;
	if(verbose_) ROS_INFO_STREAM("SVD elapsed time: " << (float) elapsed_time.count() / 1000.0 << " seconds.");

	// recover embedding matrix
	N_ = Vl_.block(0, Vl_.cols()-m_, Vl_.rows(), m_);
	assert(N_.rows() == Vl_.rows());
	assert(N_.cols() == m_);

	// L2 normalize each row
	for (unsigned i = 0 ; i < N_.rows(); ++i){
		N_.row(i) = N_.row(i) / N_.row(i).norm();
	}

	if(verbose_) ROS_INFO_STREAM("Finding cluster centers...");
	// find cluster centers (LU)
	// t1 = high_resolution_clock::now();
	// Eigen::FullPivLU<MatrixXf> LUsolver(N_);
	// C_ = LUsolver.permutationP() * N_;
	// C_ = C_.block(0, 0, m_, m_); // each row is a cluster center
	// t2 = high_resolution_clock::now();
	// elapsed_time = t2-t1;
	// if(verbose_) ROS_INFO_STREAM("LU elapsed time: " << (float) elapsed_time.count() / 1000.0 << " seconds.");

	// find cluster centers (greedy)
	t1 = high_resolution_clock::now();
	C_ = MatrixXf::Zero(m_,m_);
	vector<bool> is_pivot(N_.rows(), false);
	vector<unsigned> pivots;
	C_.row(0) = N_.row(0);
	is_pivot[0] = true;
	pivots.push_back(0);
	MatrixXf X = N_ * N_.transpose();
	X = X.array().abs().matrix(); // compute coefficient-wise absolute values
	MatrixXf scores = MatrixXf::Zero(N_.rows(),1); // initialize score vector
	for (unsigned k = 1; k < m_; ++k){
		// incremental update to scores
		scores = scores + X.col(pivots[k-1]);
		// find k-th cluster center
		double min_score = N_.rows();
		int min_idx = -1;
		// find row with best score
		for (unsigned i = 0; i < N_.rows(); ++i){
			if (is_pivot[i]) continue;
			if(scores(i) < min_score){
				min_score = scores(i);
				min_idx = i;
			}
		}
		assert(min_idx >= 0);
		C_.row(k) = N_.row(min_idx);
		is_pivot[min_idx] = true;
		pivots.push_back(min_idx);
	}
	t2 = high_resolution_clock::now();
	elapsed_time = t2-t1;
	if(verbose_) ROS_INFO_STREAM("Greedy elapsed time: " << (float) elapsed_time.count() / 1000.0 << " seconds.");

	// scores based on dot product
	MatrixXf F = - N_ * C_.transpose();

	assignments_.resize(N_.rows(), -1);

	// go through each agent
	t1 = high_resolution_clock::now();
	if(verbose_) ROS_INFO_STREAM("Clustering...");
	for (unsigned agent = 0; agent < numSmp_.size(); ++agent){
		unsigned size = numSmp_[agent];
		unsigned start_idx = 0;
		if(agent > 0) start_idx = cumSum_[agent-1];
		MatrixXf cost = F.block(start_idx,0,size,m_);

		// convert from Matrix to std::vector (TODO: improve)
		vector<vector<double>> costMatrix;
		for (unsigned i = 0; i < size; ++i){
			// copy row i
			vector<double> row;
			for (unsigned j = 0; j < m_; ++j){
				row.push_back(cost(i,j));
			}
			costMatrix.push_back(row);
		}

		// assign to universe using Hungarian
		/*
		vector<int> a;
		HungarianAlgorithm Hungarian;
		Hungarian.Solve(costMatrix, a);

		// copy results to assignments vector
		for (unsigned i = 0; i < size; ++i){
			assert(a[i] >= 0);
			assignments_[start_idx + i] = a[i];
			fused_counts_[a[i]] = fused_counts_[a[i]] + 1;
		}
		*/

		for (unsigned i = 0; i < size; ++i) {
			Eigen::Index minIdx;
			auto const best_match = cost.row(i).minCoeff(&minIdx);
			assignments_[start_idx + i] = minIdx;
			fused_counts_[minIdx] += 1;
		}
	}
	t2 = high_resolution_clock::now();
	elapsed_time = t2-t1;
	if(verbose_) ROS_INFO_STREAM ("Hungarian elapsed time: " << (float) elapsed_time.count() / 1000.0 << " seconds.");

	// recover lifting permutation
	// if(verbose_) ROS_INFO_STREAM("Recovering lifting permutation...");
	// Y_ = MatrixXf::Zero(N_.rows(), m_);
	// // go through each observation
	// for (unsigned i = 0; i < Y_.rows(); ++i){
	// 	Y_(i, assignments_[i]) = 1;
	// }

	// // recover pairwise matches
	// if(verbose_) ROS_INFO_STREAM("Recovering pairwise associations...");
	// X_ = Y_ * Y_.transpose();
}

void MultiwayMatcher::get_X(Eigen::MatrixXf& X){
	X = X_;
}

void MultiwayMatcher::get_Y(Eigen::MatrixXf& Y){
	Y = Y_;
}

void MultiwayMatcher::get_assignments(vector<int>& assignments){
	assignments = assignments_;
}

void MultiwayMatcher::get_fused_counts(vector<unsigned>& fused_counts){
	fused_counts = fused_counts_;
}

void MultiwayMatcher::construct_D(){
	// D_ = MatrixXf::Zero(A_.rows(), A_.cols());
	// for (unsigned i = 0; i < A_.rows(); ++i){
	// 	D_(i,i) = A_.row(i).sum();
	// }

	D_sp.resize(A_sp.rows(),A_sp.cols());
	D_sp.reserve(A_sp.rows());
	for (int i = 0; i < A_sp.rows(); ++i){
		D_sp.insert(i,i) = A_sp.col(i).sum();
	}
}

void MultiwayMatcher::construct_L(){
	// L_ = D_ - A_;
	L_sp = D_sp - A_sp;
	// make symmetric
	// L_ = (L_ + L_.transpose())/2.0;
}

void MultiwayMatcher::construct_Lnrm(){
	// MatrixXf N = D_;
	// for (unsigned i = 0; i < D_.rows(); ++i){
	// 	double degree = D_(i,i);
	// 	assert(degree >= 0);
	// 	N(i,i) = 1 / std::sqrt((degree + 1)); // plus one to avoid dividing by zeros
	// }
	// Lnrm_ = N * L_ * N;

	Eigen::SparseMatrix<float> N_sp;
	N_sp.resize(L_sp.rows(), L_sp.cols());
	N_sp.reserve(L_sp.rows());
	for (int i = 0; i < D_sp.rows(); ++i){
		N_sp.insert(i,i) = 1 / std::sqrt((D_sp.coeffRef(i,i) + 1)); // plus one to avoid dividing by zeros
	}
	Lnrm_sp = N_sp * L_sp * N_sp;
	Lnrm_ = MatrixXf(Lnrm_sp);
}

/*
Y.T.
Dump all data for debugging purpose
*/
void MultiwayMatcher::save_data(){

	// std::string adj_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARA.txt";
	// std::ofstream A_file(adj_filename.c_str());
	// if (A_file.is_open()){
	// 	A_file << A_;
	//     A_file.close();
	// }

	// std::string numsmp_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARnumsmp.txt";
	// std::ofstream nsmp_file(numsmp_filename.c_str());
	// if (nsmp_file.is_open()){
	// 	for (const auto &e : numSmp_) nsmp_file << e << "\n";
	//     nsmp_file.close();
	// }

	// std::string lnrm_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARLnrm.txt";
	// std::ofstream Lnrm_file(lnrm_filename.c_str());
	// if (Lnrm_file.is_open()){
	// 	Lnrm_file << Lnrm_;
	//     Lnrm_file.close();
	// }

	// std::string clearsl_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARsl.txt";
	// std::ofstream sl_file(clearsl_filename.c_str());
	// if (sl_file.is_open()){
	// 	sl_file << sl_;
	//     sl_file.close();
	// }

	// std::string clearVl_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARVl.txt";
	// std::ofstream vl_file(clearVl_filename.c_str());
	// if (vl_file.is_open()){
	// 	vl_file << Vl_;
	//     vl_file.close();
	// }

	// std::string clearx_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARX.txt";
	// std::ofstream X_file(clearx_filename.c_str());
	// if (X_file.is_open()){
	// 	X_file << X_;
	//     X_file.close();
	// }
	// std::string cleary_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARY.txt";
	// std::ofstream Y_file(cleary_filename.c_str());
	// if (Y_file.is_open()){
	// 	Y_file << Y_;
	//     Y_file.close();
	// }
	// std::string clearc_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARC.txt";
	// std::ofstream C_file(clearc_filename.c_str());
	// if (C_file.is_open()){
	// 	C_file << C_;
	//     C_file.close();
	// }
	// std::string clearn_filename = "/home/yulun/srtc/src/srtc_map_merge/CLEARN.txt";
	// std::ofstream N_file(clearn_filename.c_str());
	// if (N_file.is_open()){
	// 	N_file << N_;
	//     N_file.close();
	// }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FAST_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FAST_HYPOT_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/fast_hypot.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_greater_equal.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::fast_hypot(tofloat(a0), tofloat(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      A0 x =  boost::simd::abs(a0);
      A0 y =  boost::simd::abs(a1);
      bA0 gtyx = gt(y,x);
      A0 xx = select(gtyx,y,x);
      A0 yy = select(gtyx,x,y);
      A0 r =  xx*sqrt(One<A0>()+sqr(yy/xx));
      return select(ge(xx*Eps<A0>(), yy), xx, r);
   }
  };
} } }

#endif


// Before doing anything else, grab in the definitions of the test
// space and trial spaces that make up the finite element system.
#include <qdove/base/test_space.h>
#include <qdove/base/trial_space.h>
#include <qdove/materials/constants.h>

// Start by solving Schroedinger's problem
#include <qdove/models/schroedinger.h>

// Let's use a function from the qdove library for a psudo potential.
#include <qdove/psuedopotentials/function_library.h>

// Also make use of the predefined generalised eigenspectrum system -
// we need this for the Schroedinger problem.
#include <qdove/generic_linear_algebra/eigenspectrum_system.h>

// Next up are some deal.II objects that have not been generalised
// away yet...
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/numerics/data_out.h>

// C++
#include <cassert>
#include <iostream>
#include <fstream>

// The purpose of this example is to solve a problem with a known
// analytical solution.
template<int dim>
class SchroedingerProblem
{
public:
  SchroedingerProblem (dealii::Triangulation<dim> &triangulation);
  ~SchroedingerProblem ();

  void write_gnuplot (const dealii::PETScWrappers::Vector &vector,
		      const std::string                   &name);
  void run ();

private:
  // The description of the finite element basis
  qdove::TrialSpace<dim> trial_space;

  // Geometry description and boundary constraints
  qdove::TestSpace<dim> test_space;

  // The eigenspectrum system that will be solved by SLEPc
  qdove::Schroedinger::Problem<dim> schroedinger_problem;

  // The eigenpairs from schroedinger's problem
  std::vector<dealii::PETScWrappers::Vector> eigenvectors;
  std::vector<double>                        eigenvalues;
};

template<int dim>
SchroedingerProblem<dim>::SchroedingerProblem (dealii::Triangulation<dim> &triangulation)
  :
  trial_space (triangulation),
  test_space (trial_space),
  schroedinger_problem (trial_space, test_space)
{}

template<int dim>
SchroedingerProblem<dim>::~SchroedingerProblem ()
{}

template<int dim>
void
SchroedingerProblem<dim>::write_gnuplot (const dealii::PETScWrappers::Vector &vector,
					 const std::string                   &name)
{
  // Output a vector to gnuplot style file.
  std::ostringstream filename;
  filename << "solution-" << name << ".gpl";
  std::ofstream output (filename.str ().c_str ());

  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler (test_space.dofs ());
  data_out.add_data_vector (vector, name);

  // generate default patches and output.
  data_out.build_patches ();
  data_out.write_gnuplot (output);
}

template<int dim>
void
SchroedingerProblem<dim>::run ()
{
  std::cout << "Test space:" << std::endl
	    << "   Finite element type:          "
	    << test_space.fe ().get_name ()
	    << std::endl
	    << "   Number of degrees of freedom: "
	    << test_space.n_dofs ()
	    << std::endl;

  const double radius = 50e-10;

  dealii::PETScWrappers::Vector material_function (test_space.n_dofs ());
  for (unsigned int i=0; i<material_function.size (); ++i)
    material_function[i] = 1.;
  write_gnuplot (material_function, "material_function");

  // Get started on Schroedinger's problem
  std::cout << "Schroedinger's problem:"
	    << std::endl;
  schroedinger_problem.reinit ();

  // set up the effective mass
  dealii::PETScWrappers::Vector eff_mass (test_space.n_dofs ());
  const double ke_term = (qdove::HBAR*qdove::HBAR) / (2.*qdove::mstar_p_CdTe*qdove::M0);
  std::cout << "   Kinetic energy term:          "
	    << ke_term << std::endl;
  eff_mass.add (ke_term, material_function);
  write_gnuplot (eff_mass, "effective_mass");

  // set up the potential
  dealii::PETScWrappers::Vector potential (test_space.n_dofs ());

  schroedinger_problem.assemble (eff_mass, potential);
  schroedinger_problem.solve ();
  schroedinger_problem.get_solution_eigenpairs (eigenvalues, eigenvectors);
  write_gnuplot (eigenvectors[0], "electron_function");

  // output
  std::cout << "   Eigenvalues:                  ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    std::cout << eigenvalues[i] << " ";
  std::cout << std::endl;

  std::cout << "   Scaled values (eV):           ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    std::cout << eigenvalues[i]/(1e-03*qdove::E0) << " ";
  std::cout << std::endl;

  std::cout << "   Scaled analytic values:       ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    {
      // This equation is equation 3.13 in Harrisson's book. It reads:
      // E_n = (hbar*pi*n)^2 / (2mL^2)
      const unsigned int n          = i+1;
      const double L                = 2.*radius;
      const double analytical_value = (qdove::HBAR*qdove::HBAR*qdove::PI*qdove::PI*n*n) / (2*qdove::mstar_p_CdTe*qdove::M0*L*L);
      std::cout << analytical_value/(1e-03*qdove::E0) << " ";
    }
  std::cout << std::endl;
}

int main (int argc, char **argv)
{
  try
    {
      dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);
      {
	// Create a grid
	dealii::Triangulation<1> triangulation;
	dealii::GridGenerator::hyper_cube (triangulation, -50e-10, 50e-10);
	triangulation.refine_global (9);

	// Run Schroedinger's problem on that grid
	SchroedingerProblem<1> schroedinger_problem (triangulation);
	schroedinger_problem.run ();
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#pragma once

#include <Eigen/Dense>

namespace vision_filter {
/**
 * Abstract class that should be inherited from to create specific
 * kalman filters
 *
 * Must initialize:
 *  x_k1_k1, x_k_k1, x_k_k,
 *  P_k1_k1, P_k_k1, P_k_k,
 *  F_k, B_k, H_k,
 *  Q_k, R_k
 *
 * Every predict, must update:
 *  u_k
 *
 * Every PredictWithUpdate, must update:
 *  u_k, z_k
 *
 * The most recently updated values
 *  x_k_k, P_k_k
 *
 * Taken from https://en.wikipedia.org/wiki/Kalman_filter
 *
 * Conversion between code notation and wiki notation is...
 * x_k1_k1 is X_(k-1, k-1)
 * x_k_k is X_(k, k)
 * etc
 */
class KalmanFilter {
public:
    /**
     * Creates a general kalman filter with the given sizes
     * Use a child class to setup the specific state matricies
     * Assumes 1 input
     *
     * @param state_size The size of the state vector
     * @param observation_size The size of the observation vector
     */
    KalmanFilter(unsigned int state_size, unsigned int observation_size)
        : x_k1_k1_(state_size),
          x_k_k1_(state_size),
          x_k_k_(state_size),
          u_k_(1),
          z_k_(observation_size),
          y_k_k1_(observation_size),
          y_k_k_(observation_size),
          P_k1_k1_(state_size, state_size),
          P_k_k1_(state_size, state_size),
          P_k_k_(state_size, state_size),
          S_k_(observation_size, observation_size),
          K_k_(state_size, observation_size),
          F_k_(state_size, state_size),
          B_k_(state_size, 1),
          H_k_(observation_size, state_size),
          Q_k_(state_size, state_size),
          R_k_(observation_size, observation_size),
          identity_(Eigen::MatrixXd::Identity(state_size, state_size)) {}

    /**
     * Predicts without update
     */
    void predict();

    /**
     * Predicts with update
     * z_k must be set with the observation
     */
    void predict_with_update();

protected:
    Eigen::VectorXd x_k1_k1_;
    Eigen::VectorXd x_k_k1_;
    Eigen::VectorXd x_k_k_;

    Eigen::VectorXd u_k_;
    Eigen::VectorXd z_k_;

    Eigen::VectorXd y_k_k1_;
    Eigen::VectorXd y_k_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k1_k1_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k_k1_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd S_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd K_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd F_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd B_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd H_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd Q_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd R_k_;

    Eigen::MatrixXd identity_;
};
}  // namespace vision_filter