//  Copyright Joel de Guzman 2002-2004. 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)
//  Hello World Example from the tutorial
//  [Joel de Guzman 10/9/2002]

#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <iostream>
using std::cout;

char const* greet()
{
   return "hello, world";
}

char const* ten()
{
	boost::math::chi_squared mydist(3);
	double p = boost::math::cdf(mydist,.660);
	cout << "p-value " << p;
	return "10";
}

BOOST_PYTHON_MODULE(hello_ext)
{
    using namespace boost::python;
    def("greet", greet);
    def("ten", ten);
}


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

#define BOOST_TEST_MODULE shacal2_cipher_test

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/block/shacal2.hpp>

#include <nil/crypto3/block/algorithm/encrypt.hpp>
#include <nil/crypto3/block/algorithm/decrypt.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::block;

BOOST_TEST_DONT_PRINT_LOG_VALUE(block::shacal2<256>::block_type)

BOOST_AUTO_TEST_SUITE(shacal_test_suite)

BOOST_AUTO_TEST_CASE(shacal2_single_block_encrypt1) {
    typedef block::shacal2<256> bct;

    // Test with the equivalent of SHA-256("")
    bct::block_type plaintext = {
        {0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19}};
    bct::key_type key = {{0x80000000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
    bct cipher(key);
    bct::block_type ciphertext = cipher.encrypt(plaintext);
    bct::block_type expected_ciphertext = {
        {0x79a6dddb, 0xdd946d8f, 0x5e8d0156, 0xf41fc3ea, 0xd69fef65, 0xc9962ac0, 0x8511bf70, 0x1c71eb3c}};

    BOOST_CHECK_EQUAL(ciphertext, expected_ciphertext);

    bct::block_type new_plaintext = cipher.decrypt(ciphertext);

    BOOST_CHECK_EQUAL(plaintext, new_plaintext);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <polyfem/ProblemWithSolution.hpp>

#include <Eigen/Dense>

#include <vector>
#include <string>

namespace polyfem
{
	class KernelProblem: public ProblemWithSolution
	{
	public:
		KernelProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override { assert(false); return AutodiffHessianPt(1); }
		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;
		bool is_scalar() const override;

	private:
		std::string formulation_ = "Laplacian";
		int n_kernels_ = 5;
		double kernel_distance_ = 0.05;
	};
}


#ifndef __SDT_IDEAL_HH__
#define __SDT_IDEAL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Sensor Data Transport module)
LIBRARY DEPENDENCY:
      ((../../src/SDT_ideal.cpp))
*******************************************************************************/
#include <armadillo>
#include <functional>
#include "sdt.hh"

class SDT_ideal : public SDT
{
    TRICK_INTERFACE(SDT_ideal);

public:
    SDT_ideal(unsigned int kl_in);

    virtual void init(LaunchVehicle *VehicleIn);
    virtual void algorithm(LaunchVehicle *VehicleIn);

private:
    arma::mat33 build_321_rotation_matrix(arma::vec3 angle);
};

#endif  //  __SDT_IDEAL_HH__


#define BOOST_TEST_MODULE "test_debye_huckel_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/global/DebyeHuckelPotential.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/util/logger.hpp>

BOOST_AUTO_TEST_CASE(DH_double)
{
    mjolnir::LoggerManager::set_default_logger("test_debye_huckel_potential.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;

    using boundary_type = traits_type::boundary_type;
    using system_type   = mjolnir::System<traits_type>;

    // to make the value realistic, we need to modify the unit system.
    using phys_type = mjolnir::physics::constants<real_type>;
    using unit_type = mjolnir::unit::constants<real_type>;

    phys_type::set_kB(unit_type::boltzmann_constant() *
                      1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
    phys_type::set_NA(unit_type::avogadro_constant());
    phys_type::set_eps0((unit_type::vacuum_permittivity() /
        unit_type::elementary_charge()) / unit_type::elementary_charge() *
        (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
        (1.0 / unit_type::m_to_angstrom()));

    phys_type::set_m_to_length(1e-10);
    phys_type::set_length_to_m(1e+10);
    phys_type::set_L_to_volume(1e+27);
    phys_type::set_volume_to_L(1e-27);

    constexpr std::size_t N = 10000;
    constexpr real_type   h = 1e-6;

    system_type sys(0, boundary_type{});
    sys.attribute("temperature")    = 300.0; // [K]
    sys.attribute("ionic_strength") =   0.1; // [M]

    mjolnir::Topology top(0);
    top.construct_molecules();

    const real_type charge = 1.0;
    mjolnir::DebyeHuckelPotential<traits_type> dh(
        mjolnir::DebyeHuckelPotential<traits_type>::default_cutoff(),
        {{0u, charge}, {1u, charge}}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
        );

    dh.initialize(sys, top);

    const real_type x_min = 0.5 * dh.debye_length();
    const real_type x_max = dh.max_cutoff_length();
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = dh.potential(0, 1, x + h);
        const real_type pot2 = dh.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = dh.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(DH_float)
{
    using traits_type = mjolnir::SimulatorTraits<float, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;

    using boundary_type = traits_type::boundary_type;
    using system_type   = mjolnir::System<traits_type>;

    using phys_type = mjolnir::physics::constants<real_type>;
    using unit_type = mjolnir::unit::constants<real_type>;

    phys_type::set_kB(unit_type::boltzmann_constant() *
        (1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant()));
    phys_type::set_NA(unit_type::avogadro_constant());
    phys_type::set_eps0((unit_type::vacuum_permittivity() /
        unit_type::elementary_charge()) / unit_type::elementary_charge() *
        (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
        (1.0 / unit_type::m_to_angstrom()));

    phys_type::set_m_to_length(1e-10);
    phys_type::set_length_to_m(1e+10);
    phys_type::set_L_to_volume(1e+27);
    phys_type::set_volume_to_L(1e-27);

    constexpr static std::size_t N = 1000;
    constexpr static real_type   h = 1e-2;

    system_type sys(0, boundary_type{});
    sys.attribute("temperature")    = 300.0; // [K]
    sys.attribute("ionic_strength") =   0.1; // [M]

    mjolnir::Topology top(0);
    top.construct_molecules();

    const real_type charge = 1.0;
    mjolnir::DebyeHuckelPotential<traits_type> dh(
        mjolnir::DebyeHuckelPotential<traits_type>::default_cutoff(),
        {{0u, charge}, {1u, charge}}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
        );

    dh.initialize(sys, top);

    const real_type x_min = 0.5 * dh.debye_length();
    const real_type x_max = dh.max_cutoff_length();
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = dh.potential(0, 1, x + h);
        const real_type pot2 = dh.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = dh.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}


#include <boost/geometry/formulas/flattening.hpp>


//
// Created by hhy on 2022/3/11.
//
#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <chrono>
#include <iostream>

using namespace std;

static uint64_t Now() {
  auto now = std::chrono::high_resolution_clock::now();
  return now.time_since_epoch().count();
}

int main() {
  OsqpEigen::Solver solver;

  auto t1 = Now();
  solver.settings()->setVerbosity(true);
  solver.settings()->setAlpha(1.0);

  int numOfVar = 2;
  int numOfCons = 5;

  solver.data()->setNumberOfVariables(numOfVar);
  solver.data()->setNumberOfConstraints(numOfCons);

  Eigen::SparseMatrix<double> hessian;
  Eigen::VectorXd gradient;
  Eigen::SparseMatrix<double> linearMatrix;
  Eigen::VectorXd lowerBound;
  Eigen::VectorXd upperBound;

  hessian.resize(numOfVar, numOfVar);
  gradient.resize(numOfVar);
  linearMatrix.resize(numOfCons, numOfVar);
  lowerBound.resize(numOfCons);
  upperBound.resize(numOfCons);

  hessian.insert(0, 0) = 1;
  hessian.insert(0, 1) = 0;
  hessian.insert(1, 0) = 0;
  hessian.insert(1, 1) = 0;
  std::cout << "hessian:\n" << hessian << "\n";

  gradient << 3, 4;
  std::cout << "gradient:\n" << gradient << "\n";

  linearMatrix.insert(0, 0) = -1;
  linearMatrix.insert(1, 0) = 0;
  linearMatrix.insert(2, 0) = -1;
  linearMatrix.insert(3, 0) = 2;
  linearMatrix.insert(4, 0) = 3;

  linearMatrix.insert(0, 1) = 0;
  linearMatrix.insert(1, 1) = -1;
  linearMatrix.insert(2, 1) = -3;
  linearMatrix.insert(3, 1) = 5;
  linearMatrix.insert(4, 1) = 4;

  std::cout << "linearMatrix:\n" << linearMatrix << "\n";

  lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY,
      -OsqpEigen::INFTY, -OsqpEigen::INFTY;
  upperBound << 0, 0, -15, 100, 80;
  std::cout << "lowerBound:\n" << lowerBound << "\n";
  std::cout << "upperBound:\n" << upperBound << "\n";

  assert(solver.data()->setHessianMatrix(hessian));
  assert(solver.data()->setGradient(gradient));
  assert(solver.data()->setLinearConstraintsMatrix(linearMatrix));
  assert(solver.data()->setLowerBound(lowerBound));
  assert(solver.data()->setUpperBound(upperBound));

  //  assert(InitMat(solver));
  assert(solver.initSolver());

  //  assert(solver.solve()== true);
  assert(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError);

  auto res = solver.getSolution();
  auto t2 = Now();
  std::cout << "use time: " << double(t2 - t1) / (1e6) << "ms" << std::endl;

  cout << res << endl;
}


#pragma once

#include <roerei/generic/sliced_sparse_matrix.hpp>
#include <roerei/generic/sparse_matrix.hpp>
#include <roerei/generic/encapsulated_vector.hpp>

#include <roerei/generic/common.hpp>

#include <boost/optional.hpp>

#include <vector>
#include <map>

#define invalid_value (std::numeric_limits<size_t>::max())

namespace roerei
{

template <typename M, typename N, typename T>
class compact_sparse_matrix_t
{
public:
	typedef M row_key_t;
	typedef N column_key_t;

	struct row_t
	{
		size_t const start, length;

		row_t(size_t _start, size_t _length)
			: start(_start)
			, length(_length)
		{}

		row_t()
			: start(invalid_value)
			, length(invalid_value)
		{}

		inline bool is_invalid() const
		{
			return start == invalid_value;
		}
	};

private:
	const size_t m, n;
	std::vector<std::pair<N, T>> buf;
	encapsulated_vector<M, row_t> rows;

public:
	template<typename MATRIX, typename ITERATOR>
	class row_proxy_base_t
	{
	public:
		typedef ITERATOR iterator;
		typedef MATRIX parent_t;

	private:
		friend MATRIX;

		MATRIX& parent;
		row_t const& row;

	public:
		M const row_i;

	private:
		row_proxy_base_t(MATRIX& _parent, row_t const& _row, M const _row_i)
			: parent(_parent)
			, row(_row)
			, row_i(_row_i)
		{
			assert(!row.is_invalid());
		}

	public:
		T const& operator[](N const j) const
		{
			assert(j < parent.n);
			auto const& it(std::lower_bound(begin(), end(), std::make_pair(j, invalid_value), [](std::pair<N, T> const& x, std::pair<N, T> const& y) {
				return x.first < y.first;
			}));

			if(it == end() || j < it->first)
				throw std::out_of_range("Element is not present");

			return it->second;
		}

		iterator begin() const
		{
			return parent.buf.data() + row.start;
		}

		iterator end() const
		{
			return begin() + row.length;
		}

		size_t size() const
		{
			return parent.n;
		}

		size_t nonempty_size() const
		{
			return row.length;
		}
	};

	typedef row_proxy_base_t<compact_sparse_matrix_t<M, N, T>, std::pair<N, T>*> row_proxy_t;
	typedef row_proxy_base_t<compact_sparse_matrix_t<M, N, T> const, std::pair<N, T> const*> const_row_proxy_t;

public:
	compact_sparse_matrix_t(compact_sparse_matrix_t&&) = default;
	compact_sparse_matrix_t(compact_sparse_matrix_t&) = delete;

	template<typename MATRIX>
	compact_sparse_matrix_t(MATRIX const& mat)
		: m(mat.size_m())
		, n(mat.size_n())
		, buf()
		, rows()
	{
		rows.reserve(mat.size_m());

		size_t nonempty_elements = 0;
		size_t i = 0;
		mat.citerate([&](typename MATRIX::const_row_proxy_t const& xs) {
			for(; i < xs.row_i.unseal(); ++i)
				rows.emplace_back();

			size_t size = xs.nonempty_size();
			rows.emplace_back(nonempty_elements, size);
			nonempty_elements += size;

			++i;
		});

		for(; i < m; ++i)
			rows.emplace_back();

		assert(rows.size() == m);

		buf.reserve(nonempty_elements);
		mat.citerate([&](typename MATRIX::const_row_proxy_t const& xs) {
			buf.insert(buf.end(), xs.begin(), xs.end());
		});
	}

	row_proxy_t operator[](M i)
	{
		assert(i < m);

		if(rows[i].is_invalid())
			throw std::out_of_range("Row is not present");

		return row_proxy_t(*this, rows[i], i);
	}

	const_row_proxy_t const operator[](M i) const
	{
		assert(i < m);

		if(rows[i].is_invalid())
			throw std::out_of_range("Row is not present");

		return const_row_proxy_t(*this, rows[i], i);
	}

	size_t size_m() const
	{
		return m;
	}

	size_t size_n() const
	{
		return n;
	}

	template<typename F>
	void iterate(F const& f)
	{
		for(size_t i = 0; i < m; ++i)
		{
			M im(i);
			if(!rows[i].is_invalid())
				f(row_proxy_t(*this, rows[i], M(i)));
		}
	}

	template<typename F>
	void citerate(F const& f) const
	{
		for(size_t i = 0; i < m; ++i)
		{
			M im(i);
			if(!rows[im].is_invalid())
				f(const_row_proxy_t(*this, rows[im], im));
		}
	}

	template<typename F>
	void iterate(F const& f) const
	{
		citerate(f);
	}
};

}


//==============================================================================
//         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/round.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/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/maxflint.hpp>

NT2_TEST_CASE_TPL ( round_cplx,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::round;
  using nt2::tag::round_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;
  typedef typename nt2::dry<T>             dT;
  typedef native<dT,ext_t>                vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Inf<vcT>()), nt2::Inf<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Minf<vcT>()), nt2::Minf<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Nan<vcT>()), nt2::Nan<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Inf<vdT>()), nt2::Inf<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Minf<vdT>()), nt2::Minf<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Nan<vdT>()), nt2::Nan<vdT>(),0);
#endif
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(-1.1, -1.6))), nt2::splat<vcT>(cT(-1, -2)),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(1.1, 1.6))),  nt2::splat<vcT>(cT(1, 2)),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Mone<vcT>()), nt2::Mone<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::One<vcT>()), nt2::One<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Zero<vcT>()), nt2::Zero<vcT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vdT>(dT(-1.1))), nt2::Mone<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vdT>(dT(1.1))), nt2::One<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Mone<vdT>()), nt2::Mone<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::One<vdT>()), nt2::One<vdT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::round(nt2::Zero<vdT>()), nt2::Zero<vdT>(),0);
}

