//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 <boost/simd/arithmetic/include/functions/divround.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/maxflint.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>

NT2_TEST_CASE_TPL ( divround_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::divround;
  using boost::simd::tag::divround_;
  typedef typename boost::dispatch::meta::call<divround_(T,T)>::type r_t;
  typedef T wished_r_t;

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

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(divround(boost::simd::Inf<T>(), boost::simd::Inf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Minf<T>(), boost::simd::Minf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Nan<T>(), boost::simd::Nan<T>()), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(divround(T(4),T(0)), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(divround(T(4),T(3)), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Mone<T>(),boost::simd::Zero<T>()), boost::simd::Minf<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::One<T>(),boost::simd::Zero<T>()), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Zero<T>(),boost::simd::Zero<T>()), boost::simd::Nan<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( divround_unsigned_int,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::divround;
  using boost::simd::tag::divround_;
  typedef typename boost::dispatch::meta::call<divround_(T,T)>::type r_t;
  typedef T wished_r_t;

  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(divround(T(4),T(0)), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(divround(T(4),T(3)), T(1));
  NT2_TEST_EQUAL(divround(T(6),T(4)), T(2));
  NT2_TEST_EQUAL(divround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::Valmax<T>(),  boost::simd::Two<T>()), boost::simd::Valmax<r_t>()/boost::simd::Two<T>()+boost::simd::One<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( divround_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::divround;
  using boost::simd::tag::divround_;
  typedef typename boost::dispatch::meta::call<divround_(T,T)>::type r_t;
  typedef T wished_r_t;

  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(divround(T(-4),T(0)), boost::simd::Valmin<r_t>());
  NT2_TEST_EQUAL(divround(T(4),T(0)), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(divround(T(4),T(3)), T(1));
  NT2_TEST_EQUAL(divround(T(-4),T(-3)), T(1));
  NT2_TEST_EQUAL(divround(T(4),T(-3)), T(-1));
  NT2_TEST_EQUAL(divround(T(-4),T(3)), T(-1));
  NT2_TEST_EQUAL(divround(T(5),T(3)), T(2));
  NT2_TEST_EQUAL(divround(T(-5),T(-3)), T(2));
  NT2_TEST_EQUAL(divround(T(5),T(-3)), T(-2));
  NT2_TEST_EQUAL(divround(T(-5),T(3)), T(-2));

  NT2_TEST_EQUAL(divround(T(5),T(4)), T(1));
  NT2_TEST_EQUAL(divround(T(-5),T(-4)), T(1));
  NT2_TEST_EQUAL(divround(T(5),T(-4)), T(-1));
  NT2_TEST_EQUAL(divround(T(-5),T(4)), T(-1));
  NT2_TEST_EQUAL(divround(T(6),T(4)), T(2));
  NT2_TEST_EQUAL(divround(T(-6),T(-4)), T(2));
  NT2_TEST_EQUAL(divround(T(6),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround(T(-6),T(4)), T(-2));
  NT2_TEST_EQUAL(divround(T(8),T(4)), T(2));
  NT2_TEST_EQUAL(divround(T(-8),T(-4)), T(2));
  NT2_TEST_EQUAL(divround(T(8),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround(T(-8),T(4)), T(-2));
  NT2_TEST_EQUAL(divround(T(9),T(4)), T(2));
  NT2_TEST_EQUAL(divround(T(-9),T(-4)), T(2));
  NT2_TEST_EQUAL(divround(T(9),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround(T(-9),T(4)), T(-2));
  NT2_TEST_EQUAL(divround(T(10),T(4)), T(3));
  NT2_TEST_EQUAL(divround(T(-10),T(-4)), T(3));
  NT2_TEST_EQUAL(divround(T(10),T(-4)), T(-3));
  NT2_TEST_EQUAL(divround(T(-10),T(4)), T(-3));

  NT2_TEST_EQUAL(divround(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
} // end of test for signed_int_


/* Boost numeric test of the symplectic steppers test file

 Copyright 2012 Mario Mulansky
 Copyright 2012 Karsten Ahnert

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_symplectic

#include <iostream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double value_type;

typedef boost::array< double ,1 > state_type;

// harmonic oscillator, analytic solution x[0] = sin( t )
struct osc
{
    void operator()( const state_type &q , state_type &dpdt ) const
    {
        dpdt[0] = -q[0];
    }
};

BOOST_AUTO_TEST_SUITE( numeric_symplectic_test )

/* generic test for all symplectic steppers */
template< class Stepper >
struct perform_symplectic_test
{
    void operator()( void )
    {

        Stepper stepper;
        const int o = stepper.order()+1; //order of the error is order of approximation + 1

        const state_type q0 = {{ 0.0 }};
        const state_type p0 = {{ 1.0 }};
        state_type q1,p1;
        std::pair< state_type , state_type >x1( q1 , p1 );
        const double t = 0.0;
        /* do a first step with dt=0.1 to get an estimate on the prefactor of the error dx = f * dt^(order+1) */
        double dt = 0.5;
        stepper.do_step( osc() , std::make_pair( q0 , p0 ) , t , x1 , dt );
        const double f = 2.0 * std::abs( sin(dt) - x1.first[0] ) / std::pow( dt , o );

        std::cout << o << " , " << f << std::endl;

        /* as long as we have errors above machine precision */
        while( f*std::pow( dt , o ) > 1E-16 )
        {
            stepper.do_step( osc() , std::make_pair( q0 , p0 ) , t , x1 , dt );
            std::cout << "Testing dt=" << dt << std::endl;
            BOOST_CHECK_SMALL( std::abs( sin(dt) - x1.first[0] ) , f*std::pow( dt , o ) );
            dt *= 0.5;
        }
    }
};

typedef mpl::vector<
    symplectic_euler< state_type > ,
    symplectic_rkn_sb3a_mclachlan< state_type >
    > symplectic_steppers;

BOOST_AUTO_TEST_CASE_TEMPLATE( symplectic_test , Stepper, symplectic_steppers )
{
    perform_symplectic_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__SE2_HPP_
#define SMOOTH__SE2_HPP_

#include <Eigen/Core>

#include <complex>

#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "internal/se2.hpp"
#include "so2.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SE3;
// \endcond

/**
 * @brief Base class for SE2 Lie group types.
 *
 * Internally represented as \f$\mathbb{U}(1) \times \mathbb{R}^2\f$.
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [x, y, q_z, q_w] \f$
 * - Tangent:  \f$ \mathbf{a} = [v_x, v_y, \omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$q_z^2 + q_w^2 = 1 \f$
 * - Tangent: \f$ -\pi < \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * \f[
 * \mathbf{X} =
 * \begin{bmatrix}
 *  q_w & -q_z & x \\
 *  q_z &  q_w & y \\
 *  0   &    0 & 1
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 *
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   0 & -\omega_z & v_x \\
 *  \omega_z &   0 & v_y \\
 *  0 & 0 & 0
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 */
template<typename _Derived>
class SE2Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  SE2Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Access SO(2) part.
   */
  Eigen::Map<SO2<Scalar>> so2() requires is_mutable
  {
    return Eigen::Map<SO2<Scalar>>(static_cast<_Derived &>(*this).data() + 2);
  }

  /**
   * @brief Const access SO(2) part.
   */
  Eigen::Map<const SO2<Scalar>> so2() const
  {
    return Eigen::Map<const SO2<Scalar>>(static_cast<const _Derived &>(*this).data() + 2);
  }

  /**
   * @brief Access R2 part.
   */
  Eigen::Map<Eigen::Matrix<Scalar, 2, 1>> r2() requires is_mutable
  {
    return Eigen::Map<Eigen::Matrix<Scalar, 2, 1>>(static_cast<_Derived &>(*this).data());
  }

  /**
   * @brief Const access R2 part.
   */
  Eigen::Map<const Eigen::Matrix<Scalar, 2, 1>> r2() const
  {
    return Eigen::Map<const Eigen::Matrix<Scalar, 2, 1>>(
      static_cast<const _Derived &>(*this).data());
  }

  /**
   * @brief Tranformation action on 2D vector.
   */
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return so2() * v + r2();
  }

  /**
   * @brief Lift to SE3.
   *
   * @note SE3 header must be included.
   */
  SE3<Scalar> lift_se3() const
  {
    return SE3<Scalar>(
      so2().lift_so3(), Eigen::Matrix<Scalar, 3, 1>(r2().x(), r2().y(), Scalar(0)));
  }
};

// \cond
template<typename _Scalar>
class SE2;
// \endcond

// \cond
template<typename _Scalar>
struct lie_traits<SE2<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = SE2Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = SE2<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class SE2 : public SE2Base<SE2<_Scalar>>
{
  using Base = SE2Base<SE2<_Scalar>>;

  SMOOTH_GROUP_API(SE2);

public:
  /**
   * @brief Construct from SO2 and R2.
   *
   * @param so2 orientation component.
   * @param r2 translation component.
   */
  template<typename SO2Derived, typename T2Derived>
  SE2(const SO2Base<SO2Derived> & so2, const Eigen::MatrixBase<T2Derived> & r2)
  {
    Base::so2() = so2;
    Base::r2()  = r2;
  }
};

using SE2f = SE2<float>;   ///< SE2 with float
using SE2d = SE2<double>;  ///< SE2 with double

}  // namespace smooth

// MAP TYPE TRAITS

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<smooth::SE2<_Scalar>>>
    : public lie_traits<smooth::SE2<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<smooth::SE2<_Scalar>> : public smooth::SE2Base<Eigen::Map<smooth::SE2<_Scalar>>>
{
  using Base = smooth::SE2Base<Eigen::Map<smooth::SE2<_Scalar>>>;

  SMOOTH_MAP_API(Map);
};

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<const smooth::SE2<_Scalar>>>
    : public lie_traits<smooth::SE2<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<const smooth::SE2<_Scalar>>
    : public smooth::SE2Base<Eigen::Map<const smooth::SE2<_Scalar>>>
{
  using Base = smooth::SE2Base<Eigen::Map<const smooth::SE2<_Scalar>>>;

  SMOOTH_CONST_MAP_API(Map);
};

#endif  // SMOOTH__SE2_HPP_


#include <iostream>
#include <fstream>
#include <ctime>
#include <stdlib.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
using namespace std;
using namespace boost;

int main(int argc, char* argv[]) {

 typedef adjacency_list<setS, vecS, undirectedS> graph_type;
 size_t numNodes = atoi(argv[1]);
 size_t numEdges = atoi(argv[2]);
 ofstream outputFile;
 outputFile.open(argv[3]);
 boost::mt19937 rng;
 rng.seed(uint32_t(time(0)));

// Build graph
 graph_type graph(0);

 generate_random_graph(graph, numNodes, numEdges, rng, false, false);

 write_graphviz(outputFile, graph);
 outputFile.close();

 return 0;
}


#include "test_ntl.hpp"
#include <NTL/ZZ.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#define LINEARITY_CHECK
#include "tinymt32.h"
#include "f2p_gmp.h"
#include <string>
#include <stdio.h>
#include <inttypes.h>
#include <ctype.h>

using namespace NTL;
using namespace std;

// parameter no check
void tinymt32_add(tinymt32_t *a, tinymt32_t *b)
{
    for (int i = 0; i < 4; i++) {
        a->status[i] ^= b->status[i];
    }
}

void ok_print(bool ok, bool verbose)
{
    if (verbose) {
        if (ok) {
            printf("o");
        } else {
            printf("x");
        }
    }
}

void ntl_calc_jump(GF2X& jump, const GF2X& minpoly, ZZ& step)
{
    PowerXMod(jump, step, minpoly);
}

void gmp_calc_jump(GF2X& jump, mpz_t minpoly, mpz_t step)
{
    mpz_t jpoly;
    static char buff[2000];
    mpz_init(jpoly);
    f2p_calc_jump(jpoly, minpoly, step);
    f2p_get_hexstr(buff, jpoly);
    hexto_poly(jump, buff);
    mpz_clear(jpoly);
}

bool test_calc_jump(long degree, bool verbose)
{
    bool ok = true;
    GF2X poly;
    GF2X ntl_minpoly;
    BuildIrred(poly, degree);
    long step = degree * 10 + 7;
    ZZ ntl_step(step);
    GF2X ntl_jump;
    GF2X f2p_jump;
    mpz_t f2p_minpoly;
    mpz_t f2p_step;
    mpz_inits(f2p_minpoly, f2p_step, NULL);
    mpz_set_ui(f2p_step, step);
    string work;
    for (int i = 0; i < 10; i++) {
        BuildRandomIrred(ntl_minpoly, poly);
        to_hexstring(work, ntl_minpoly);
        f2p_set_hexstr(f2p_minpoly, work.c_str());
        ntl_calc_jump(ntl_jump, ntl_minpoly, ntl_step);
        gmp_calc_jump(f2p_jump, f2p_minpoly, f2p_step);
        if (ntl_jump != f2p_jump) {
            to_hexstring(work, ntl_jump);
            printf("ntl_jump:%s\n", work.c_str());
            to_hexstring(work, f2p_jump);
            printf("f2p_jump:%s\n", work.c_str());
            ok = false;
        }
        ok_print(ok, verbose);
    }
    if (verbose) {
        printf("\n");
    }
    mpz_clears(f2p_minpoly, f2p_step, NULL);
    return ok;
}

int main(int argc, char * argv[])
{
    bool verbose = false;
    bool ok = true;
    long degree = 127;
    if (argc > 1 && argv[1][0] == 'v') {
        verbose = true;
    }
    if (argc > 2) {
        degree = strtol(argv[2], NULL, 10);
    }
#if 0
    tiny32.mat1 = 0x8f7011ee;
    tiny32.mat2 = 0xfc78ff1f;
    tiny32.tmat = 0x3793fdff;

    tinymt32_init(&tiny32, 1);
#endif

    ok = ok && test_calc_jump(degree, verbose);
    if (ok) {
        return 0;
    } else {
        return -1;
    }
}


//
// Helper functions to construct MPC-style (continuous replanning) style policies using the iLQRTree
// class.
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>
#include <ilqr/ilqr_tree.hh>

#include <Eigen/Dense>

#include <vector>

namespace policy
{

// Does hindsight optimization over a probabilistic split in dynamics.
// Returns a control vector "u" by constructing a hindsight_split style tree.
// Linearly interpolates from xt to xT for the initialization for iLQR with nominal_control for all
// timesteps.
Eigen::VectorXd hindsight_tree_policy(const int t,
                                      const Eigen::VectorXd& xt,
                                      const int T,
                                      const Eigen::VectorXd& xT,
                                      const Eigen::VectorXd& nominal_control,
                                      const std::vector<double> &probabilities,
                                      const std::vector<ilqr::DynamicsFunc> &dynamics_funcs,
                                      const ilqr::CostFunc &cost,
                                      ilqr::iLQRTree& ilqr_tree);

// Does hindsight optimization over a probabilistic split in cost functions.
// Returns a control vector "u" by constructing a hindsight_split style tree.
// Linearly interpolates from xt to xT for the initialization for iLQR with nominal_control for all
// timesteps.
Eigen::VectorXd hindsight_tree_policy(const int t,
                                      const Eigen::VectorXd& xt,
                                      const int T,
                                      const Eigen::VectorXd& xT,
                                      const Eigen::VectorXd& nominal_control,
                                      const std::vector<double> &probabilities,
                                      const ilqr::DynamicsFunc &dynamics_func,
                                      const std::vector<ilqr::CostFunc> &cost_funcs,
                                      ilqr::iLQRTree& ilqr_tree);

// Returns a control vector "u" by constructing a chain tree structure.
Eigen::VectorXd chain_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const ilqr::DynamicsFunc &dynamics,
        const ilqr::CostFunc &cost,
        ilqr::iLQRTree& ilqr_tree);

// Probability weighted controller over a probabilistic split in dynamics functions.
Eigen::VectorXd probability_weighted_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const std::vector<double> &probabilities,
        const std::vector<ilqr::DynamicsFunc> &dynamics_funcs,
        const ilqr::CostFunc &cost);

// Probability weighted controller over a probabilistic split in cost functions.
Eigen::VectorXd probability_weighted_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const std::vector<double> &probabilities,
        const ilqr::DynamicsFunc &dynamics_func,
        const std::vector<ilqr::CostFunc> &cost_funcs);

} // namespace policy


/* test_uniform_real_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_real_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::uniform_real_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE -0.5
#define BOOST_RANDOM_ARG2_VALUE 1.5

#define BOOST_RANDOM_DIST0_MIN 0.0
#define BOOST_RANDOM_DIST0_MAX 1.0
#define BOOST_RANDOM_DIST1_MIN -0.5
#define BOOST_RANDOM_DIST1_MAX 1.0
#define BOOST_RANDOM_DIST2_MIN -0.5
#define BOOST_RANDOM_DIST2_MAX 1.5

#define BOOST_RANDOM_TEST1_PARAMS (-1.0, 0.0)
#define BOOST_RANDOM_TEST1_MIN -1.0
#define BOOST_RANDOM_TEST1_MAX 0.0

#define BOOST_RANDOM_TEST2_PARAMS
#define BOOST_RANDOM_TEST2_MIN 0.0
#define BOOST_RANDOM_TEST2_MAX 1.0

#include "test_distribution.ipp"


#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Core>

#include "update_ops.hpp"
#include "update_ops_cpp.hpp"
#include "utility.hpp"

void double_qubit_dense_matrix_gate(UINT target_qubit_index1,
    UINT target_qubit_index2, const CTYPE matrix[16], CTYPE* state, ITYPE dim) {
    double_qubit_dense_matrix_gate_c(
        target_qubit_index1, target_qubit_index2, matrix, state, dim);
}

void double_qubit_dense_matrix_gate(UINT target_qubit_index1,
    UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    double_qubit_dense_matrix_gate_eigen(
        target_qubit_index1, target_qubit_index2, eigen_matrix, state, dim);
}

void double_qubit_dense_matrix_gate_eigen(UINT target_qubit_index1,
    UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // target mask

    const UINT min_qubit_index =
        get_min_ui(target_qubit_index1, target_qubit_index2);
    const UINT max_qubit_index =
        get_max_ui(target_qubit_index1, target_qubit_index2);
    const ITYPE min_qubit_mask = 1ULL << min_qubit_index;
    const ITYPE max_qubit_mask = 1ULL << (max_qubit_index - 1);
    const ITYPE low_mask = min_qubit_mask - 1;
    const ITYPE mid_mask = (max_qubit_mask - 1) ^ low_mask;
    const ITYPE high_mask = ~(max_qubit_mask - 1);

    const ITYPE target_mask1 = 1ULL << target_qubit_index1;
    const ITYPE target_mask2 = 1ULL << target_qubit_index2;
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // loop variables
    const ITYPE loop_dim = dim / 4;
    ITYPE state_index;

    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create index
        ITYPE basis_0 = (state_index & low_mask) +
                        ((state_index & mid_mask) << 1) +
                        ((state_index & high_mask) << 2);

        // gather index
        ITYPE basis_1 = basis_0 + target_mask1;
        ITYPE basis_2 = basis_0 + target_mask2;
        ITYPE basis_3 = basis_1 + target_mask2;

        // fetch values
        Eigen::Vector4cd vec(
            state[basis_0], state[basis_1], state[basis_2], state[basis_3]);
        vec = eigen_matrix * vec;
        eigen_state[basis_0] = vec[0];
        eigen_state[basis_1] = vec[1];
        eigen_state[basis_2] = vec[2];
        eigen_state[basis_3] = vec[3];
    }
}


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

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MODF_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MODF_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD(modf_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     using result = std::pair < A0, A0>;
     BOOST_FORCEINLINE result operator()(A0 const& a0) const
      {
        A0 ent = bs::trunc(a0);
        A0 frac = a0-ent;
        return result(frac, ent);
      }
   };

} } }

#endif


#include <fstream>
#include <functional>
#include <iostream>
#include <queue>
#include <unordered_map>

#include <boost/dynamic_bitset.hpp>
#include <boost/serialization/unordered_map.hpp>
#include "serialize.hpp"

#include "huffman.hpp"

using namespace std;
using namespace boost::archive;
using boost::dynamic_bitset;

struct node {
    char c;
    uint32_t count;

    node *lhs, *rhs;

    ~node() {
        delete lhs;
        delete rhs;
    }
};

using huffman_tree = priority_queue<node *, vector<node *>, function<bool(const node *, const node *)>>;

auto count(ifstream &ifs) {
    unordered_map<char, uint32_t> freq_map;
    char c;

    while (ifs.get(c)) {
        ++freq_map[c];
    }

    return freq_map;
}

bool node_cmp(const node *lhs, const node *rhs) {
    return lhs->count == rhs->count ? lhs->c < rhs->c : lhs->count > rhs->count;
}

void _codes(const node *node,
            dynamic_bitset<> &code,
            unordered_map<char, dynamic_bitset<>> &encode_map) {
    if (node->c == '\0') {
        code.push_back(0); _codes(node->lhs, code, encode_map); code.pop_back();
        code.push_back(1); _codes(node->rhs, code, encode_map); code.pop_back();
    }
    else {
        encode_map[node->c] = { code };
    }
}

auto codes(const huffman_tree &pq) {
    dynamic_bitset<> code;
    unordered_map<char, dynamic_bitset<>> encode_map;

    _codes(pq.top(), code, encode_map);

    return encode_map;
}

auto build_tree(const unordered_map<char, uint32_t> &freq_map) {
    huffman_tree pq(node_cmp);

    for (auto && [ key, value ] : freq_map) {
        pq.push(new node({ key, value, nullptr, nullptr }));
    }

    while (pq.size() > 1) {
        auto lhs = pq.top(); pq.pop();
        auto rhs = pq.top(); pq.pop();

        auto root = new node({ '\0', lhs->count + rhs->count, lhs, rhs });
        pq.push(root);
    }

    return pq;
}

void encode(const string &file) {
    ifstream ifs(file);

    auto freq_map = count(ifs);
    auto pq = build_tree(freq_map);

    ifs.clear();
    ifs.seekg(0, ios::beg);

    auto encode_map = codes(pq);

    dynamic_bitset<> data;
    char c;

    while (ifs.get(c)) {
        for (auto i = 0u; i < encode_map[c].size(); ++i) {
            data.push_back(encode_map[c][i]);
        }
    }

    ofstream ofs(file + ".huf"s);
    binary_oarchive oa(ofs);
    oa << freq_map;
    oa << data;
}

void decode(const string &file) {
    unordered_map<char, uint32_t> freq_map;
    dynamic_bitset<> data;
    ifstream ifs(file);

    binary_iarchive ia(ifs);
    ia >> freq_map;
    ia >> data;

    auto pq = build_tree(freq_map);

    ofstream ofs(file.substr(0, file.length() - 4) + ".dec"s);

    auto curr = pq.top();

    for (auto i = 0u; i < data.size(); ++i) {
        curr = data[i] ? curr->rhs : curr->lhs;

        if (curr->c != '\0') {
            ofs << curr->c;
            curr = pq.top();
        }
    }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "weighted_alpha_complex_non_visible_points"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epick_d.h>
#include <CGAL/Epeck_d.h>

#include <vector>

#include <gudhi/Alpha_complex.h>
#include <gudhi/Simplex_tree.h>

using list_of_1d_kernel_variants = boost::mpl::list<CGAL::Epeck_d< CGAL::Dynamic_dimension_tag >,
                                                    CGAL::Epeck_d< CGAL::Dimension_tag<1>>,
                                                    CGAL::Epick_d< CGAL::Dynamic_dimension_tag >,
                                                    CGAL::Epick_d< CGAL::Dimension_tag<1>>
                                                    >;

BOOST_AUTO_TEST_CASE_TEMPLATE(Weighted_alpha_complex_non_visible_points, Kernel, list_of_1d_kernel_variants) {
  // check that for 2 closed weighted 1-d points, one with a high weight to hide the second one with a small weight,
  // that the point with a small weight has the same high filtration value than the edge formed by the 2 points
  using Point_d = typename Kernel::Point_d;
  std::vector<Point_d> points;
  std::vector<double> p1 {0.};
  points.emplace_back(p1.begin(), p1.end());
  // closed enough points
  std::vector<double> p2 {0.1};
  points.emplace_back(p2.begin(), p2.end());
  std::vector<typename Kernel::FT> weights {100., 0.01};

  Gudhi::alpha_complex::Alpha_complex<Kernel, true> alpha_complex(points, weights);
  Gudhi::Simplex_tree<> stree;
  BOOST_CHECK(alpha_complex.create_complex(stree));

  std::clog << "Iterator on weighted alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : stree.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : stree.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << stree.filtration(f_simplex) << "] " << std::endl;
  }

  BOOST_CHECK(stree.filtration(stree.find({0})) == -100.);
  BOOST_CHECK(stree.filtration(stree.find({1})) == stree.filtration(stree.find({0, 1})));
  BOOST_CHECK(stree.filtration(stree.find({1})) > 100000);
}


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

using namespace Eigen;

// [functor]
template<class ArgType, class RowIndexType, class ColIndexType>
class indexing_functor {
  const ArgType &m_arg;
  const RowIndexType &m_rowIndices;
  const ColIndexType &m_colIndices;
public:
  typedef Matrix<typename ArgType::Scalar,
                 RowIndexType::SizeAtCompileTime,
                 ColIndexType::SizeAtCompileTime,
                 ArgType::Flags&RowMajorBit?RowMajor:ColMajor,
                 RowIndexType::MaxSizeAtCompileTime,
                 ColIndexType::MaxSizeAtCompileTime> MatrixType;

  indexing_functor(const ArgType& arg, const RowIndexType& row_indices, const ColIndexType& col_indices)
    : m_arg(arg), m_rowIndices(row_indices), m_colIndices(col_indices)
  {}

  const typename ArgType::Scalar& operator() (Index row, Index col) const {
    return m_arg(m_rowIndices[row], m_colIndices[col]);
  }
};
// [functor]

// [function]
template <class ArgType, class RowIndexType, class ColIndexType>
CwiseNullaryOp<indexing_functor<ArgType,RowIndexType,ColIndexType>, typename indexing_functor<ArgType,RowIndexType,ColIndexType>::MatrixType>
mat_indexing(const Eigen::MatrixBase<ArgType>& arg, const RowIndexType& row_indices, const ColIndexType& col_indices)
{
  typedef indexing_functor<ArgType,RowIndexType,ColIndexType> Func;
  typedef typename Func::MatrixType MatrixType;
  return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices));
}
// [function]

int main()
{
  std::cout << "[main1]\n";
  Eigen::MatrixXi A = Eigen::MatrixXi::Random(4,4);
  Array3i ri(1,2,1);
  ArrayXi ci(6); ci << 3,2,1,0,0,2;
  Eigen::MatrixXi B = mat_indexing(A, ri, ci);
  std::cout << "A =" << std::endl;
  std::cout << A << std::endl << std::endl;
  std::cout << "A([" << ri.transpose() << "], [" << ci.transpose() << "]) =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "[main1]\n";

  std::cout << "[main2]\n";
  B =  mat_indexing(A, ri+1, ci);
  std::cout << "A(ri+1,ci) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#if __cplusplus >= 201103L
  B =  mat_indexing(A, ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3));
  std::cout << "A(ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#endif
  std::cout << "[main2]\n";
}


//
// 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/core/containers/grid/grid.h>
#include <cmath>

#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>

template <typename grid_type>
void grid_test( grid_type & G )
{
  typedef typename grid_type::value_type     value_type;
  typedef typename grid_type::math_types     math_types;
  typedef typename math_types::vector3_type  vector3_type;
  typedef typename math_types::real_type     real_type;

  value_type tol = value_type(0.01);

  size_t I = 3;
  size_t J = 3;
  size_t K = 3;

  // testing creation
  {
    vector3_type min_coord (-1.0, -1.0, -1.0 );
    vector3_type max_coord ( 1.0,  1.0,  1.0 );

    G.create( min_coord, max_coord, I, J, K);

    BOOST_CHECK( I == G.I() );
    BOOST_CHECK( J == G.J() );
    BOOST_CHECK( K == G.K() );
    BOOST_CHECK( (I*J*K) == G.size() );
  }

  grid_type const & H = G;

  // testing read and write access to grid
  {
    size_t linear_index = 0;
    value_type input    = 0;
    for(size_t k = 0;k<K;++k)
    {
      for(size_t j = 0;j<J;++j)
      {
        for(size_t i = 0;i<I;++i)
        {
          value_type output0 = G(i,j,k);
          BOOST_CHECK_CLOSE(output0, G.unused(), tol);

          G(i,j,k) = input;

          value_type const output1 = H(i,j,k);
          BOOST_CHECK_CLOSE(input, output1, tol);

          value_type const output2 = H.get_value(i,j,k);
          BOOST_CHECK_CLOSE(input, output2, tol);

          G( linear_index ) = input;

          value_type const output3 = H(linear_index);
          BOOST_CHECK_CLOSE(input, output3, tol);

          value_type const output4 = H.get_value(linear_index);
          BOOST_CHECK_CLOSE(input, output4, tol);

          linear_index += 1;
          input = input + 1;
        }
      }
    }
  }

  // iterator testing
  {
    grid_type cpy = G;

    typedef typename grid_type::iterator iterator;
    typedef typename grid_type::const_iterator const_iterator;

    iterator c = cpy.begin();
    iterator c_end = cpy.end();
    const_iterator g = H.begin();
    const_iterator g_end = H.end();

    for(;c!=c_end;++c,++g)
    {
      BOOST_CHECK_CLOSE( *c, *g, tol);
    }
  }

  // index iterator testing
  {
    grid_type cpy = G;

    typedef typename grid_type::index_iterator         index_iterator;
    typedef typename grid_type::const_index_iterator   const_index_iterator;

    index_iterator c = cpy.begin();
    index_iterator c_end = cpy.end();
    const_index_iterator g = H.begin();
    const_index_iterator g_end = H.end();

    for(size_t k = 0;k<K;++k)
    {
      for(size_t j = 0;j<J;++j)
      {
        for(size_t i = 0;i<I;++i)
        {
          BOOST_CHECK_CLOSE( *c, *g, tol);

          BOOST_CHECK( c.i() == i );
          BOOST_CHECK( c.j() == j );
          BOOST_CHECK( c.k() == k );

          BOOST_CHECK( g.i() == i );
          BOOST_CHECK( g.j() == j );
          BOOST_CHECK( g.k() == k );

          ++c;
          ++g;

        }
      }
    }
  }

  // min_element and max_element testing
  {
    value_type const min_value = - G(2,2,2)*2;
    value_type const max_value = - min_value;
    G(1,1,1) = min_value;
    G(0,1,1) = max_value;
    value_type tst_min = OpenTissue::grid::min_element( G );
    value_type tst_max = OpenTissue::grid::max_element( G );
    BOOST_CHECK_CLOSE( tst_min, min_value, tol);
    BOOST_CHECK_CLOSE( tst_max, max_value, tol);
  }

  // fabs testing
  {
    G = OpenTissue::grid::fabs(G);
  }

  // negate testing
  {
    G = OpenTissue::grid::negate(G);
  }
  // scale testing
  {
    value_type value = value_type(2);
    G = OpenTissue::grid::scale(G, value );
  }

}

BOOST_AUTO_TEST_SUITE(opentissue_grid);

BOOST_AUTO_TEST_CASE(test_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  {
    typedef OpenTissue::grid::Grid<float,math_types>         grid_type;
    grid_type G;
    grid_test(G);
  }
  {
    typedef OpenTissue::grid::Grid<double,math_types>        grid_type;
    grid_type G;
    grid_test(G);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <QApplication>
#include <QMessageBox>
#include <QMainWindow>
#include "Kernel_type.h"
#include "Polyhedron_type.h"
#include "Scene_polyhedron_item.h"
#include "Scene_surface_mesh_item.h"
#include "Scene_polylines_item.h"

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/Polygon_mesh_processing/stitch_borders.h>

#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/boost/graph/helpers.h>
#include <boost/graph/filtered_graph.hpp>

template <typename G>
struct Is_border {
  const G& g;
  Is_border(const G& g)
    : g(g)
  {}

 template <typename Descriptor>
  bool operator()(const Descriptor& d) const {
   return is_border(d,g);
  }

  bool operator()(typename boost::graph_traits<G>::vertex_descriptor d) const {
    return is_border(d,g) != boost::none;
  }

};

using namespace CGAL::Three;
class Polyhedron_demo_polyhedron_stitching_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

  QAction* actionDetectBorders;
  QAction* actionStitchBorders;
public:
  QList<QAction*> actions() const { return QList<QAction*>() << actionDetectBorders << actionStitchBorders; }
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* /* m */)
  {
    scene = scene_interface;
    actionDetectBorders= new QAction(tr("Detect Boundaries"), mainWindow);
    actionStitchBorders= new QAction(tr("Stitch Duplicated Boundaries"), mainWindow);
    actionDetectBorders->setObjectName("actionDetectBorders");
    actionStitchBorders->setObjectName("actionStitchBorders");
    actionStitchBorders->setProperty("subMenuName", "Polygon Mesh Processing");
    actionDetectBorders->setProperty("subMenuName", "Polygon Mesh Processing");
    autoConnectActions();
  }

  bool applicable(QAction*) const {
    Q_FOREACH(int index, scene->selectionIndices())
    {
      if ( qobject_cast<Scene_polyhedron_item*>(scene->item(index)) ||
           qobject_cast<Scene_surface_mesh_item*>(scene->item(index)) )
        return true;
    }
    return false;
  }

  template <typename Item>
  void on_actionDetectBorders_triggered(Scene_interface::Item_id index);

  template <typename Item>
  void on_actionStitchBorders_triggered(Scene_interface::Item_id index);

public Q_SLOTS:
  void on_actionDetectBorders_triggered();
  void on_actionStitchBorders_triggered();

}; // end Polyhedron_demo_polyhedron_stitching_plugin

template <typename Poly>
struct Polyline_visitor
{
  Scene_polylines_item* new_item;
  typename boost::property_map<Poly, CGAL::vertex_point_t>::const_type vpm;

  Polyline_visitor(const Poly& poly, Scene_polylines_item* new_item)
    : new_item(new_item), vpm(get(CGAL::vertex_point,poly))
  {}

  void start_new_polyline()
  {
    new_item->polylines.push_back( Scene_polylines_item::Polyline() );
  }

  void add_node(typename boost::graph_traits<Poly>::vertex_descriptor vd)
  {

    new_item->polylines.back().push_back(get(vpm,vd));
  }

  void end_polyline(){}
};

template <typename Item>
void Polyhedron_demo_polyhedron_stitching_plugin::on_actionDetectBorders_triggered(Scene_interface::Item_id index)
{
  typedef typename Item::Face_graph  FaceGraph;
  Item* item = qobject_cast<Item*>(scene->item(index));

  if(item)
    {
      Scene_polylines_item* new_item = new Scene_polylines_item();

      FaceGraph* pMesh = item->polyhedron();
      normalize_border(*pMesh);

      typedef boost::filtered_graph<FaceGraph,Is_border<FaceGraph>, Is_border<FaceGraph> > BorderGraph;

      Is_border<FaceGraph> ib(*pMesh);
      BorderGraph bg(*pMesh,ib,ib);
      Polyline_visitor<FaceGraph> polyline_visitor(*pMesh, new_item);
      CGAL::split_graph_into_polylines( bg,
                                        polyline_visitor,
                                        CGAL::internal::IsTerminalDefault() );

      if (new_item->polylines.empty())
        {
          delete new_item;
        }
      else
        {
          new_item->setName(tr("Boundary of %1").arg(item->name()));
          new_item->setColor(Qt::red);
          scene->addItem(new_item);
          new_item->invalidateOpenGLBuffers();
        }
    }
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionDetectBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()){
    on_actionDetectBorders_triggered<Scene_polyhedron_item>(index);
    on_actionDetectBorders_triggered<Scene_surface_mesh_item>(index);
  }
}

template <typename Item>
void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchBorders_triggered(Scene_interface::Item_id index)
{
  Item* item =
    qobject_cast<Item*>(scene->item(index));

  if(item){
    typename Item::Face_graph* pMesh = item->polyhedron();
    CGAL::Polygon_mesh_processing::stitch_borders(*pMesh);
    item->invalidateOpenGLBuffers();
    scene->itemChanged(item);
  }
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()){
    on_actionStitchBorders_triggered<Scene_polyhedron_item>(index);
    on_actionStitchBorders_triggered<Scene_surface_mesh_item>(index);
  }
}
#include "Polyhedron_stitching_plugin.moc"


/*
 * Regression.cpp
 *
 *  Created on: 2013/06/15
 *      Author: kryozahiro
 */

#include "Regression.h"

#include <cassert>
#include <boost/lexical_cast.hpp>
using namespace std;

Regression::Regression(const DataSet& dataSet) : Problem(dataSet.getProgramType()), dataSet(dataSet) {
}

Regression::Regression(const boost::property_tree::ptree& gameTree, mt19937_64& randomEngine) :
		Problem(fromPtree(gameTree, randomEngine).getProgramType()),
		dataSet(fromPtree(gameTree, randomEngine)) {
}

string Regression::toString() const {
	return dataSet.toString();
}

double Regression::evaluate(Program& program) {
	double diff = 0;
	for (int i = 0; i < dataSet.getSize(); ++i) {
		vector<double> output = program(dataSet[i].first);
		assert(getProgramType().getOutputType().accepts(output));

		//二乗誤差を求める
		for (int k = 0; k < getProgramType().getOutputType().getSize(); ++k) {
			double error = output[k] - dataSet[i].second[k];
			//diff += error * error;
			diff += fabs(error);
		}
	}
	if (diff < 0) {
		diff = DBL_MAX;
	}
	return diff;
}

/*string Regression::showProgram(Program& program) const {
	string ret;
	for (int i = 0; i < dataSet.getSize(); ++i) {
		const pair<vector<double>, vector<double>>& point = dataSet[i];

		ret += "in = ( ";
		for (double inputElement : point.first) {
			ret += boost::lexical_cast<string>(inputElement) + " ";
		}
		ret += ")\tdiffout = ( ";
		vector<double> output = program(point.first);
		for (unsigned int i = 0; i < output.size(); ++i) {
			ret += boost::lexical_cast<string>(output[i] - point.second[i]) + " ";
		}
		ret += ")\n";
	}
	return ret;
}*/