NT2_TEST_CASE_TPL ( round_real2,  BOOST_SIMD_SIMD_REAL_TYPES)
{

  using nt2::round;
  using nt2::tag::round_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;
  typedef nt2::dry<T>                      dT;
  typedef native<dT,ext_t>                vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round(nt2::Inf<vcT>(), 2), nt2::Inf<vcT>());
  NT2_TEST_EQUAL(round(nt2::Minf<vcT>(), 2), nt2::Minf<vcT>());
  NT2_TEST_EQUAL(round(nt2::Nan<vcT>(), 2), nt2::Nan<vcT>());
#endif
  NT2_TEST_EQUAL(round(nt2::Mhalf<vcT>(), 2), nt2::Mhalf<vcT>());
  NT2_TEST_EQUAL(round(nt2::Mone<vcT>(), 2), nt2::Mone<vcT>());
  NT2_TEST_EQUAL(round(nt2::One<vcT>(), 2), nt2::One<vcT>());
  NT2_TEST_EQUAL(round(nt2::Zero<vcT>(), 2), nt2::Zero<vcT>());
  NT2_TEST_EQUAL(round(nt2::Maxflint<vcT>()-nt2::Half<vcT>(), 2),nt2::Maxflint<vcT>());
  NT2_TEST_EQUAL(round(nt2::Maxflint<vcT>(), 2),nt2::Maxflint<vcT>());
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(1.44, 1.44)), 1), nt2::splat<vcT>(cT(1.4,1.4 )));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(1.45, 1.45)), 1), nt2::splat<vcT>(cT(1.5,1.5 )));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(1.46, 1.46)), 1), nt2::splat<vcT>(cT(1.5,1.5 )));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(2.45, 2.45)), 1), nt2::splat<vcT>(cT(2.5,2.5 )));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(-1.44,-1.44)), 1), nt2::splat<vcT>(cT(-1.4,-1.4)));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(-1.45,-1.45)), 1), nt2::splat<vcT>(cT(-1.5,-1.5)));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(-1.46,-1.46)), 1), nt2::splat<vcT>(cT(-1.5,-1.5)));
  NT2_TEST_EQUAL(round(nt2::splat<vcT>(cT(-2.45,-2.45)), 1), nt2::splat<vcT>(cT(-2.5,-2.5)));
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(145,145)), -2), nt2::splat<vcT>(cT(100,100)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(150,150)), -2), nt2::splat<vcT>(cT(200,200)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(156,156)), -2), nt2::splat<vcT>(cT(200,200)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(250,250)), -2), nt2::splat<vcT>(cT(300,300)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(-145,-145)), -2), nt2::splat<vcT>(cT(-100,-100)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(-155,-155)), -2), nt2::splat<vcT>(cT(-200,-200)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(-156,-156)), -2), nt2::splat<vcT>(cT(-200,-200)), 0.5);
  NT2_TEST_ULP_EQUAL(round(nt2::splat<vcT>(cT(-255,-255)), -2), nt2::splat<vcT>(cT(-300,-300)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vdT>(dT(-1.11)), 1),  nt2::splat<vdT>(dT(-1.1)),0);
} // end of test for floating_

NT2_TEST_CASE_TPL ( round_real2b,  BOOST_SIMD_SIMD_REAL_TYPES)
{

  using nt2::round;
  using nt2::tag::round_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;
  typedef native<T ,ext_t>                vT;
  typedef typename nt2::meta::as_integer<vT>::type viT;
  typedef nt2::dry<T>                      dT;
  typedef native<dT ,ext_t>               vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(nt2::round(nt2::Inf<vcT>(), nt2::splat<viT>(2)), nt2::Inf<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Minf<vcT>(), nt2::splat<viT>(2)), nt2::Minf<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Nan<vcT>(), nt2::splat<viT>(2)), nt2::Nan<vcT>());