DataSet Regression::fromPtree(const boost::property_tree::ptree& gameTree, std::mt19937_64& randomEngine) {
	boost::property_tree::ptree dataTree = gameTree.get_child("DataSet");
	return DataSet(dataTree, randomEngine);
}


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2f M = Matrix2f::Random();
Matrix2f m;
m = M;
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Now we want to copy a column into a row." << endl;
cout << "If we do m.col(1) = m.row(0), then m becomes:" << endl;
m.col(1) = m.row(0);
cout << m << endl << "which is wrong!" << endl;
cout << "Now let us instead do m.col(1) = m.row(0).eval(). Then m becomes" << endl;
m = M;
m.col(1) = m.row(0).eval();
cout << m << endl << "which is right." << endl;

  return 0;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <functional>
#include <Eigen/LU>

#include "smpqeq.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(SMP_QEq)

Eigen::VectorXd SMP_QEq::EE_system(const std::vector<const Atom *> &atoms, double total_charge) const {

    size_t n = atoms.size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    for (int iter = 0; iter < 5; iter++)
    {
        for (size_t i = 0; i < n; i++) {
            const auto &atom_i = *atoms[i];
            A(i, i) = 2 * (parameters_->atom()->parameter(atom::second)(atom_i) +
                                parameters_->atom()->parameter(atom::third)(atom_i) * b(i) +
                                parameters_->atom()->parameter(atom::fourth)(atom_i) * b(i) * b(i));
            b(i) = -parameters_->atom()->parameter(atom::first)(atom_i);
            for (size_t j = i + 1; j < n; j++) {
                const auto &atom_j = *atoms[j];
                auto gamma = 2 * std::sqrt(parameters_->atom()->parameter(atom::second)(atom_i) *
                                           parameters_->atom()->parameter(atom::second)(atom_j));
                auto expr = 1 / std::cbrt(1 / std::pow(gamma, 3) + std::pow(distance(atom_i, atom_j), 3));
                A(i, j) = expr;
                A(j, i) = expr;
            }
        }

        A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
        A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
        A(n, n) = 0;
        b(n) = total_charge;

        b = A.partialPivLu().solve(b);
    }

    return b.head(n);
}