#endif
  NT2_TEST_EQUAL(nt2::round(nt2::Mhalf<vcT>(), nt2::splat<viT>(2)), nt2::Mhalf<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Mone<vcT>(), nt2::splat<viT>(2)), nt2::Mone<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::One<vcT>(), nt2::splat<viT>(2)), nt2::One<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Zero<vcT>(), nt2::splat<viT>(2)), nt2::Zero<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Maxflint<vcT>()-nt2::Half<vcT>(), nt2::splat<viT>(2)),nt2::Maxflint<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::Maxflint<vcT>(), nt2::splat<viT>(2)),nt2::Maxflint<vcT>());
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(1.44, 1.44)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(1.4,1.4 )));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(1.45, 1.45)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(1.5,1.5 )));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(1.46, 1.46)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(1.5,1.5 )));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(2.45, 2.45)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(2.5,2.5 )));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(-1.44,-1.44)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(-1.4,-1.4)));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(-1.45,-1.45)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(-1.5,-1.5)));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(-1.46,-1.46)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(-1.5,-1.5)));
  NT2_TEST_EQUAL(nt2::round(nt2::splat<vcT>(cT(-2.45,-2.45)), nt2::splat<viT>(1)), nt2::splat<vcT>(cT(-2.5,-2.5)));
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(145,145)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(100,100)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(150,150)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(200,200)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(156,156)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(200,200)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(250,250)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(300,300)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(-145,-145)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(-100,-100)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(-155,-155)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(-200,-200)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(-156,-156)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(-200,-200)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vcT>(cT(-255,-255)), nt2::splat<viT>(-2)), nt2::splat<vcT>(cT(-300,-300)), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::round(nt2::splat<vdT>(dT(-1.11)), nt2::splat<viT>(1)),  nt2::splat<vdT>(dT(-1.1)),0);
} // end of test for floating_


//
// Copyright 2005-2007 Adobe Systems Incorporated
// Copyright 2018-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil/channel.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/typedefs.hpp>

#include <boost/core/lightweight_test.hpp>

#include <cmath>
#include <cstdint>
#include <limits>

namespace gil = boost::gil;

// FIXME: Remove when https://github.com/boostorg/core/issues/38 happens
#define BOOST_GIL_TEST_IS_CLOSE(a, b, epsilon) BOOST_TEST_LT(std::fabs((a) - (b)), (epsilon))

struct int_minus_value  { static std::int8_t apply() { return -64; } };
struct int_plus_value   { static std::int8_t apply() { return  64; } };
using fixture = gil::scoped_channel_value
    <
        std::uint8_t, int_minus_value, int_plus_value
    >;

void test_scoped_channel_value_default_constructor()
{
    fixture f;
    std::uint8_t v = f;
    BOOST_TEST_EQ(v, std::uint8_t{0});
}

void test_scoped_channel_value_user_defined_constructors()
{
    fixture f{1};
    std::uint8_t v = f;
    BOOST_TEST_EQ(v, std::uint8_t{1});
}

void test_scoped_channel_value_copy_constructors()
{
    fixture f1{128};
    fixture f2{f1};

    BOOST_TEST_EQ(std::uint8_t{f1}, std::uint8_t{128});
    BOOST_TEST_EQ(std::uint8_t{f1}, std::uint8_t{f2});
}

void test_scoped_channel_value_assignment()
{
    fixture f;
    f = 64;
    std::uint8_t v = f;
    BOOST_TEST_EQ(v, std::uint8_t{64});
}

void test_scoped_channel_value_float32_t()
{
    auto const epsilon = std::numeric_limits<float>::epsilon();
    // min
    BOOST_GIL_TEST_IS_CLOSE(gil::float_point_zero<float>::apply(), 0.0, epsilon);
    BOOST_TEST_EQ(gil::channel_traits<gil::float32_t>::min_value(), 0.0);
    // max
    BOOST_GIL_TEST_IS_CLOSE(gil::float_point_one<float>::apply(), 1.0, epsilon);
    BOOST_TEST_EQ(gil::channel_traits<gil::float32_t>::max_value(), 1.0);
}

void test_scoped_channel_value_float64_t()
{
    auto const epsilon = std::numeric_limits<double>::epsilon();
    // min
    BOOST_GIL_TEST_IS_CLOSE(gil::float_point_zero<double>::apply(), 0.0, epsilon);
    BOOST_GIL_TEST_IS_CLOSE(gil::channel_traits<gil::float64_t>::min_value(), 0.0, epsilon);
    // max
    BOOST_GIL_TEST_IS_CLOSE(gil::float_point_one<double>::apply(), 1.0, epsilon);
    BOOST_GIL_TEST_IS_CLOSE(gil::channel_traits<gil::float64_t>::max_value(), 1.0, epsilon);
}

void test_scoped_channel_value_halfs()
{
    // Create a double channel with range [-0.5 .. 0.5]
    struct minus_half { static double apply() { return -0.5; } };
    struct plus_half { static double apply() { return 0.5; } };
    using halfs = gil::scoped_channel_value<double, minus_half, plus_half>;

    auto const epsilon = std::numeric_limits<double>::epsilon();
    BOOST_GIL_TEST_IS_CLOSE(gil::channel_traits<halfs>::min_value(), minus_half::apply(), epsilon);
    BOOST_GIL_TEST_IS_CLOSE(gil::channel_traits<halfs>::max_value(), plus_half::apply(), epsilon);
    // scoped channel maximum should map to the maximum
    BOOST_GIL_TEST_IS_CLOSE(gil::channel_convert<std::uint16_t>(
        gil::channel_traits<halfs>::max_value()), 65535, epsilon);
}

int main()
{
    test_scoped_channel_value_default_constructor();
    test_scoped_channel_value_user_defined_constructors();
    test_scoped_channel_value_copy_constructors();
    test_scoped_channel_value_assignment();
    test_scoped_channel_value_float32_t();
    test_scoped_channel_value_float64_t();
    test_scoped_channel_value_halfs();

    return ::boost::report_errors();
}