std::vector<double> SMP_QEq::calculate_charges(const Molecule &molecule) const {
    auto f = [this](const std::vector<const Atom *> &atoms, double total_charge) -> Eigen::VectorXd {
        return EE_system(atoms, total_charge);
    };

    Eigen::VectorXd q = solve_EE(molecule, f);
    return std::vector<double>(q.data(), q.data() + q.size());
}


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <unsupported/Eigen/MatrixFunctions>

//Global Variables
using Eigen::Vector3f;
Vector3f g(0,0,-9.81); //gravity

//Cross Product Equivalent
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f crossProductEquivalent(Vector3f v)
{
  Matrix3f c;
  c << 0, -v(2), v(1),
       v(2), 0, -v(0),
       -v(1), v(0), 0;
  //std::cout << c << std::endl;
  return c;
}

//Quaternion Multiplication
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qMultiply(Quaternionf q1, Quaternionf q2)
{
  float w1 = q1.w();
  float w2 = q2.w();
  Vector3f v1 = q1.vec();
  Vector3f v2 = q2.vec();
  float wReturn = w1*w2 - v1(0)*v2(0) - v1(1)*v2(1) - v1(2)*v2(2);
  Vector3f vReturn;
  vReturn(0) = w1*v2(0) + v1(0)*w2 + v1(1)*v2(2) - v1(2)*v2(1);
  vReturn(1) = w1*v2(1) - v1(0)*v2(2) + v1(1)*w2 + v1(2)*v2(0);
  vReturn(2) = w1*v2(2) + v1(0)*v2(1) - v1(1)*v2(0) + v1(2)*w2;
  Quaternionf qReturn;
  qReturn.w() = wReturn;
  qReturn.vec() = vReturn;
  //std::cout << qReturn.w() << std::endl << qReturn.vec() << std::endl;
  return qReturn;
}

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

//Quaternion to DCM
//ref: mathworks Quat2DCM documentation
using Eigen::Quaternionf;
using Eigen::Matrix3f;
Matrix3f q2dcm(Quaternionf q)
{
  float q0 = q.w();
  float q1 = q.x();
  float q2 = q.y();
  float q3 = q.z();
  Matrix3f DCM;
  DCM << (q0*q0 + q1*q1 - q2*q2 - q3*q3), 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2),
          2*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3), 2*(q2*q3 + q0*q1),
          2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), (q0*q0 - q1*q1 - q2*q2 + q3*q3);
  return DCM;
}

//Euler angles to DCM
//e = [phi, theta, psi]
//3-1-2 rotation
//ref: Todd Humphreys euler2dcm MATLAB function, "Aerial Robotics," 2019
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f euler2dcm(Vector3f e)
{
  std::cout << "Calculating trig values..." << std::endl;
  float cPhi = cos(e(0));
  float sPhi = sin(e(0));
  float cThe = cos(e(1));
  float sThe = sin(e(1));
  float cPsi = cos(e(2));
  float sPsi = sin(e(2));
  std::cout << "Building DCM..." << std::endl;
  Matrix3f DCM;
  DCM << (cPhi*cThe - sPhi*sPsi*sThe), (cThe*sPsi + cPsi * sPhi*sThe), (-cPhi*sThe),
         (-cPhi*sPsi),                                    (cPhi*cPsi),         sPhi,
         (cPsi*sThe + cThe*sPhi*sPsi), (sPsi*sThe - cPsi*cThe*sPhi),    (cPhi*cThe);
  return DCM;
}

//Estimate class
using Eigen::Quaternionf;
using Eigen::Vector3f;
class StateEstimate
{
  private:
    Quaternionf qHat;
    Vector3f pHat;
    Vector3f vHat;
    /*Vector3f dThetaHat;*/
    Vector3f bgHat;
    Vector3f baHat;
    float tk;