//==============================================================================
//         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_CONSTANT_CONSTANTS_MINEXPONENT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MINEXPONENT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/meta/int_c.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Minexponent generic tag

     Represents the Minexponent constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct Minexponent : ext::pure_constant_<Minexponent>
    {
      typedef double default_type;
      typedef ext::pure_constant_<Minexponent> parent;

      /// INTERNAL ONLY
      template<class Target, class Dummy=void>
      struct  apply : meta::int_c<typename Target::type,0> {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_Minexponent( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Minexponent::apply<boost::dispatch::meta::single_<T>,Dummy>
      : meta::int_c<boost::simd::int32_t,-126> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Minexponent::apply<boost::dispatch::meta::double_<T>,Dummy>
        : meta::int_c<boost::simd::int64_t,-1022> {};
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Minexponent, Site> dispatching_Minexponent(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Minexponent, Site>();
   }
   template<class... Args>
   struct impl_Minexponent;
  }
  /*!
    Generates the smallest floating point exponent.

    @par Semantic:

    @code
    T r = Minexponent<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r =  -1022;
    else if T is float
      r = -126;
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Minexponent, Minexponent)
} }

#include <boost/simd/constant/common.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/algorithm/frames.hpp>

#include "crocoddyl/multibody/contacts/contact-2d.hpp"
#include "crocoddyl/multibody/contacts/contact-3d.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"

#include "factory/contact.hpp"
#include "unittest_common.hpp"

using namespace crocoddyl::unittest;
using namespace boost::unit_test;

//----------------------------------------------------------------------------//

void test_construct_data(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);
}

void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x);

  // Getting the jacobian from the model
  model->calc(data, x);

  // Check that only the Jacobian has been filled
  BOOST_CHECK(!data->Jc.isZero());
  BOOST_CHECK(!data->a0.isZero());
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x);

  // Getting the jacobian from the model
  model->calc(data, x);
  model->calcDiff(data, x);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(!data->Jc.isZero());
  BOOST_CHECK(!data->a0.isZero());
  BOOST_CHECK(!data->da0_dx.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_update_force(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
  model->updateForce(data, f);
  boost::shared_ptr<crocoddyl::ContactModel3D> m = boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->a0.isZero());
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(!data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_update_force_diff(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::MatrixXd df_dx = Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
  Eigen::MatrixXd df_du = Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
  model->updateForceDiff(data, df_dx, df_du);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->a0.isZero());
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(!data->df_dx.isZero());
  BOOST_CHECK(!data->df_du.isZero());
}

void test_partial_derivatives_against_numdiff(ContactModelTypes::Type contact_type,
                                              PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(contact_type, model_type);

  // create the corresponding data object
  pinocchio::Model& pinocchio_model = *model->get_state()->get_pinocchio().get();
  pinocchio::Data pinocchio_data(pinocchio_model);
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);

  // Create the equivalent num diff model and data.
  crocoddyl::ContactModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff = model_num_diff.createData(&pinocchio_data);

  // Generating random values for the state
  const Eigen::VectorXd& x = model->get_state()->rand();

  // Compute all the pinocchio function needed for the models.
  crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);

  // set the function that needs to be called at every step of the numdiff
  std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
  reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio, &pinocchio_model, &pinocchio_data, _1));
  model_num_diff.set_reevals(reevals);

  // Computing the contact derivatives
  model->calc(data, x);
  model->calcDiff(data, x);

  // Computing the contact derivatives via numerical differentiation
  model_num_diff.calc(data_num_diff, x);
  model_num_diff.calcDiff(data_num_diff, x);

  // Checking the partial derivatives against NumDiff
  double tol = NUMDIFF_MODIFIER * model_num_diff.get_disturbance();
  BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isMuchSmallerThan(1.0, tol));
}

//----------------------------------------------------------------------------//

void register_contact_model_unit_tests(ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << contact_type << "_" << model_type;
  std::cout << "Running " << test_name.str() << std::endl;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
  ts->add(BOOST_TEST_CASE(boost::bind(&test_construct_data, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force_diff, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, contact_type, model_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size(); ++contact_type) {
    for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size(); ++model_type) {
      register_contact_model_unit_tests(ContactModelTypes::all[contact_type], PinocchioModelTypes::all[model_type]);
    }
  }
  return true;
}

int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }


/*
*   nanoflann wrapper
*   by R. Falque
*   21/11/2018
*/

#ifndef NANOFLANN_WRAPPER
#define NANOFLANN_WRAPPER

#include <iostream>
#include <memory>
#include <Eigen/Core>

#include "nanoflann.hpp"

class nanoflann_wrapper
{
public:
	nanoflann_wrapper(Eigen::MatrixXd& target)
	{
		if (target.cols() != 3)
		{
			std::cout << "Error: wrong input size\n";
			exit(0);
		}

		// set up kdtree
		int leaf_size=10;
		int dimensionality=3;

		this->kd_tree_index = std::make_shared< nanoflann::KDTreeEigenMatrixAdaptor< Eigen::MatrixXd> >(dimensionality, target, leaf_size);
		this->kd_tree_index->index->buildIndex();
	}

	~nanoflann_wrapper(){
	}

	std::vector< int > return_k_closest_points(Eigen::Vector3d query_point, int k)
	{
		// Query point:
		std::vector<double> query_pt;
		for (int d=0; d<3; d++)
			query_pt.push_back( query_point(d) );

		// set wtf vectors
		std::vector<size_t> ret_indexes(k);
		std::vector<double> out_dists_sqr(k);
		nanoflann::KNNResultSet<double> resultSet(k);
		resultSet.init( &ret_indexes.at(0), &out_dists_sqr.at(0) );

		// knn search
		this->kd_tree_index->index->findNeighbors(resultSet, &query_pt.at(0), nanoflann::SearchParams(k));

		// pack result into std::vector<int>
		std::vector< int > indexes;
		for (int i = 0; i < k; i++)
			indexes.push_back( ret_indexes.at(i) );

		return indexes;
	}

	bool return_k_closest_points(Eigen::Vector3d query_point, int k, std::vector<int> & indexes, std::vector<double> & distances)
	{
		// Query point:
		std::vector<double> query_pt;
		for (int d=0; d<3; d++)
			query_pt.push_back( query_point(d) );

		indexes.clear();
		distances.clear();

		// set wtf vectors
		std::vector<size_t> ret_indexes(k);
		std::vector<double> out_dists_sqr(k);
		nanoflann::KNNResultSet<double> resultSet(k);
		resultSet.init( &ret_indexes.at(0), &out_dists_sqr.at(0) );

		// knn search
		this->kd_tree_index->index->findNeighbors(resultSet, &query_pt.at(0), nanoflann::SearchParams(k));

		// pack results back into std::vector<int>
		for (int i = 0; i < ret_indexes.size(); i++)
		{
			indexes.push_back( ret_indexes.at(i) );
			distances.push_back( out_dists_sqr.at(i) );
		}

		return true;
	}

	std::vector< int > radius_search(Eigen::Vector3d query_point, double max_dist)
	{
		// Query point:
		std::vector<double> query_pt;
		for (int d=0; d<3; d++)
			query_pt.push_back( query_point(d) );

		//search
		nanoflann::SearchParams params;
		std::vector<std::pair<Eigen::Index, double> > matches;
		this->kd_tree_index->index->radiusSearch(&query_pt.at(0), max_dist, matches, params);

		// pack result into std::vector<int>
		std::vector< int > indexes;
		for (int i = 0; i < matches.size(); i++)
			indexes.push_back( matches.at(i).first );

		return indexes;
	}

	bool radius_search(Eigen::Vector3d query_point, double max_dist, std::vector<int> & indexes, std::vector<double> & distances)
	{
		// Query point:
		std::vector<double> query_pt;
		for (int d=0; d<3; d++)
			query_pt.push_back( query_point(d) );

		//search
		nanoflann::SearchParams params;
		std::vector<std::pair<Eigen::Index, double> > matches;
		this->kd_tree_index->index->radiusSearch(&query_pt.at(0), max_dist, matches, params);

		// pack result into std::vector<int>
		for (int i = 0; i < matches.size(); i++)
		{
			indexes.push_back( matches.at(i).first );
			distances.push_back( matches.at(i).second );
		}

		return true;
	}

private:
	std::shared_ptr < nanoflann::KDTreeEigenMatrixAdaptor< Eigen::MatrixXd > > kd_tree_index;

};

#endif


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [transform_reduce2]
#include <boost/simd/algorithm/transform_reduce.hpp>
#include <boost/simd/function/sqr.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f} ;
  std::vector<float> e{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f} ;

  auto s1 = bs::transform_reduce( d.data(), d.data()+10, e.data(), bs::multiplies, 0.0f, bs::plus);
  std::cout << "s1 = " << s1 << std::endl;
  auto s2 = bs::transform_reduce( d.data()+1, d.data()+10, e.data()+1, bs::multiplies, 1.0f, bs::plus);
  std::cout << "s2 = " << s2 << std::endl;
  auto s3 = bs::transform_reduce( d.data()+1, d.data()+10, e.data(), bs::multiplies, 0.0f, bs::plus);
  std::cout << "s3 = " << s3 << std::endl;
  auto s4 = bs::transform_reduce( d.data(), d.data()+10, e.data(), bs::multiplies, 1.0f, bs::multiplies);
  std::cout << "s4 = " << s4 << std::endl;
  return 0;
}
//! [transform_reduce2]


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_AUTO_SORT_HPP
#define BOOST_HANA_TEST_AUTO_SORT_HPP

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/permutations.hpp>
#include <boost/hana/sort.hpp>
#include <boost/hana/transform.hpp>

#include "test_case.hpp"
#include <laws/base.hpp>
#include <support/equivalence_class.hpp>

TestCase test_sort{[]{
    namespace hana = boost::hana;
    using hana::test::ct_eq;
    using hana::test::ct_ord;

    // Test without a custom predicate
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE()),
            MAKE_TUPLE()
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(ct_ord<0>{})),
            MAKE_TUPLE(ct_ord<0>{})
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(ct_ord<0>{}, ct_ord<1>{})),
            MAKE_TUPLE(ct_ord<0>{}, ct_ord<1>{})
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(ct_ord<1>{}, ct_ord<0>{})),
            MAKE_TUPLE(ct_ord<0>{}, ct_ord<1>{})
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(ct_ord<1>{}, ct_ord<0>{}, ct_ord<4>{}, ct_ord<2>{})),
            MAKE_TUPLE(ct_ord<0>{}, ct_ord<1>{}, ct_ord<2>{}, ct_ord<4>{})
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(ct_ord<1>{}, ct_ord<0>{}, ct_ord<-4>{}, ct_ord<2>{})),
            MAKE_TUPLE(ct_ord<-4>{}, ct_ord<0>{}, ct_ord<1>{}, ct_ord<2>{})
        ));
    }

    // Test with a custom predicate
    {
        auto pred = [](auto x, auto y) {
            return hana::less(x.unwrap, y.unwrap);
        };
        auto a = [](auto z) { return ::equivalence_class(ct_eq<999>{}, z); };
        auto b = [](auto z) { return ::equivalence_class(ct_eq<888>{}, z); };

        auto check = [=](auto ...sorted) {
            auto perms = hana::transform(
                hana::permutations(MAKE_TUPLE(a(sorted)...)),
                hana::sort.by(pred)
            );
            BOOST_HANA_CONSTANT_CHECK(hana::all_of(perms, [=](auto xs) {
                return hana::equal(xs, MAKE_TUPLE(a(sorted)...));
            }));
        };

        check();
        check(ct_ord<1>{});
        check(ct_ord<1>{}, ct_ord<2>{});
        check(ct_ord<1>{}, ct_ord<2>{}, ct_ord<3>{});

        // check stability
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{})), pred),
            MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{}))
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(b(ct_ord<1>{}), a(ct_ord<1>{})), pred),
            MAKE_TUPLE(b(ct_ord<1>{}), a(ct_ord<1>{}))
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{})), pred),
            MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{}))
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(a(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<1>{}), b(ct_ord<2>{})), pred),
            MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{}))
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(b(ct_ord<1>{}), a(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{})), pred),
            MAKE_TUPLE(b(ct_ord<1>{}), a(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{}))
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(a(ct_ord<2>{}), b(ct_ord<1>{}), b(ct_ord<2>{}), a(ct_ord<1>{})), pred),
            MAKE_TUPLE(b(ct_ord<1>{}), a(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<2>{}))
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::sort(MAKE_TUPLE(a(ct_ord<1>{}), a(ct_ord<3>{}), b(ct_ord<1>{}), a(ct_ord<2>{}), b(ct_ord<3>{})), pred),
            MAKE_TUPLE(a(ct_ord<1>{}), b(ct_ord<1>{}), a(ct_ord<2>{}), a(ct_ord<3>{}), b(ct_ord<3>{}))
        ));
    }
}};

#endif // !BOOST_HANA_TEST_AUTO_SORT_HPP


#ifndef LIVENESS_ANALYSIS_HPP
#define LIVENESS_ANALYSIS_HPP

/* Liveness analysis */

#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/types.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>

#include <boost/noncopyable.hpp>

namespace crab {

  namespace analyzer {

   template<typename V>
   using liveness_domain = crab::domains::flat_killgen_domain<V>;

   template<class CFG>
   class liveness_ops:
      public crab::iterators::
      killgen_operations_api<CFG, liveness_domain<typename CFG::varname_t> > {

   public:

     typedef liveness_domain<typename CFG::varname_t> liveness_domain_t;
     typedef typename CFG::basic_block_label_t basic_block_label_t;

   private:

     typedef crab::iterators::killgen_operations_api<CFG, liveness_domain_t> this_type;
     typedef std::pair<liveness_domain_t, liveness_domain_t> binding_t;
     typedef boost::unordered_map<basic_block_label_t, binding_t> liveness_map_t;

     liveness_map_t _liveness_map;

    public:

     liveness_ops (CFG cfg): this_type (cfg) { }

     virtual bool is_forward () { return false; }

     virtual liveness_domain_t entry()
     { return liveness_domain_t::bottom();}

     virtual liveness_domain_t merge(liveness_domain_t d1,  liveness_domain_t d2)
     { return d1 | d2; }

     virtual void init_fixpoint() {
        for (auto &b: boost::make_iterator_range(this->_cfg.begin(),this->_cfg.end())) {
          liveness_domain_t kill, gen;
          for (auto &s: boost::make_iterator_range(b.rbegin(),b.rend())) {
             auto live = s.get_live();
             for (auto d: boost::make_iterator_range(live.defs_begin(),
                                                     live.defs_end())) {
               kill += d;
               gen -= d;
             }
             for (auto u: boost::make_iterator_range(live.uses_begin(),
                                                     live.uses_end())) {
               gen  += u;
             }
          }
          _liveness_map.insert(std::make_pair(b.label(), binding_t(kill, gen)));
        }
      }

      virtual liveness_domain_t analyze (basic_block_label_t bb_id, liveness_domain_t in) {
        auto it = _liveness_map.find (bb_id);
        assert(it != _liveness_map.end ());
        in -= it->second.first;
        in += it->second.second;
        return in;
      }

      virtual std::string name () { return "liveness";}
   };

   //! Live variable analysis
   template<typename CFG>
   class liveness:
      boost::noncopyable,
      public crab::iterators::killgen_fixpoint_iterator <CFG, liveness_ops<CFG> >{

    public:

     typedef typename CFG::basic_block_label_t basic_block_label_t;
     typedef typename CFG::statement_t statement_t;
     typedef typename CFG::varname_t varname_t;

    private:

     typedef liveness_ops<CFG> liveness_ops_t;
     typedef typename liveness_ops_t::liveness_domain_t liveness_domain_t;
     typedef crab::iterators::killgen_fixpoint_iterator<CFG,liveness_ops_t>
             killgen_fixpoint_iterator_t;

    public:

     //for backward compatibility
     //XXX: maybe unused already
     typedef liveness_domain_t set_t;

    private:

     // output of the analysis: map basic blocks to set of dead
     // variables at the end of the blocks
     boost::unordered_map<basic_block_label_t, liveness_domain_t> _dead_map;

     // statistics
     unsigned _max_live;
     unsigned _total_live;
     unsigned _total_blks;

     void process_post (basic_block_label_t bb,
                        liveness_domain_t live_out) {
       // --- Collect dead variables at the exit of bb
       if (!live_out.is_bottom ()) {
         liveness_domain_t dead_set = this->_cfg.get_node(bb).live();
         dead_set -= live_out;
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << dead_set <<"\n";);
         _dead_map.insert (std::make_pair(bb, dead_set));
         // update statistics
         _total_live += live_out.size ();
         _max_live = std::max (_max_live, live_out.size ());
         _total_blks ++;
       } else {
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << live_out <<"\n";);
       }
     }

    public:

     liveness (CFG cfg)
         : killgen_fixpoint_iterator_t(cfg),
           _max_live (0), _total_live (0), _total_blks (0) { }

     void exec() {
       this->run();
       for (auto p : boost::make_iterator_range(this->out_begin(), this->out_end()))
       { process_post (p.first, p.second); }
       this->release_memory();
     }

     //! return the set of dead variables at the exit of block bb
     liveness_domain_t dead_exit (basic_block_label_t bb) const {
       auto it = _dead_map.find(bb);
       if (it == _dead_map.end())
         return liveness_domain_t();
       else
         return it->second;
     }

     void get_stats (unsigned& total_live,
                     unsigned& max_live_per_blk,
                     unsigned& avg_live_per_blk)  const {
       total_live = _total_live;
       max_live_per_blk = _max_live;
       avg_live_per_blk = (_total_blks == 0 ? 0 : (int) _total_live / _total_blks);
     }

     // TODO
     void write (crab_os &o) const { }
   };

   template <typename CFG>
   crab_os& operator << (crab_os& o, const liveness<CFG> &l) {
     l.write (o);
     return o;
   }

  } // end namespace analyzer
} // end namespace crab
#endif


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <boost/algorithm/string.hpp>

struct advent_2017_12 : problem
{
	advent_2017_12() noexcept : problem(2017, 12) {
	}

protected:
	utility::disjoint_set<std::size_t> ds;

	void prepare_input(std::ifstream& fin) override {
		for(std::string line; std::getline(fin, line); ) {
			std::vector<std::string> fragments;
			boost::split(fragments, line, [](char c) { return c == ' '; });
			for(std::size_t i = 2; i != fragments.size(); ++i) {
				ds.merge_sets(std::stoull(fragments[0]), std::stoull(fragments[i].substr(0, fragments[i].find(','))));
			}
		}
	}

	std::string part_1() override {
		return std::to_string(ds.set_size(0u));
	}

	std::string part_2() override {
		return std::to_string(ds.count_sets());
	}
};

REGISTER_SOLVER(2017, 12);


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral_constant.hpp>

#include <boost/hana/tuple.hpp>

#include <laws/constant.hpp>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Constant
    //////////////////////////////////////////////////////////////////////////
    {
        // value
        static_assert(value(integral_constant<int, 0>) == 0, "");
        static_assert(value(integral_constant<int, 1>) == 1, "");

        // laws
        test::TestConstant<IntegralConstant<int>>{tuple_c<int, -10, -2, 0, 1, 3, 4>};
        test::TestConstant<IntegralConstant<bool>>{tuple_c<bool, true, false>};
    }
}


#include <stdexcept>

#include <boost/test/unit_test.hpp>

#include "base-types.hpp"
#include "circle.hpp"

namespace
{
  const double eps = 1e-10;
}

BOOST_AUTO_TEST_SUITE(CircleConstructor)

BOOST_AUTO_TEST_CASE(ValidArguments_ValidInitialization)
{
  double radius = 4.3;
  yakovlev::point_t pos = { 2.3, -93.2 };
  yakovlev::Circle circle = { radius, pos };
  BOOST_CHECK_CLOSE(circle.getRadius(), radius, eps);
  BOOST_CHECK_CLOSE(circle.getCenter().x, pos.x, eps);
  BOOST_CHECK_CLOSE(circle.getCenter().y, pos.y, eps);
}

BOOST_AUTO_TEST_CASE(InvalidRadius_ThrowsInvalidArgument)
{
  BOOST_CHECK_THROW(yakovlev::Circle(-3.2, { 2.3, -93.2 }), std::invalid_argument);
  BOOST_CHECK_THROW(yakovlev::Circle(0.0, { 2.3, -93.2 }), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END()

//=====================================================================================

BOOST_AUTO_TEST_SUITE(CircleFrame)

BOOST_AUTO_TEST_CASE(ValidCircle_ValidFraming)
{
  double radius = 43.2;
  yakovlev::point_t pos = { 0.23, 213.12 };
  yakovlev::Circle circle = { radius, pos };
  yakovlev::rectangle_t frame = circle.getFrameRect();
  BOOST_CHECK_CLOSE(frame.width, radius * 2.0, eps);
  BOOST_CHECK_CLOSE(frame.height, radius * 2.0, eps);
  BOOST_CHECK_CLOSE(frame.pos.x, pos.x, eps);
  BOOST_CHECK_CLOSE(frame.pos.y, pos.y, eps);
}

BOOST_AUTO_TEST_SUITE_END()

//=====================================================================================

BOOST_AUTO_TEST_SUITE(CircleMove)

BOOST_AUTO_TEST_CASE(By_ValidArguments_CenterMoved)
{
  yakovlev::point_t center = { 543.3, 693.2 };
  yakovlev::Circle circle = { 543.3, center };
  double moveX = -85.23, moveY = -74.27;
  circle.move(moveX, moveY);
  yakovlev::point_t movedCenter = circle.getCenter();
  BOOST_CHECK_CLOSE(movedCenter.x, center.x + moveX, eps);
  BOOST_CHECK_CLOSE(movedCenter.y, center.y + moveY, eps);
}

BOOST_AUTO_TEST_CASE(To_ValidArguments_CenterMoved)
{
  yakovlev::Circle circle = { 12.43, { 2.3, -93.2 } };
  yakovlev::point_t newCenter = { 53.2, 0.04327 };
  circle.move(newCenter);
  BOOST_CHECK_CLOSE(circle.getCenter().x, newCenter.x, eps);
  BOOST_CHECK_CLOSE(circle.getCenter().y, newCenter.y, eps);
}

BOOST_AUTO_TEST_SUITE_END()

//=====================================================================================

BOOST_AUTO_TEST_SUITE(CircleMovePersistence)

BOOST_AUTO_TEST_CASE(By_ValidArguments_RadiusPersists)
{
  double radius = 4.3;
  yakovlev::Circle circle = { radius, { 2.3, -93.2 } };
  circle.move(23.4, -123.5432);
  BOOST_CHECK_CLOSE(circle.getRadius(), radius, eps);
}

BOOST_AUTO_TEST_CASE(To_ValidArguments_RadiusPersists)
{
  double radius = 5.4;
  yakovlev::Circle circle = { radius, { 2.3, -93.2 } };
  circle.move({ 234.21, 0.0 });
  BOOST_CHECK_CLOSE(circle.getRadius(), radius, eps);
}

BOOST_AUTO_TEST_CASE(By_ValidArguments_AreaPersists)
{
  yakovlev::Circle circle = { 543.2, { 2.3, -93.2 } };
  double area = circle.getArea();
  circle.move(23.4, -123.5432);
  BOOST_CHECK_CLOSE(circle.getArea(), area, eps);
}

BOOST_AUTO_TEST_CASE(To_ValidArguments_AreaPersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  double area = circle.getArea();
  circle.move({ 234.21, 0.0 });
  BOOST_CHECK_CLOSE(circle.getArea(), area, eps);
}

BOOST_AUTO_TEST_CASE(By_ValidArguments_FramePersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  yakovlev::rectangle_t frame = circle.getFrameRect();
  circle.move(43.4, 23.5432);
  yakovlev::rectangle_t movedFrame = circle.getFrameRect();
  BOOST_CHECK_CLOSE(movedFrame.width, frame.width, eps);
  BOOST_CHECK_CLOSE(movedFrame.height, frame.height, eps);
  BOOST_CHECK_CLOSE(movedFrame.pos.x, circle.getCenter().x, eps);
  BOOST_CHECK_CLOSE(movedFrame.pos.y, circle.getCenter().y, eps);
}

BOOST_AUTO_TEST_CASE(To_ValidArguments_FramePersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  yakovlev::rectangle_t frame = circle.getFrameRect();
  circle.move({ 23.2, 64.2 });
  yakovlev::rectangle_t movedFrame = circle.getFrameRect();
  BOOST_CHECK_CLOSE(movedFrame.width, frame.width, eps);
  BOOST_CHECK_CLOSE(movedFrame.height, frame.height, eps);
  BOOST_CHECK_CLOSE(movedFrame.pos.x, circle.getCenter().x, eps);
  BOOST_CHECK_CLOSE(movedFrame.pos.y, circle.getCenter().y, eps);
}

BOOST_AUTO_TEST_SUITE_END()

//=====================================================================================

BOOST_AUTO_TEST_SUITE(CircleRotate)

BOOST_AUTO_TEST_CASE(AnyAngle_AreaPersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  double area = circle.getArea();
  circle.rotate(12);
  BOOST_CHECK_CLOSE(circle.getArea(), area, eps);
}

BOOST_AUTO_TEST_CASE(AnyAngle_CenterPersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  yakovlev::point_t center = circle.getCenter();
  circle.rotate(-12);
  BOOST_CHECK_CLOSE(circle.getCenter().x, center.x, eps);
  BOOST_CHECK_CLOSE(circle.getCenter().y, center.y, eps);
}

BOOST_AUTO_TEST_CASE(AnyAngle_FrameIsStatic)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  yakovlev::rectangle_t frame = circle.getFrameRect();
  circle.rotate(23);
  yakovlev::rectangle_t updatedFrame = circle.getFrameRect();
  BOOST_CHECK_CLOSE(updatedFrame.width, frame.width, eps);
  BOOST_CHECK_CLOSE(updatedFrame.height, frame.height, eps);
  BOOST_CHECK_CLOSE(updatedFrame.pos.x, frame.pos.x, eps);
  BOOST_CHECK_CLOSE(updatedFrame.pos.y, frame.pos.y, eps);
}

BOOST_AUTO_TEST_SUITE_END()

//=====================================================================================

BOOST_AUTO_TEST_SUITE(CircleScale)

BOOST_AUTO_TEST_CASE(ValidCoefficient_AreaScales)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  double area = circle.getArea();
  double coef = 2.34;
  circle.scale(coef);
  BOOST_CHECK_CLOSE(circle.getArea(), area * coef * coef, eps);
}