  public:
    StateEstimate(Quaternionf q, Vector3f p, Vector3f v, /*Vector3f dTheta,*/ Vector3f bg, Vector3f ba, float tk) {
      qHat = q;
      pHat = p;
      vHat = v;
      /*dThetaHat = dTheta;*/
      bgHat = bg;
      baHat = ba;
    }

    Quaternionf getqHat(){return qHat;}
    void setqHat(Quaternionf q){this->qHat = q;}
    /*Vector3f getdThetaHat(){return dThetaHat;}
    void setdThetaHat(Vector3f dTheta){this->dThetaHat = dThetaHat;}*/
    Vector3f getpHat(){return pHat;}
    void setpHat(Vector3f p){this->pHat = p;}
    Vector3f getvHat(){return vHat;}
    void setvHat(Vector3f v){this->vHat = v;}
    Vector3f getbgHat(){return bgHat;}
    void setbgHat(Vector3f bg){this->bgHat = bg;}
    Vector3f getbaHat(){return baHat;}
    void setbaHat(Vector3f ba){this->baHat = ba;}
    void gettk(){return tk;}
    void settk(float t){this->tk = t;}

};

//Estimator state class
using Eigen::Vector3f;
using Eigen::Matrix3f;
class EstimatorState
{
private:
	Quaternionf qBar;
	Vector3f pBar;
	Vector3f vBar;
	Vector3f e;
	Vector3f bgBar;
	Vector3f baBar;

	Matrix3f Pp;
	Matrix3f Pv;
	Matrix3f Pe;
	Matrix3f Pba;
	Matrix3f Pbg;

	float tk;

public:
	EstimatorState(Quaternionf q, Vector3f p, Vector3f v, Vector3f ee, Vector3f bg,
		Vector3f ba, Matrix3f pp, Matrix3f pv, Matrix3f pe, Matrix3f pba, Matrix3f pbg, float tk) {
        qBar = q;
        pBar = p;
        vBar = v;
        e = ee;
        bgBar = bg;
        baBar = ba;
        Pe = pe;
        Pp = pp;
        Pv = pv;
        Pba = pba;
        Pbg = pbg;
	}

	Quaternionf getqBar(){return qBar;}
    void setqBar(Quaternionf q){this->qBar = q;}
    /*Vector3f getdThetaHat(){return dThetaHat;}
    void setdThetaHat(Vector3f dTheta){this->dThetaHat = dThetaHat;}*/
    Vector3f getpBar(){return pBar;}
    void setpBar(Vector3f p){this->pBar = p;}
    Vector3f getvBar(){return vBar;}
    void sete(Vector3f ee){this->e = ee;}
    Vector3f gete(){return e;}
    void setvBar(Vector3f v){this->vBar = v;}
    Vector3f getbgBar(){return bgBar;}
    void setbgBar(Vector3f bg){this->bgBar = bg;}
    Vector3f getbaBar(){return baBar;}
    void setbaBar(Vector3f ba){this->baBar = ba;}
    void gettk(){return tk;}
    void settk(float t){this->tk = t;}
}

//Measurement class
using Eigen::Vector3f;
using Eigen::Matrix3f;
class Measurement
{
private:
	Vector3f wm;
	Vector3f am;

public:
	Measurement(Vector3f a, Vector3f w) {
        am = a;
        wm = w;
	}

	Vector3f getwm(){return wm;}
    Vector3f getam(){return am;}

}

//Error state class
/*using Eigen::Quaternionf;
using Eigen::Vector3f;
class ErrorState
{
  private:
    Quaternionf qTilde;
    Vector3f pTilde;
    Vector3f vTilde;
    Vector3f bgTilde;
    Vector3f baTilde;

  public:
    ErrorState(Quaternionf qT, Vector3f pT, Vector3f vT, Vector3f bgT, Vector3f baT) {
      qTilde = qT;
      pTilde = pT;

    }
}*/

//State Estimation (prop, update)
//EKF-based estimation
//ref: "Aerial Robotics," 'Laboratory Exercise 3: Meas. Simulation and State Estimation,' 2019
void prop(StateEstimate S, EstimatorState E)
{

}

void update(StateEstimate S, EstimatorState E, Measurements M)
{
    //Convert measurements to I-frame
    Vector3f wm = M.getwm();
    Vector3f am = M.getam();
    Matrix3f RBIBark = q2dcm(E.getqBar());
    Vector3f wmI = (RBIBark.transpose())*wm;
    Vector3f amI = (RBIBark.transpose())*am;

    //Push estimator state through measurement function
    //(relationship bewteen measurements and dynamics)
    //do stuff with image features

}

//Propagate State Estimate
/*StateEstimate propState(StateEstimate S, Vector3f am, Vector3f wm, float dt)
{
  // Constants
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  Matrix3f I3 = Matrix3f::Identity(3,3);
  Matrix3f O3 = Matrix3f::Zero();
  Matrix3f wmx = crossProductEquivalent(wm);

  //State values
  Quaternionf qHat = S.getqHat();
  Vector3f dThetaHat = S.getdThetaHat();
  Vector3f pHat = S.getpHat();
  Vector3f vHat = S.getvHat();
  Vector3f bgHat = S.getbgHat();
  Vector3f baHat = S.getbaHat();

  //Propagate quaternion
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  Quaternionf qExp = qExponential(dt, wm);
  Quaternionf qHatProp = qMultiply(qHat, qExp);

  //Calculate TIB, the DCM
  Matrix3f TIB = q2dcm(qHat);
  Matrix3f TBI = TIB.transpose();

  //Propagate p, the position
  Vector3f pHatProp = pHat
                      + vHat*dt
                      + 0.5*(TBI*crossProductEquivalent(am*(dt*dt)))*dThetaHat
                      - 0.5*TIB*baHat;

  //Propagate v, the velocity
  Vector3f vHatProp = vHat
  					  + (TBI*crossProductEquivalent((am-baHat)*dt))*dThetaHat
                      - TIB*dt*baHat;

  //Propagate dThetaHat, the 3x1 vector of angles of attitude error
  Vector3f dThetaHatProp = ((-crossProductEquivalent(wm)*dt).exp())*dThetaHat
                  - bgHat;

  //Propagate bg and ba, the gyroscope and accelerometer biases
  Vector3f bgHatProp = bgHat;
  Vector3f baHatProp = baHat;

  //Return the propagated state
  StateEstimate Sprop(qHatProp, pHatProp, vHatProp, dThetaHatProp, bgHatProp, baHatProp);
  return Sprop;
}*/

//State Estimator
//EKF-based estimation
//ref: "Aerial Robotics," 'Laboratory Exercise 3: Meas. Simulation and State Estimation,' 2019
StateEstimate stateEstimatorEKF(float tk, Vector3f am, Vector3f wm, )

int main()
{
  /*using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  Matrix3f A;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  Vector3f b;
  b << 1, 2, 3;
  Vector3f c;
  c << 4, 5, 6;
  std::cout << A*b << std::endl;
  std::cout << 0.5*A*b <<std::endl;
  float dt = 0.1;
  std::cout << 0.5*b*dt*dt <<std::endl;*/

  /*using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  Vector3f v(0.0, 3.1415, 6.2830);
  Matrix3f DCM = euler2dcm(v);
  std::cout << DCM << std::endl;*/

  Quaternionf q;
  Vector3f qv(0,0,0);
  float qw = 1;
  q.w() = qw;
  q.vec() = qv;
  Vector3f dTheta(0,0,0);
  Vector3f p(0,0,0);
  Vector3f v(0,0,0);
  Vector3f bg(0,0,0);
  Vector3f ba(0,0,0);
  StateEstimate S(q, p, v, dTheta, bg, ba);

  Vector3f am(0.2,0,-9.81);
  Vector3f wm(0.5*3.1415,0,0);
  float dt = 0.1;

  for ( int ii = 0; ii < 10; ii = ii + 1 ){
    StateEstimate Sprop = propState(S,am,wm,dt);
    std::cout << "qHatProp (w): " << Sprop.getqHat().w() << std::endl;
    std::cout << "qHatProp (v): " << std::endl << Sprop.getqHat().vec() << std::endl;
    std::cout << "dThetaHatProp: " << std::endl << Sprop.getdThetaHat() << std::endl;
    std::cout << "pHatProp: " << std::endl << Sprop.getpHat() << std::endl;
    std::cout << "vHatProp: " << std::endl << Sprop.getvHat() << std::endl;
    std::cout << "baHatProp: " << std::endl << Sprop.getbaHat() << std::endl;
    std::cout << "bgHatProp: " << std::endl << Sprop.getbgHat() << std::endl;
    std::cout << std::endl;
    S = Sprop;
  }

}