BOOST_AUTO_TEST_CASE(InvalidCoefficient_ThrowsInvalidArgument)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  BOOST_CHECK_THROW(circle.scale(-3.4), std::invalid_argument);
  BOOST_CHECK_THROW(circle.scale(0.0), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(ValidCoefficient_FramePersists)
{
  yakovlev::Circle circle = { 23.634, { 2.3, -93.2 } };
  yakovlev::rectangle_t frame = circle.getFrameRect();
  double coef = 23.3;
  circle.scale(coef);
  yakovlev::rectangle_t scaledFrame{ circle.getFrameRect() };
  BOOST_CHECK_CLOSE(scaledFrame.width, frame.width * coef, eps);
  BOOST_CHECK_CLOSE(scaledFrame.height, frame.height * coef, eps);
  BOOST_CHECK_CLOSE(scaledFrame.pos.x, frame.pos.x, eps);
  BOOST_CHECK_CLOSE(scaledFrame.pos.y, frame.pos.y, eps);
}

BOOST_AUTO_TEST_SUITE_END()


#include "bal_problem_stereo.h"

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <ceres/rotation.h>
//#include <glog/logging.h>

namespace optim {

namespace {
typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;

inline double RandDouble() {
  double r = static_cast<double>(rand());
  return r / RAND_MAX;
}

// Box-Muller algorithm for normal random number generation.
// http://en.wikipedia.org/wiki/Box-Muller_transform
inline double RandNormal() {
  double x1, x2, w;
  do {
    x1 = 2.0 * RandDouble() - 1.0;
    x2 = 2.0 * RandDouble() - 1.0;
    w = x1 * x1 + x2 * x2;
  } while ( w >= 1.0 || w == 0.0 );

  w = sqrt((-2.0 * log(w)) / w);
  return x1 * w;
}

template<typename T>
void FscanfOrDie(FILE* fptr, const char* format, T* value) {
  int num_scanned = fscanf(fptr, format, value);
  if (num_scanned != 1) {
    //LOG(FATAL) << "Invalid UW data file.";
    throw "Invalid UW data file.";
  }
}

void PerturbPoint3(const double sigma, double* point) {
  for (int i = 0; i < 3; ++i) {
    point[i] += RandNormal() * sigma;
  }
}

double Median(std::vector<double>* data) {
  int n = data->size();
  std::vector<double>::iterator mid_point = data->begin() + n / 2;
  std::nth_element(data->begin(), mid_point, data->end());
  return *mid_point;
}

}  // namespace

BALProblemStereo::BALProblemStereo(bool use_quaternions) : use_quaternions_(use_quaternions)
{}

BALProblemStereo::BALProblemStereo(const std::string& filename, bool use_quaternions)
                                  : use_quaternions_(use_quaternions)
{
  FILE* fptr = fopen(filename.c_str(), "r");

  if (fptr == NULL) {
    //LOG(FATAL) << "Error: unable to open file " << filename;
    std::cout << "Error: unable to open file " << filename;
    return;
  };

  num_intr_ = 5;
  cam_intrinsics_ = new double[num_intr_];
  for(int i = 0; i < num_intr_; i++) {
    FscanfOrDie(fptr, "%lf", &cam_intrinsics_[i]);
    std::cout << cam_intrinsics_[i] << " - cam_intrinsics_\n";
  }

  // This wil die horribly on invalid files. Them's the breaks.
  FscanfOrDie(fptr, "%d", &num_cameras_);
  FscanfOrDie(fptr, "%d", &num_points_);
  FscanfOrDie(fptr, "%d", &num_observations_);

  std::cout << "Header: " << num_cameras_
          << " " << num_points_
          << " " << num_observations_;

  point_index_ = new int[num_observations_];
  camera_index_ = new int[num_observations_];
  observations_ = new double[4 * num_observations_];

  num_parameters_ = camera_block_size() * num_cameras_ + point_block_size() * num_points_;
  parameters_ = new double[num_parameters_];

  for (int i = 0; i < num_observations_; ++i) {
    FscanfOrDie(fptr, "%d", camera_index_ + i);
    FscanfOrDie(fptr, "%d", point_index_ + i);
    for (int j = 0; j < 4; ++j) {
      FscanfOrDie(fptr, "%lf", observations_ + 4*i + j);
    }
  }

  for (int i = 0; i < num_parameters_; ++i) {
    FscanfOrDie(fptr, "%lf", parameters_ + i);
  }

  fclose(fptr);

  if (use_quaternions) {
    // Switch the angle-axis rotations to quaternions.
    num_parameters_ = camera_block_size() * num_cameras_ + point_block_size() * num_points_;
    double* quaternion_parameters = new double[num_parameters_];
    double* original_cursor = parameters_;
    double* quaternion_cursor = quaternion_parameters;
    for (int i = 0; i < num_cameras_; ++i) {
      ceres::AngleAxisToQuaternion(original_cursor, quaternion_cursor);
      quaternion_cursor += 4;
      original_cursor += 3;
      for (int j = 4; j < camera_block_size(); ++j) {
       *quaternion_cursor++ = *original_cursor++;
      }
    }
    // Copy the rest of the points.
    for (int i = 0; i < point_block_size() * num_points_; ++i) {
      *quaternion_cursor++ = *original_cursor++;
    }
    // Swap in the quaternion parameters.
    delete[] parameters_;
    parameters_ = quaternion_parameters;
  }
}

// This function writes the problem to a file in the same format that
// is read by the constructor.
void BALProblemStereo::WriteToFile(const std::string& filename) const {
  FILE* fptr = fopen(filename.c_str(), "w");

  if (fptr == NULL) {
    std::cout << "Error: unable to open file " << filename;
    return;
  };

  for(int i = 0; i < num_intr_; i++)
    fprintf(fptr, "%g ", cam_intrinsics_[i]);
  fprintf(fptr, "\n");

  fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);

  for (int i = 0; i < num_observations_; ++i) {
    fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
    for (int j = 0; j < 4; ++j) {
      fprintf(fptr, " %g", observations_[4 * i + j]);
    }
    fprintf(fptr, "\n");
  }

  for (int i = 0; i < num_cameras(); ++i) {
    double angleaxis[6];
    if (use_quaternions_) {
      // Output in angle-axis format.
      ceres::QuaternionToAngleAxis(parameters_ + camera_block_size() * i, angleaxis);
      memcpy(angleaxis + 3, parameters_ + camera_block_size() * i + 4, (camera_block_size()-4) * sizeof(double));
    } else {
      memcpy(angleaxis, parameters_ + camera_block_size() * i, camera_block_size() * sizeof(double));
    }
    for (int j = 0; j < camera_block_size(); ++j) {
      fprintf(fptr, "%.16g\n", angleaxis[j]);
    }
  }

  const double* points = parameters_ + camera_block_size() * num_cameras_;
  for (int i = 0; i < num_points(); ++i) {
    const double* point = points + i * point_block_size();
    for (int j = 0; j < point_block_size(); ++j) {
      fprintf(fptr, "%.16g\n", point[j]);
    }
  }

  fclose(fptr);
}

void BALProblemStereo::CameraToAngleAxisAndCenter(const double* camera,
                                            double* angle_axis,
                                            double* center) {
  VectorRef angle_axis_ref(angle_axis, 3);
  if (use_quaternions_) {
    ceres::QuaternionToAngleAxis(camera, angle_axis);
  } else {
    angle_axis_ref = ConstVectorRef(camera, 3);
  }

  int point_pos = (use_quaternions_ ? 4 : 3);
  // c = -R't
  Eigen::VectorXd inverse_rotation = -angle_axis_ref;
  ceres::AngleAxisRotatePoint(inverse_rotation.data(), camera + point_pos, center);
  VectorRef(center, 3) *= -1.0;
}

void BALProblemStereo::AngleAxisAndCenterToCamera(const double* angle_axis,
                                            const double* center,
                                            double* camera) {
  ConstVectorRef angle_axis_ref(angle_axis, 3);
  if (use_quaternions_) {
    ceres::AngleAxisToQuaternion(angle_axis, camera);
  } else {
    VectorRef(camera, 3) = angle_axis_ref;
  }

  int point_pos = (use_quaternions_ ? 4 : 3);
  // t = -R * c
  ceres::AngleAxisRotatePoint(angle_axis,
                       center,
                       camera + point_pos);
  VectorRef(camera + point_pos, 3) *= -1.0;
}

void BALProblemStereo::Normalize() {
  // Compute the marginal median of the geometry.
  std::vector<double> tmp(num_points_);
  Eigen::Vector3d median;
  double* points = mutable_points();
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < num_points_; ++j) {
      tmp[j] = points[3 * j + i];
    }
    median(i) = Median(&tmp);
  }

  for (int i = 0; i < num_points_; ++i) {
    VectorRef point(points + 3 * i, 3);
    tmp[i] = (point - median).lpNorm<1>();
  }

  const double median_absolute_deviation = Median(&tmp);

  // Scale so that the median absolute deviation of the resulting
  // reconstruction is 100.
  const double scale = 100.0 / median_absolute_deviation;

  std::cout << "median: " << median.transpose();
  std::cout << "median absolute deviation: " << median_absolute_deviation;
  std::cout  << "scale: " << scale;

  // X = scale * (X - median)
  for (int i = 0; i < num_points_; ++i) {
    VectorRef point(points + 3 * i, 3);
    point = scale * (point - median);
  }

  double* cameras = mutable_cameras();
  double angle_axis[3];
  double center[3];
  for (int i = 0; i < num_cameras_; ++i) {
    double* camera = cameras + camera_block_size() * i;
    CameraToAngleAxisAndCenter(camera, angle_axis, center);
    // center = scale * (center - median)
    VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
    AngleAxisAndCenterToCamera(angle_axis, center, camera);
  }
}