#include "BVH.h"
#include <iostream>
#include <Eigen/Geometry>
#include "dart/dart.hpp"
namespace MSS
{
Eigen::Matrix3d
R_x(double x)
{
	double cosa = cos(x*3.141592/180.0);
	double sina = sin(x*3.141592/180.0);
	Eigen::Matrix3d R;
	R<<	1,0		,0	  ,
		0,cosa	,-sina,
		0,sina	,cosa ;
	return R;
}
Eigen::Matrix3d R_y(double y)
{
	double cosa = cos(y*3.141592/180.0);
	double sina = sin(y*3.141592/180.0);
	Eigen::Matrix3d R;
	R <<cosa ,0,sina,
		0    ,1,   0,
		-sina,0,cosa;
	return R;
}
Eigen::Matrix3d R_z(double z)
{
	double cosa = cos(z*3.141592/180.0);
	double sina = sin(z*3.141592/180.0);
	Eigen::Matrix3d R;
	R<<	cosa,-sina,0,
		sina,cosa ,0,
		0   ,0    ,1;
	return R;
}
BVHNode::
BVHNode(const std::string& name,BVHNode* parent)
	:mParent(parent),mName(name),mChannelOffset(0),mNumChannels(0)
{

}
void
BVHNode::
SetChannel(int c_offset,std::vector<std::string>& c_name)
{
	mChannelOffset = c_offset;
	mNumChannels = c_name.size();
	for(const auto& cn : c_name)
		mChannel.push_back(CHANNEL_NAME[cn]);
}
void
BVHNode::
Set(const Eigen::VectorXd& m_t)
{
	mR.setIdentity();

	for(int i=0;i<mNumChannels;i++)
	{
		switch(mChannel[i])
		{
		case Xpos:break;
		case Ypos:break;
		case Zpos:break;
		case Xrot:mR = mR*R_x(m_t[mChannelOffset+i]);break;
		case Yrot:mR = mR*R_y(m_t[mChannelOffset+i]);break;
		case Zrot:mR = mR*R_z(m_t[mChannelOffset+i]);break;
		default:break;
		}
	}

}
void
BVHNode::
Set(const Eigen::Matrix3d& R_t)
{
	mR = R_t;
}
Eigen::Matrix3d
BVHNode::
Get()
{
	return mR;
}

void
BVHNode::
AddChild(BVHNode* child)
{
	mChildren.push_back(child);
}
BVHNode*
BVHNode::
GetNode(const std::string& name)
{
	if(!mName.compare(name))
		return this;

	for(auto& c : mChildren)
	{
		BVHNode* bn = c->GetNode(name);
		if(bn!=nullptr)
			return bn;
	}

	return nullptr;
}

Eigen::Vector3d
BVH::
GetP0()
{
	Eigen::VectorXd m_t = mMotions[0];

	Eigen::Vector3d p0 = m_t.segment<3>(0) - mRootCOMOffset;
	p0 *= 0.01;

	return p0;
}

BVH::
BVH()
{

}

void
BVH::
SetMotion(double t)
{

	int k = ((int)std::floor(t/mTimeStep));
	k = std::max(0,std::min(k,mNumTotalFrames-1));
	double dt = t/mTimeStep - std::floor(t/mTimeStep);
	Eigen::VectorXd m_t = mMotions[k];

	for(auto& bn: mMap)
		bn.second->Set(m_t);

	mRootCOM = m_t.segment<3>(0) - mRootCOMOffset;
	mRootCOM *= 0.01;

}
Eigen::Matrix3d
BVH::
Get(const std::string& bvh_node)
{
	return mMap[bvh_node]->Get();
}
void
BVH::
Parse(const std::string& file)
{
	std::ifstream is(file);

	char buffer[256];

	if(!is)
	{
		std::cout<<"Can't Open File"<<std::endl;
		return;
	}
	while(is>>buffer)
	{
		if(!strcmp(buffer,"HIERARCHY"))
		{
			is>>buffer;//Root
			is>>buffer;//Name
			int c_offset = 0;
			mRoot = ReadHierarchy(nullptr,buffer,c_offset,is);
			mNumTotalChannels = c_offset;
		}
		else if(!strcmp(buffer,"MOTION"))
		{
			is>>buffer; //Frames:
			is>>buffer; //num_frames
			mNumTotalFrames = atoi(buffer);
			is>>buffer; //Frame
			is>>buffer; //Time:
			is>>buffer; //time step
			mTimeStep = atof(buffer);
			mMotions.resize(mNumTotalFrames);
			for(auto& m_t : mMotions)
				m_t = Eigen::VectorXd::Zero(mNumTotalChannels);
			double val;
			for(int i=0;i<mNumTotalFrames;i++)
			{
				for(int j=0;j<mNumTotalChannels;j++)
				{
					is>>val;
					mMotions[i][j]=val;
				}
			}
		}
	}
	is.close();
}
BVHNode*
BVH::
ReadHierarchy(BVHNode* parent,const std::string& name,int& channel_offset,std::ifstream& is)
{
	char buffer[256];
	double offset[3];
	std::vector<std::string> c_name;

	BVHNode* new_node = new BVHNode(name,parent);
	mMap.insert(std::make_pair(name,new_node));

	is>>buffer; //{

	while(is>>buffer)
	{
		if(!strcmp(buffer,"}"))
			break;
		if(!strcmp(buffer,"OFFSET"))
		{
			//Ignore
			double x,y,z;

			is>>x;
			is>>y;
			is>>z;
			if(parent==nullptr)
			{
				mRootCOMOffset[0] = x;
				mRootCOMOffset[1] = y;
				mRootCOMOffset[2] = z;
			}
		}
		else if(!strcmp(buffer,"CHANNELS"))
		{

			is>>buffer;
			int n;
			n= atoi(buffer);

			for(int i=0;i<n;i++)
			{
				is>>buffer;
				c_name.push_back(std::string(buffer));
			}

			new_node->SetChannel(channel_offset,c_name);

			channel_offset+=n;
		}
		else if(!strcmp(buffer,"JOINT"))
		{
			is>>buffer;
			BVHNode* child = ReadHierarchy(new_node,std::string(buffer),channel_offset,is);
			new_node->AddChild(child);
		}
		else if(!strcmp(buffer,"End"))
		{
			is>>buffer;
			BVHNode* child = ReadHierarchy(new_node,std::string("EndEffector"),channel_offset,is);
			new_node->AddChild(child);
		}
	}

	return new_node;
}
std::map<std::string,MSS::BVHNode::CHANNEL> BVHNode::CHANNEL_NAME =
{
	{"Xposition",Xpos},
	{"XPOSITION",Xpos},
	{"Yposition",Ypos},
	{"YPOSITION",Ypos},
	{"Zposition",Zpos},
	{"ZPOSITION",Zpos},
	{"Xrotation",Xrot},
	{"XROTATION",Xrot},
	{"Yrotation",Yrot},
	{"YROTATION",Yrot},
	{"Zrotation",Zrot},
	{"ZROTATION",Zrot}
};
};


//	Prior for reconstructing the Equation of State of Dark Energy
//	ref:
//
//  @Jan-19-2017: This file is mainly copied from another projetc of mine, xcos.
//  Some improvemnet were made to make the code more readable.
//
//	@May-17-2019: Now the covariance matrix is loaded from a pre-computed data file

#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>
#include "Prior.hpp"

using namespace imcmc;
using namespace imcmc::parser;
using namespace std;

DDE_CPZ::DDE_CPZ(){
    floating = false;
    z = NULL;
    a = NULL;
    save_invcov = false; // this is for debug, set to false by default
}

DDE_CPZ::~DDE_CPZ() {

    if( dde_spacing == 0 )
        delete[] a;
    else if( dde_spacing == 1 )
        delete[] z;

}

void DDE_CPZ::Init( std::string& prior_settings ) {

    data_info.GetInfo(prior_settings);

    floating		= Read::Read_Bool_from_File(prior_settings, "dde_wfid_floating");
    nw 				= Read::Read_Int_from_File(prior_settings, "eos_bin_num");

    dde_z_max 		= Read::Read_Double_from_File(prior_settings, "tabulated_w_z_max");
    dde_a_min		= 1./(1.+dde_z_max);

    sigmafid 		= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_sigmafid");

    save_invcov     = Read::Read_Bool_from_File(prior_settings, "save_invcov");

//	====================================
//  load pre-computed covariance matrix
    string cov_file = Read::Read_String_from_File(prior_settings, "pre-computed_covmat");

//	====================================
//  load pre-determined bin centers
    string abin_file= Read::Read_String_from_File(prior_settings, "abin_centers");

    arma::vec abin_centers;
    abin_centers.load(abin_file,arma::raw_ascii);

//	============================================================
//	@ Nov-8, 2016, update 'sigma_mean_w' option names ...
//	sigma_mean_w is the same for either evenly spaced in z or a
//	============================================================
    sigma_mean_w	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_sigma_mean_w");

    C	= arma::zeros(nw, nw);
    iC	= arma::zeros(nw, nw);

    C.load(cov_file, arma::raw_ascii);

    //  We choose sigma_mean_w=0.04 as a reference , so if one wants to change sigma_mean_w,
    //  he just needs to rescale the loaded covariance matrix without re-compute
    //  the covariance matrix
    C = C * pow(sigma_mean_w/0.04,2);

    iC	= C.i();	//	evaluate the inverse of the covariance matrix

//  make a copy of the original covariance matrix without local-average
    C_original  = C;
    iC_original = iC;

    dde_spacing = Read::Read_Int_from_File(prior_settings,"dde_spacing");
    if( dde_spacing == 0 )
        a = new double[nw];
    else if( dde_spacing == 1 )
        z = new double[nw];

//	====================================================================
//	NOTE: zbin[] and abin[] defined below is DIFFERENT from that used in
//	eos approximation !!!
//	====================================================================
    double zbin[nw];
    double abin[nw];

    double Delta;
    double xi0;
    double xij, x_plus, x_minus, xbar;

    if( dde_spacing == 0 ) {		//	evenly spaced in a

        ac	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_ac");

        //  try to read smoothing scale $a_s$ from "prior_settings"
		if( Read::Has_Key_in_File(prior_settings,"dde_cpz_prior_as") ){
			as	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_as");
		}
		else{
			as	= ac;
		}

    //  Correct bin center of the last abin to make sure that it is correlated with the one next to it.
    // 	Added by XYH @20190516
        abin_centers[nw-1] = abin_centers[nw-2] - (as-1e-10);

        double da = (1.0-dde_a_min)/nw;
        for(int i=0; i<nw; ++i) {
            abin[i] = 1.0 - (i+0.5)*da; // center of each a-bin

            if( floating ){
                a[i] = abin_centers[i];
            }
        }

        Delta 	= da;
        xi0		= sigma_mean_w*sigma_mean_w*(1-dde_a_min)/_PI_/ac;
    }
    else if( dde_spacing == 1 ) {	//	evenly spaced in z

        zc 	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_zc");

		if( Read::Has_Key_in_File(prior_settings,"dde_cpz_prior_zs") ) {
			zs	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_zs");
		}
		else{
			zs	= zc;
		}

        double dz = dde_z_max/nw;
        for(int i=0; i<nw; ++i) {
            zbin[i] = (i+0.5)*dz;	// center of each a-bin

            if( floating )
                z[i] = zbin[i];
        }

        Delta 	= dz;
        xi0		= sigma_mean_w*sigma_mean_w*dde_z_max/_PI_/zc;
    }
    else {
        string err = "\n*** DDE_CPZ::Init() ==> unsupported z-spacing !!!";
        throw runtime_error(err);
    }

    S	= arma::zeros(nw, nw);  // smoothing matrix
    I	= arma::eye(nw,nw);

    if( floating ) {

        for(int i=0; i<nw; ++i) {
            int count=0;

            if( dde_spacing == 0 ) {
                for( int j=0; j<nw; ++j ) {
                    if( fabs(a[j] - a[i]) <= as + 1E-10 ) {
                        S(i,j) = 1.0;
                        ++count;
                    }
                }
            }
            else if( dde_spacing == 1 ) {
                for( int j=0; j<nw; ++j ) {
                    if( fabs(z[j] - z[i]) <= zs + 1E-10 ) {
                        S(i,j) = 1.0;
                        ++count;
                    }
                }
            }

            for( int j=0; j<nw; ++j )
                S(i,j) = S(i,j)/count;
        }

        IS = I-S;
        iC = IS.t() * iC * IS;
    }

//    S.save("EoS_smooth_matrix.txt", arma::raw_ascii);
//    iC.save("EoS_inv_covmat_20190701.txt",arma::raw_ascii);

    if( save_invcov ){
        if( MPI::COMM_WORLD.Get_rank() == 0 ){
            cout << "Saving inverse of the CPZ covariance matrix \n";
            iC.save("DDE_CPZ_prior_invcov_uncorrected.txt",arma::raw_ascii);
        }
    }

//    exit(0);
}

double Prior_DDE_CPZ(  	imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state ) {

    state.this_like_is_ok=true;

    lndet = chisq = 0;

    DDE_CPZ *cpz = static_cast<DDE_CPZ*>(data);

    arma::rowvec row_w = arma::zeros<arma::rowvec>(cpz->nw);
    arma::colvec col_w = arma::zeros<arma::colvec>(cpz->nw);

//  those extra high-z w_i will not be constrained by CPZ prior.

    for(int i=0; i<cpz->nw; ++i) {
        string wname= "DDE_w"+Read::IntToString(i);
        row_w[i] = param[wname];
        col_w[i] = row_w[i];
    }

    chisq = arma::as_scalar( row_w * cpz->iC * col_w );

    return -lndet - 0.5*chisq;
}


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE( JointPrismatic )

BOOST_AUTO_TEST_CASE(spatial)
{
  typedef TransformPrismaticTpl<double,0,0> TransformX;
  typedef TransformPrismaticTpl<double,0,1> TransformY;
  typedef TransformPrismaticTpl<double,0,2> TransformZ;

  typedef SE3::Vector3 Vector3;

  const double displacement = 0.2;
  SE3 Mplain, Mrand(SE3::Random());

  TransformX Mx(displacement);
  Mplain = Mx;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));

  TransformY My(displacement);
  Mplain = My;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));

  TransformZ Mz(displacement);
  Mplain = Mz;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));

  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPrismaticTpl<double,0,0> mp_x(2.);
  Motion mp_dense_x(mp_x);

  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

  MotionPrismaticTpl<double,0,1> mp_y(2.);
  Motion mp_dense_y(mp_y);

  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

  MotionPrismaticTpl<double,0,2> mp_z(2.);
  Motion mp_dense_z(mp_z);

  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
}

BOOST_AUTO_TEST_CASE( test_kinematics )
{
  using namespace pinocchio;

  Motion expected_v_J(Motion::Zero());
  Motion expected_c_J(Motion::Zero());

  SE3 expected_configuration(SE3::Identity());

  JointDataPX joint_data;
  JointModelPX joint_model;

  joint_model.setIndexes(0, 0, 0);

  Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
  Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));

  // -------
  q << 0. ;
  q_dot << 0.;

  joint_model.calc(joint_data, q, q_dot);

  BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
  BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
  BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
  BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));

  // -------
  q << 1.;
  q_dot << 1.;

  joint_model.calc(joint_data, q, q_dot);

  expected_configuration.translation() << 1, 0, 0;

  expected_v_J.linear() << 1., 0., 0.;

  BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
  BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
  BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
  BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
}

BOOST_AUTO_TEST_CASE( test_rnea )
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
  Eigen::VectorXd v(Eigen::VectorXd::Zero(model.nv));
  Eigen::VectorXd a(Eigen::VectorXd::Zero(model.nv));

  rnea(model, data, q, v, a);

  Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(model.nq));
  tau_expected  << 0;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));

  // -----
  q = Eigen::VectorXd::Ones(model.nq);
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << 1;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));

  q << 3;
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << 1;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
}

BOOST_AUTO_TEST_CASE( test_crba )
{
  using namespace pinocchio;
  using namespace std;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
  Eigen::MatrixXd M_expected(model.nv,model.nv);

  crba(model, data, q);
  M_expected << 1.0;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));

  q = Eigen::VectorXd::Ones(model.nq);

  crba(model, data, q);

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));

  q << 3;

  crba(model, data, q);

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1.,2.,3.),6.);
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsPX)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Eigen::Vector3d axis;
  axis << 1.0, 0.0, 0.0;

  Model modelPX, modelPrismaticUnaligned;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelPrismaticUnaligned joint_model_PU(axis);

  addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
  addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);

  Data dataPX(modelPX);
  Data dataPrismaticUnaligned(modelPrismaticUnaligned);

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelPX.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPX.nv);
  Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.nv);
  Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.nv);
  Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.nv);
  Eigen::VectorXd aPrismaticUnaligned(aPX);

  forwardKinematics(modelPX, dataPX, q, v);
  forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);

  computeAllTerms(modelPX, dataPX, q, v);
  computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);

  BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
  BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
  BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
  BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));

  BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
  BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));

  // InverseDynamics == rnea
  tauPX = rnea(modelPX, dataPX, q, v, aPX);
  tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);

  BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
  Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);

  BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));

  // crba
  crba(modelPX, dataPX,q);
  crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q);

  BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
  computeJointJacobians(modelPX, dataPX, q);
  computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
  getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
  getJointJacobian(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);

  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}

BOOST_AUTO_TEST_SUITE_END()


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines helper functions for converting between `DataVector`s and boost
/// quaternions.

#pragma once

#include "DataStructures/BoostMultiArray.hpp"

#include <boost/math/quaternion.hpp>
#include <boost/numeric/odeint.hpp>

#include "DataStructures/DataVector.hpp"

/// \cond
namespace gsl {
template <class T>
class not_null;
}  // namespace gsl
/// \endcond

/// Convert a `boost::math::quaternion` to a `DataVector`
DataVector quaternion_to_datavector(
    const boost::math::quaternion<double>& input);

/// \brief Convert a `DataVector` to a `boost::math::quaternion`
///
/// \details To convert to a quaternion, a `DataVector` must have either 3 or 4
/// components. If it has 3 components, the quaternion will be constructed with
/// 0 scalar part while the vector part is the `DataVector`. If the `DataVector`
/// has 4 components, the quaternion is just the `DataVector` itself.
boost::math::quaternion<double> datavector_to_quaternion(
    const DataVector& input);

/// Normalize a `boost::math::quaternion`
void normalize_quaternion(
    gsl::not_null<boost::math::quaternion<double>*> input);

// Necessary for odeint to be able to integrate boost quaternions
namespace boost::numeric::odeint {
template <>
struct vector_space_norm_inf<boost::math::quaternion<double>> {
  using result_type = double;
  result_type operator()(const boost::math::quaternion<double>& q) const {
    return sup(q);
  }
};
}  // namespace boost::numeric::odeint


#include <Eigen/Core>
#include <iostream>
#include <iomanip>
#include <string>

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

void eigs_sym_F77(MatrixXd &M, VectorXd &init_resid, int k, int m, double &time_used, double &prec_err, int &nops);
void eigs_gen_F77(MatrixXd &M, VectorXd &init_resid, int k, int m, double &time_used, double &prec_err, int &nops);
void eigs_sym_Cpp(MatrixXd &M, VectorXd &init_resid, int k, int m, double &time_used, double &prec_err, int &nops);
void eigs_gen_Cpp(MatrixXd &M, VectorXd &init_resid, int k, int m, double &time_used, double &prec_err, int &nops);

void print_header(std::string title)
{
    const int width = 80;
    const char sep = ' ';

    std::cout << std::endl
              << std::string(width, '=') << std::endl;
    std::cout << std::string((width - title.length()) / 2, ' ') << title << std::endl;
    std::cout << std::string(width, '-') << std::endl;

    std::cout << std::left << std::setw(7) << std::setfill(sep) << "size";
    std::cout << std::left << std::setw(10) << std::setfill(sep) << "dataset";
    std::cout << std::left << std::setw(11) << std::setfill(sep) << "F77/time";
    std::cout << std::left << std::setw(13) << std::setfill(sep) << "error";
    std::cout << std::left << std::setw(7) << std::setfill(sep) << "nops";
    std::cout << std::left << std::setw(11) << std::setfill(sep) << "C++/time";
    std::cout << std::left << std::setw(13) << std::setfill(sep) << "error";
    std::cout << std::left << std::setw(7) << std::setfill(sep) << "nops";
    std::cout << std::endl;

    std::cout << std::string(width, '-') << std::endl;
}

void print_row(int n, int dataset,
               double time_f77, double err_f77, int nops_f77,
               double time_cpp, double err_cpp, int nops_cpp)
{
    const char sep = ' ';

    std::cout.precision(5);

    std::cout << std::left << std::setw(7) << std::setfill(sep) << n;
    std::cout << std::left << std::setw(10) << std::setfill(sep) << dataset;
    std::cout << std::left << std::setw(11) << std::setfill(sep) << time_f77;
    std::cout << std::left << std::setw(13) << std::setfill(sep) << err_f77;
    std::cout << std::left << std::setw(7) << std::setfill(sep) << nops_f77;
    std::cout << std::left << std::setw(11) << std::setfill(sep) << time_cpp;
    std::cout << std::left << std::setw(13) << std::setfill(sep) << err_cpp;
    std::cout << std::left << std::setw(7) << std::setfill(sep) << nops_cpp;
    std::cout << std::endl;
}

void print_footer()
{
    const int width = 80;
    std::cout << std::string(width, '=') << std::endl
              << std::endl;
}

void run_eigs_sym(int n_experiment, int n_replicate, int n, int k, int m)
{
    double time_f77, time_cpp;
    double err_f77, err_cpp;
    int nops_f77, nops_cpp;

    for (int i = 0; i < n_experiment; i++)
    {
        MatrixXd A = MatrixXd::Random(n, n);
        MatrixXd M = A.transpose() + A;

        VectorXd init_resid = VectorXd::Random(M.cols());
        init_resid.array() -= 0.5;
        init_resid = M * init_resid;

        for (int j = 0; j < n_replicate; j++)
        {
            eigs_sym_F77(M, init_resid, k, m, time_f77, err_f77, nops_f77);
            eigs_sym_Cpp(M, init_resid, k, m, time_cpp, err_cpp, nops_cpp);
            print_row(n, i + 1, time_f77, err_f77, nops_f77, time_cpp, err_cpp, nops_cpp);
        }
    }
}

void run_eigs_gen(int n_experiment, int n_replicate, int n, int k, int m)
{
    double time_f77, time_cpp;
    double err_f77, err_cpp;
    int nops_f77, nops_cpp;

    for (int i = 0; i < n_experiment; i++)
    {
        MatrixXd A = MatrixXd::Random(n, n);

        VectorXd init_resid = VectorXd::Random(A.cols());
        init_resid.array() -= 0.5;
        init_resid = A * init_resid;

        for (int j = 0; j < n_replicate; j++)
        {
            eigs_gen_F77(A, init_resid, k, m, time_f77, err_f77, nops_f77);
            eigs_gen_Cpp(A, init_resid, k, m, time_cpp, err_cpp, nops_cpp);
            print_row(n, i + 1, time_f77, err_f77, nops_f77, time_cpp, err_cpp, nops_cpp);
        }
    }
}

int main()
{
    std::srand(123);
    int n_experiment = 5;
    int n_replicate = 10;

    print_header("eigs_sym");
    run_eigs_sym(n_experiment, n_replicate, 100, 10, 20);
    run_eigs_sym(n_experiment, n_replicate, 1000, 10, 30);
    print_footer();

    print_header("eigs_gen");
    run_eigs_gen(n_experiment, n_replicate, 100, 10, 20);
    run_eigs_gen(n_experiment, n_replicate, 1000, 10, 30);
    print_footer();

    return 0;
}


#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <EigenRand/EigenRand>

template <class T>
class ContinuousDistTest : public testing::Test
{
};

using ETypes = testing::Types<float, double>;

TYPED_TEST_CASE(ContinuousDistTest, ETypes);