void BALProblemStereo::Perturb(const double rotation_sigma,
                               const double translation_sigma,
                               const double point_sigma)
{
  CHECK_GE(point_sigma, 0.0);
  CHECK_GE(rotation_sigma, 0.0);
  CHECK_GE(translation_sigma, 0.0);

  double* points = mutable_points();
  if (point_sigma > 0) {
    for (int i = 0; i < num_points_; ++i) {
      PerturbPoint3(point_sigma, points + 3 * i);
    }
  }

  for (int i = 0; i < num_cameras_; ++i) {
    double* camera = mutable_cameras() + camera_block_size() * i;

    double angle_axis[3];
    double center[3];
    // Perturb in the rotation of the camera in the angle-axis
    // representation.
    CameraToAngleAxisAndCenter(camera, angle_axis, center);
    if (rotation_sigma > 0.0) {
      PerturbPoint3(rotation_sigma, angle_axis);
    }
    AngleAxisAndCenterToCamera(angle_axis, center, camera);

    int point_pos = (use_quaternions_ ? 4 : 3);
    if (translation_sigma > 0.0) {
      PerturbPoint3(translation_sigma, camera + point_pos);
    }
  }
}

BALProblemStereo::~BALProblemStereo() {
  delete[] point_index_;
  delete[] camera_index_;
  delete[] observations_;
  delete[] parameters_;
  delete[] cam_intrinsics_;
}

}  // namespace optim


// Copyright 2019 Hans Dembinski
//
// 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)

//[ guide_axis_basic_demo

#include <boost/histogram/axis.hpp>
#include <limits>

int main() {
  using namespace boost::histogram;

  // make a regular axis with 10 bins over interval from 1.5 to 2.5
  auto r = axis::regular<>{10, 1.5, 2.5};
  // `<>` is needed in C++14 because the axis is templated,
  // in C++17 you can do: auto r = axis::regular{10, 1.5, 2.5};
  assert(r.size() == 10);
  // alternatively, you can define the step size with the `step` marker
  auto r_step = axis::regular<>{axis::step(0.1), 1.5, 2.5};
  assert(r_step == r);

  // histogram uses the `index` method to convert values to indices
  // note: intervals of builtin axis types are always semi-open [a, b)
  assert(r.index(1.5) == 0);
  assert(r.index(1.6) == 1);
  assert(r.index(2.4) == 9);
  // index for a value below the start of the axis is always -1
  assert(r.index(1.0) == -1);
  assert(r.index(-std::numeric_limits<double>::infinity()) == -1);
  // index for a value below the above the end of the axis is always `size()`
  assert(r.index(3.0) == 10);
  assert(r.index(std::numeric_limits<double>::infinity()) == 10);
  // index for not-a-number is also `size()` by convention
  assert(r.index(std::numeric_limits<double>::quiet_NaN()) == 10);

  // make a variable axis with 3 bins [-1.5, 0.1), [0.1, 0.3), [0.3, 10)
  auto v = axis::variable<>{-1.5, 0.1, 0.3, 10.};
  assert(v.index(-2.0) == -1);
  assert(v.index(-1.5) == 0);
  assert(v.index(0.1) == 1);
  assert(v.index(0.3) == 2);
  assert(v.index(10) == 3);
  assert(v.index(20) == 3);

  // make an integer axis with 3 bins at -1, 0, 1
  auto i = axis::integer<>{-1, 2};
  assert(i.index(-2) == -1);
  assert(i.index(-1) == 0);
  assert(i.index(0) == 1);
  assert(i.index(1) == 2);
  assert(i.index(2) == 3);

  // make an integer axis called "foo"
  auto i_with_label = axis::integer<>{-1, 2, "foo"};
  // all builtin axis types allow you to pass some optional metadata as the last
  // argument in the constructor; a string by default, but can be any copyable type

  // two axis do not compare equal if they differ in their metadata
  assert(i != i_with_label);

  // integer axis also work well with unscoped enums
  enum { red, blue };
  auto i_for_enum = axis::integer<>{red, blue + 1};
  assert(i_for_enum.index(red) == 0);
  assert(i_for_enum.index(blue) == 1);

  // make a category axis from a scoped enum and/or if the identifiers are not consecutive
  enum class Bar { red = 12, blue = 6 };
  auto c = axis::category<Bar>{Bar::red, Bar::blue};
  assert(c.index(Bar::red) == 0);
  assert(c.index(Bar::blue) == 1);
  // c.index(12) is a compile-time error, since the argument must be of type `Bar`

  // category axis can be created for any copyable and equal-comparable type
  auto c_str = axis::category<std::string>{"red", "blue"};
  assert(c_str.index("red") == 0);
  assert(c_str.index("blue") == 1);
}

//]


#ifndef MULTIPRECISION_MPCONTEXT_HPP
#define MULTIPRECISION_MPCONTEXT_HPP
#include "fhe/FHEContext.h"
#include <memory>
#include <NTL/ZZ.h>
#include <iostream>
/// MultiPrecision FHEcontext
class MPContext {
public:
    typedef std::shared_ptr<FHEcontext> contextPtr;
    /// @param m, p, r is the same with FHEcontext
    /// @param parts is how many parts to use
    MPContext(long m, long p, long r, long parts);

    void buildModChain(long L);
    /// @return the specific FHEcontext
    contextPtr get(int index) const { return contexts[index]; }

    size_t partsNum() const { return contexts.size(); }

    double precision() const;

    ZZ plainSpace() const { return m_plainSpace; }

    std::vector<long> primes() const { return m_primes; }

    long getR() const { return m_r; }
private:
    ZZ m_plainSpace = NTL::to_ZZ(1);
    long m_r = 1;
    std::vector<long> m_primes;
    std::vector<contextPtr> contexts;
};
#endif // multiprecision/mpcontext.hpp


//////////////////////////////////////////////////////////////////////////////////
// distribution::common::concept::concept::unary::quantile.hpp                  //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  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)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_CONCEPT_UNARY_QUANTILE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_CONCEPT_UNARY_QUANTILE_HPP_ER_2009
#include <boost/concept_check.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace concept{

    template<typename B>
    class Quantile : public B{
        typedef typename B::distribution_type   d_;
        typedef typename B::variable_type       x_;
        typedef typename B::probability_type    p_;

        public:

        BOOST_CONCEPT_USAGE(Quantile){ x = quantile(d,p); }

        private:
        d_ d;
        x_ x;
        p_ p;

    };

}// concept
}// distribution
}// statistics
}// detail
}// boost

#endif


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/numeric/fixed_point.hpp>
#include <srook/utility/index_sequence.hpp>
#include <srook/limits/numeric_limits.hpp>

namespace detail {

template <
    std::size_t begin_integer_bits, std::size_t end_integer_bits,
    std::size_t begin_fractional_bits, std::size_t end_fractional_bits
>
struct all_pattern {
    static SROOK_CXX14_CONSTEXPR void do_pattern()
    {
        typedef srook::numeric::fixed_point<begin_integer_bits, begin_fractional_bits> fixed_point_type;
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp = srook::numeric_limits<fixed_point_type>::max();
        all_pattern<begin_integer_bits + 1, end_integer_bits, begin_fractional_bits - 1, end_fractional_bits>::do_pattern();
    }
};

template <std::size_t integer_bits, std::size_t fractional_bits>
struct all_pattern<integer_bits, integer_bits, fractional_bits, fractional_bits> {
    static SROOK_CXX14_CONSTEXPR void do_pattern()
    {
        typedef srook::numeric::fixed_point<integer_bits, fractional_bits> fixed_point_type;
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp = srook::numeric_limits<fixed_point_type>::max();
    }
};

} // namespace detail

BOOST_AUTO_TEST_SUITE(numeric_fixed_point_test)