TYPED_TEST(ContinuousDistTest, balanced)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::Vmt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::balanced<Matrix>(8, 8, gen);
	mat = Eigen::Rand::balanced<Matrix>(3, 3, gen);
	mat = Eigen::Rand::balanced<Matrix>(5, 5, gen);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, balanced2)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::Vmt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::balanced<Matrix>(8, 8, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(3, 3, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(5, 5, gen, 0.5, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, stdNormal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::Vmt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::normal<Matrix>(8, 8, gen);
	mat = Eigen::Rand::normal<Matrix>(3, 3, gen);
	mat = Eigen::Rand::normal<Matrix>(5, 5, gen);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, normal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::Vmt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::normal<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(5, 5, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, exponential)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::Vmt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::exponential<Matrix>(8, 8, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(3, 3, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(5, 5, gen, 2);
	std::cout << mat << std::endl;
}


#include <Eigen/StdVector>

#include <week_slam/g2o/edge/edge_point_on_circle.h>
#include <week_slam/barrel_ransac.h>

void BarrelRansac::test()
{
  double radius = 0.5;
  int num_points = 10;

  std::vector<Eigen::Vector2d> points;
  points.reserve(3 * num_points);

  std::uniform_real_distribution<> distribution(0, 2 * M_PI);
  std::normal_distribution<> rad_dist(radius, 0.05);

  {
    Eigen::Vector2d center{ 1.0, 2.0 };
    for (int i = 0; i < num_points; i++)
    {
      double r = rad_dist(mt_);
      double angle = distribution(mt_);

      double x = center.x() + r * cos(angle);
      double y = center.y() + r * sin(angle);

      points.emplace_back(x, y);
    }
  }

  {
    Eigen::Vector2d center{ 5.0, 4.0 };
    for (int i = 0; i < num_points; i++)
    {
      double r = rad_dist(mt_);
      double angle = distribution(mt_);

      double x = center.x() + r * cos(angle);
      double y = center.y() + r * sin(angle);

      points.emplace_back(x, y);
    }
  }

  {
    Eigen::Vector2d center{ 6.2, 4.0 };
    for (int i = 0; i < num_points; i++)
    {
      double r = rad_dist(mt_);
      double angle = distribution(mt_);

      double x = center.x() + r * cos(angle);
      double y = center.y() + r * sin(angle);

      points.emplace_back(x, y);
    }
  }
  //  auto res = executeOnce(points);
  //  ROS_INFO_STREAM("Barrel: " << res.barrel);
  //  for (const auto& outlier : res.outliers)
  //  {
  //    ROS_INFO_STREAM("Outliers: " << outlier);
  //  }

  pcl::PointCloud<pcl::PointXYZ> scan;
  scan.points.reserve(num_points);
  for (const auto& vector : points)
  {
    pcl::PointXYZ p{ static_cast<float>(vector(0)), static_cast<float>(vector(1)), 0.0 };
    scan.points.emplace_back(p);
  }

  auto before = ros::Time::now();
  auto result = execute(scan);
  auto after = ros::Time::now();
  ROS_INFO_STREAM("Time taken: " << (after - before).toSec() << " seconds.");

  for (const auto& barrel : result)
  {
    ROS_INFO_STREAM(barrel);
  }
}

BarrelRansac::BarrelRansac(const Options& options) : options_{ options }, mt_(std::random_device()())
{
  auto block_solver = std::make_unique<BlockSolver>(std::make_unique<LinearSolver>());
  auto* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
  optimizer_.setAlgorithm(solver);
}

std::vector<Barrel> BarrelRansac::execute(const pcl::PointCloud<pcl::PointXYZ>& scan)
{
  std::vector<Eigen::Vector2d> eigen_scan;
  std::transform(scan.begin(), scan.end(), std::back_inserter(eigen_scan), [](const pcl::PointXYZ& point) {
    return Eigen::Vector2d{ point.x, point.y };
  });

  return executeSequentially(std::move(eigen_scan));
}

std::vector<Barrel> BarrelRansac::executeSequentially(std::vector<Eigen::Vector2d> scan)
{
  std::vector<Barrel> barrels;
  barrels.reserve(options_.max_barrels);

  while (static_cast<int>(barrels.size()) < options_.max_barrels &&
         static_cast<int>(scan.size()) > options_.min_scan_points)
  {
    //    ROS_INFO_STREAM("Starting loop. scan.size() == " << scan.size());
    auto result = executeOnce(scan);

//    ROS_INFO_STREAM("result.error: " << result.error);
    if (result.error > options_.max_error)
    {
      ROS_INFO_STREAM("Breaking due to error");
      break;
    }

    barrels.emplace_back(result.barrel);
    scan = std::move(result.outliers);
  }
  //  ROS_INFO_STREAM("barrels.size(): " << barrels.size() << ", scan.size(): " << scan.size());
  return barrels;
}

BarrelRansac::RANSACResult BarrelRansac::executeOnce(const std::vector<Eigen::Vector2d>& scan)
{
  //  ROS_INFO_STREAM("Starting <fn executeOnce>");
  std::vector<Eigen::Vector2d> copy = scan;
  int num_points = scan.size();

  Barrel best_barrel;
  std::vector<Eigen::Vector2d> best_outliers;
  double best_err = std::numeric_limits<double>::max();

  for (int i = 0; i < options_.iterations; i++)
  {
    std::vector<Eigen::Vector2d> inliers;
    inliers.reserve(scan.size());
    std::vector<Eigen::Vector2d> outliers;
    outliers.reserve(scan.size());

    std::shuffle(copy.begin(), copy.end(), mt_);
    inliers.emplace_back(copy[0]);
    inliers.emplace_back(copy[1]);
    inliers.emplace_back(copy[2]);

    Barrel barrel = fitToBarrel(copy[0], copy[1], copy[2]);

    // Found barrel must have radius between min and max radius
    if (barrel(2) < options_.min_barrel_radius || barrel(2) > options_.max_barrel_radius)
    {
      continue;
    }

    // Points must be in forward facing half of the circle
    if (!inForwardHalf(barrel, copy[0], copy[1], copy[2]))
    {
      continue;
    }

    for (int j = 3; j < static_cast<int>(copy.size()); j++)
    {
      if (residual(barrel, copy[j]) < options_.threshold)
      {
        inliers.emplace_back(copy[j]);
      }
      else
      {
        outliers.emplace_back(copy[j]);
      }
    }

    //    barrel = fitToBarrel(barrel, inliers);
    double error = residual(barrel, inliers);
    if (error < best_err)
    {
      best_barrel = barrel;
      best_outliers = std::move(outliers);
      best_err = error;
    }
  }

  //  ROS_INFO_STREAM("done with <fn executeOnce>");
//  ROS_INFO_STREAM("Scan size: " << scan.size() << ", inliers: " << scan.size() - best_outliers.size());
  return { best_barrel, std::move(best_outliers), best_err };
}

Barrel BarrelRansac::fitToBarrel(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const Eigen::Vector2d& p3) const
{
  auto x1 = p1(0);
  auto y1 = p1(1);
  auto x2 = p2(0);
  auto y2 = p2(1);
  auto x3 = p3(0);
  auto y3 = p3(1);

  auto a = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2;

  auto b = (x1 * x1 + y1 * y1) * (y3 - y2) + (x2 * x2 + y2 * y2) * (y1 - y3) + (x3 * x3 + y3 * y3) * (y2 - y1);

  auto c = (x1 * x1 + y1 * y1) * (x2 - x3) + (x2 * x2 + y2 * y2) * (x3 - x1) + (x3 * x3 + y3 * y3) * (x1 - x2);

  auto x = -b / (2 * a);
  auto y = -c / (2 * a);
  auto r = std::hypot(x - x1, y - y1);

  return { x, y, r };
}

Barrel BarrelRansac::fitToBarrel(const Barrel& estimate, const std::vector<Eigen::Vector2d>& inliers)
{
  auto* circle = new VertexCircle();
  circle->setId(0);
  circle->setEstimate(estimate);
  optimizer_.addVertex(circle);

  for (const auto& inlier : inliers)
  {
    auto* edge = new EdgePointOnCircle;
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    edge->setVertex(0, circle);
    edge->setMeasurement(inlier);
    optimizer_.addEdge(edge);
  }

  optimizer_.initializeOptimization();
  optimizer_.optimize(100);

  Barrel output = circle->estimate();
  optimizer_.clear();

  return output;
}

double BarrelRansac::residual(const Barrel& barrel, const Eigen::Vector2d& point) const
{
  double d = (barrel.head<2>() - point).norm() - barrel(2);
  return d * d;
}

double BarrelRansac::residual(const Barrel& barrel, const std::vector<Eigen::Vector2d>& points) const
{
  double acc = 0.0;
  for (const auto& point : points)
  {
    acc += residual(barrel, point);
  }
  return acc / points.size();
}

bool BarrelRansac::inForwardHalf(const Barrel& barrel, const Eigen::Vector2d& p1, const Eigen::Vector2d& p2,
                                 const Eigen::Vector2d& p3) const
{
  double barrel_theta = atan2(barrel(1), barrel(0));

  for (const auto& p : std::vector<Eigen::Vector2d>{ p1, p2, p3 })
  {
    double theta = atan2(p(1) - barrel(1), p(0) - barrel(0));
    double rotated_theta = BarrelRansac::normalizeAngle(theta - barrel_theta - (M_PI / 2));

    if (rotated_theta < 0)
    {
      return false;
    }
  }

  return true;
}

double BarrelRansac::normalizeAngle(double angle)
{
  return -M_PI + fmod(2 * M_PI + fmod(angle + M_PI, 2 * M_PI), 2 * M_PI);
}


/*!
 * @file loudness_test.cpp
 *
 * @author Andrzej Ciarkowski <mailto:andrzej.ciarkowski@gmail.com>
 */

#include <boost/test/auto_unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <dsp++/snd/loudness.h>
#include <dsp++/snd/reader.h>
#include <dsp++/float.h>
#include <fstream>

static void test_loudness_file(const char* path, float exp_level, float peak)
{
	using namespace dsp::snd;
	reader r;
	r.open(path);

	std::string dump(path);
	dump += ".raw";
	std::ofstream d(dump, std::ios_base::binary | std::ios_base::out);

	loudness_ebu<float> met(r.sample_rate(), r.channel_count());
	loudness_peak<float> metp(static_cast<unsigned>(r.sample_rate() * .1 +.5), 5);

	std::vector<float> buf;
	const size_t len = 9600;
	buf.resize(r.channel_count() * len);
	float vm = 0, vs = 0, vi = 0, vp = 0;
	while (true) {
		float* x = &buf[0];
		size_t read = r.read_frames(x, len);

		for (size_t i = 0; i < read; ++i, x += r.channel_count()) {
			if (met.next_frame(x)) {
				vm = met.value_m();
				vs = met.value_s();
				vi = met.value_i();
				d.write((char*)(&vm), 4);
				d.write((char*)(&vs), 4);
				d.write((char*)(&vi), 4);
			}
			metp(*x);
		}
		if (read != len)
			break;
	}

	vi = met.value_i();
	vp = metp.value_db();
	BOOST_CHECK(dsp::within_range<float>(.1f)(vi,exp_level));
	BOOST_CHECK(dsp::within_range<float>(.1f)(vp,peak));
}

BOOST_AUTO_TEST_SUITE(loudness)

BOOST_AUTO_TEST_CASE(ebu1)
{
	test_loudness_file("data/coil.wav", -11.6f, -.3f);
	test_loudness_file("data/ebu_testcase1_-23dBFS.wav", -23.f, -22.8f);
	test_loudness_file("data/ebu_testcase2_-33dBFS.wav", -33.f, -32.8f);
	test_loudness_file("data/ebu_testcase5_-23dBFS.wav", -23.f, -19.8f);
}

BOOST_AUTO_TEST_CASE(peak)
{
}

BOOST_AUTO_TEST_SUITE_END()