BOOST_AUTO_TEST_CASE(fixed_point_all_pattern_test)
{
    detail::all_pattern<1, 8, 7, 0>::do_pattern(); // 8 bits pattern
    detail::all_pattern<1, 16, 15, 0>::do_pattern(); // 16 bits pattern
    detail::all_pattern<1, 32, 31, 0>::do_pattern(); // 32 bits pattern
    detail::all_pattern<1, 64, 63, 0>::do_pattern(); // 64 bits pattern
#if defined(SROOK_HAS_INT128) && !defined(__STRICT_ANSI__)
    detail::all_pattern<1, 128, 127, 0>::do_pattern(); // 128 bits pattern
#endif
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_gaussian_filter.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/filter/gaussian/gaussian_filter.hpp>
#include <fl/filter/gaussian/update_policy/multi_sensor_sigma_point_update_policy.hpp>
#include <fl/filter/gaussian/update_policy/multi_sensor_sigma_point_update_polizzle.hpp>

namespace fl
{
// Forward declarations
template <typename...>
class MultiSensorGaussianFilter;

/**
 * \internal
 * \ingroup nonlinear_gaussian_filter
 *
 * Traits of GaussianFilter for multi sensors based on quadrature with
 * customizable policies, i.e implementations of the time and measurement
 * updates.
 */
template <typename TransitionFunction,
          typename JointSensorFunction,
          typename... Policies>
struct Traits<MultiSensorGaussianFilter<TransitionFunction,
                                        JointSensorFunction,
                                        Policies...>>
{
    typedef typename TransitionFunction::State State;
    typedef typename TransitionFunction::Input Input;
    typedef typename JointSensorFunction::Obsrv Obsrv;
    typedef Gaussian<State> Belief;
};

/**
 * \ingroup nonlinear_gaussian_filter
 */
template <typename TransitionFunction,
          typename JointSensorFunction,
          typename Quadrature>
class MultiSensorGaussianFilter<TransitionFunction,
                                JointSensorFunction,
                                Quadrature> :
/* Implement the filter interface */
#ifndef GENERATING_DOCUMENTATION
    public GaussianFilter<
        typename RemoveAdditivityOf<TransitionFunction>::Type,
        typename RemoveAdditivityOf<JointSensorFunction>::Type,
        Quadrature,
        SigmaPointPredictPolicy<
            Quadrature,
            typename AdditivityOf<TransitionFunction>::Type>,
        MultiSensorSigmaPointUpdatePolizzle<
//        MultiSensorSigmaPointUpdatePolicy<
            Quadrature,
            typename AdditivityOf<JointSensorFunction>::Type>>
#else
    public GaussianFilter<TransitionFunction,
                          SensorFunction,
                          Quadrature,
                          PredictionPolicy,
                          MultiSensorUpdatePolicy>
#endif
{
public:
    typedef GaussianFilter<
        typename RemoveAdditivityOf<TransitionFunction>::Type,
        typename RemoveAdditivityOf<JointSensorFunction>::Type,
        Quadrature,
        SigmaPointPredictPolicy<
            Quadrature,
            typename AdditivityOf<TransitionFunction>::Type>,
        MultiSensorSigmaPointUpdatePolizzle<
//        MultiSensorSigmaPointUpdatePolicy<
            Quadrature,
            typename AdditivityOf<JointSensorFunction>::Type>> Base;

    MultiSensorGaussianFilter(const TransitionFunction& transition,
                              const JointSensorFunction& sensor,
                              const Quadrature& quadrature)
        : Base(transition, sensor, quadrature)
    {
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~MultiSensorGaussianFilter() noexcept {}
    virtual std::string name() const
    {
        return "MultiSensorGaussianFilter<" +
               this->list_arguments(Base::name()) + ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Gaussian Filter with" +
               this->list_descriptions(Base::description());
    }
};
}


#include <sequential-line-search/utils.h>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <Eigen/Core>

using Eigen::VectorXd;
using Eigen::MatrixXd;

namespace sequential_line_search
{
    namespace utils
    {
        Eigen::VectorXd generateRandomVector(unsigned n)
        {
            return 0.5 * (Eigen::VectorXd::Random(n) + Eigen::VectorXd::Ones(n));
        }

        void exportMatrixToCsv(const std::string& filePath, const Eigen::MatrixXd& X)
        {
            std::ofstream file(filePath);
            Eigen::IOFormat format(Eigen::StreamPrecision, Eigen::DontAlignCols, ",");
            file << X.format(format);
        }

        std::string getCurrentTimeInString()
        {
            const std::time_t t = std::time(nullptr);
            std::stringstream s; s << std::put_time(std::localtime(&t), "%Y%m%d%H%M%S");
            return s.str();
        }
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CHOP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CHOP_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing chop capabilities

  chop (x, ndigits, base)
  truncate elements of x to a length of ndigits such that the
  resulting numbers are exactly divisible by base.  if base is not
  specified it defaults to 10.
  chop(x,n) chops aways from 0 to n digits:

  **/
  const boost::dispatch::functor<tag::chop_> chop = {};
} }
#endif

#include <boost/simd/function/scalar/chop.hpp>
#include <boost/simd/function/simd/chop.hpp>

#endif


/*
    Copyright 2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#include <adobe/config.hpp>

#define BOOST_TEST_MAIN

#include <string>
#include <cstdlib>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <adobe/poly_copyable.hpp>
#include <adobe/poly_regular.hpp>

inline double size(int x) { return static_cast<double>(std::abs(x)); }

#include "poly_measurable.hpp"
#include "poly_measurable2d.hpp"

/*************************************************************************************************/

// size for some basic types

// not needed, default: inline double size(const std::string& x) { return x.size(); }

// Compile time polymorphism requiring MeasurableConcept

template <class M1, class M2> // M1 models Measurable, M2 models Measurable
double max_size(const M1& x, const M2& y) {
    boost::function_requires<MeasurableConcept<M1>>();
    boost::function_requires<MeasurableConcept<M2>>();

    return std::max(MeasurableConcept<M1>::size(x), MeasurableConcept<M2>::size(y));
}

// Compile time test

BOOST_AUTO_TEST_CASE(compile_time_poly) {
    // tests compile-time generic max_size function template
    BOOST_CHECK_EQUAL(max_size(4, 5), 5);
    BOOST_CHECK_EQUAL(max_size(4, std::string("hello")), 5);
}

/*************************************************************************************************/

// *runtime* generic function
// (not a template)

double max_size_poly(const poly_measurable& x, const poly_measurable& y) {
    return std::max(size(x), size(y));
}

BOOST_AUTO_TEST_CASE(run_time_poly) {
    // tests compile-time generic max_size function template
    poly_measurable four(4), five(5);
    poly_measurable y(std::string("hello"));

    BOOST_CHECK_EQUAL(max_size_poly(four, five), 5);
    BOOST_CHECK_EQUAL(max_size_poly(four, y), 5);
}

/*************************************************************************************************/

struct concrete_measurable2d {
    concrete_measurable2d(double size, double height) : size_m(size), height_m(height) {}
    concrete_measurable2d() : size_m(0.0), height_m(0.0) {}
    double size() const { return size_m; }
    double height() const { return height_m; }

    double size_m;
    double height_m;
};

bool operator==(const concrete_measurable2d& x, const concrete_measurable2d& y) {
    return x.size() == y.size() && x.height() == y.height();
}

bool operator!=(const concrete_measurable2d& x, const concrete_measurable2d& y) {
    return !(x == y);
}

inline double perimeter(poly_measurable2d x) { return 2 * (x.size() + x.height()); }

BOOST_AUTO_TEST_CASE(refinement) {
    concrete_measurable2d a(2, 3), b(1, 4);

    poly_measurable ma(a), mb(b);
    BOOST_CHECK_EQUAL(ma.size(), mb.size() + 1);
    BOOST_CHECK_THROW(adobe::poly_cast<poly_measurable2d&>(ma), std::bad_cast);
    BOOST_CHECK_EQUAL(adobe::poly_cast<poly_measurable2d*>(&ma) == NULL, true);
    poly_measurable2d m2a(a), m2b(b);
    BOOST_CHECK_EQUAL(ma.size(), m2a.size());
    BOOST_CHECK_EQUAL(perimeter(m2a), perimeter(m2b));
    BOOST_CHECK_EQUAL(max_size_poly(adobe::poly_cast<poly_measurable&>(m2a),
                                    adobe::poly_cast<poly_measurable&>(m2b)),
                      2);
    poly_measurable x(m2a);
    BOOST_CHECK_EQUAL(x.size(), m2a.size());
}

BOOST_AUTO_TEST_CASE(assign) {
    concrete_measurable2d a(2, 3), b(1, 4);
    poly_measurable ma(a), mb(b);
    poly_measurable2d m2a(a), m2b(b);

    m2b.assign(a);
    BOOST_CHECK_EQUAL(m2a.size(), m2b.size());
    BOOST_CHECK_EQUAL(m2a.height(), m2b.height());

    int i = 1;

    ma.assign(i);
}

BOOST_AUTO_TEST_CASE(move_test) {
    concrete_measurable2d a(2, 3), b(1, 4);
    poly_measurable ma(a), mb(b);
    poly_measurable2d m2a(a), m2b(b);

    poly_measurable mc(adobe::move(ma));
}

BOOST_AUTO_TEST_CASE(poly_copyable) {
    using namespace adobe;
    concrete_measurable2d a(2, 3), b(1, 4);
    {
        poly<copyable> ma(a), mb(b);
        poly<copyable> m2a(a), m2b(b);

        poly<copyable> mc(adobe::move(ma));
    }
    {
        poly<regular> ma(a), mb(b);
        poly<regular> m2a(a), m2b(b);

        // poly<regular> mc(ma, move_ctor());
        BOOST_CHECK(true);
        BOOST_CHECK(ma == m2a);
        BOOST_CHECK(ma != poly<regular>());
    }
}