/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(STATE_FREQ_MOVE_HPP)
#define STATE_FREQ_MOVE_HPP

#include <vector>                           // for std::vector
#include <boost/shared_ptr.hpp>             // for boost::shared_ptr
#include <boost/weak_ptr.hpp>               // for boost::weak_ptr
#include "mcmc_updater.hpp"		// for base class MCMCUpdater
#include "partition_model.hpp"   // for PartitionModelShPtr definition
#include "dirichlet_move.hpp"
#include "multivariate_probability_distribution.hpp"

namespace phycas
{

class MCMCChainManager;
typedef boost::weak_ptr<MCMCChainManager>			ChainManagerWkPtr;

typedef boost::shared_ptr<DirichletDistribution>    DirichletShPtr;

/*----------------------------------------------------------------------------------------------------------------------
|	A StateFreqMove proposes new state frequencies that are slightly different than the current frequencies by sampling
|   from a Dirichlet distribution with parameters equal to the current frequencies multiplied by a large value (the
|   tuning parameter 'psi').
*/
class StateFreqMove : public DirichletMove
	{
	public:
									StateFreqMove();
                                    virtual ~StateFreqMove() {}

		virtual void				sendCurrValuesToModel(const double_vect_t & v);
		virtual void				getCurrValuesFromModel(double_vect_t & v) const;
		virtual double_vect_t		listCurrValuesFromModel();
        virtual void                getParams();
        virtual void                setParams(const std::vector<double> & v);

	private:

		StateFreqMove &				operator=(const StateFreqMove &);	// never use - don't define
    };

} // namespace phycas

#endif


/*
 Copyright (c) 2015-2017 Paul Lagrée, Siyu Lei, Silviu Maniu, Luyi Mo

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

#include "common.hpp"
#include "InfluenceDistribution.hpp"

#include <boost/math/distributions.hpp>
#include <random>
#include <sys/time.h>
#include <math.h>

class BetaInfluence: public InfluenceDistribution {
 private:
  double alpha_prior_, beta_prior_;
  double alpha_, beta_;
  double quartile_med_;
  double quartile_upper_;
  double quartile_stdev_;
  double original_mean_;
  std::default_random_engine gen_;

 public:
  BetaInfluence(double alpha, double beta, double orig)
      : alpha_prior_(alpha), beta_prior_(beta), alpha_(alpha), beta_(beta),
        original_mean_(orig), gen_(seed_ns()) {
    update_quartiles();
  }

  void update(unode_int hit, unode_int miss) {
    alpha_ += (double)hit;
    beta_ += (double)miss;
    hits_ += hit;
    misses_ += miss;
    update_quartiles();
  };

  void update_prior(double new_alpha, double new_beta) {
    alpha_prior_ = (new_alpha) > 0 ? new_alpha : 1.0;
    beta_prior_ = (new_beta) > 0 ? new_beta : 1.0;
    alpha_ = alpha_prior_ + (double)hits_;
    beta_ = beta_prior_ + (double)misses_;
    update_quartiles();
  }

  double mean() { return (double)alpha_ / (double)(alpha_ + beta_); }

  double sample(unsigned int interval) {
    if (interval == INFLUENCE_MED) {
      return quartile_med_;
    } else if (interval == INFLUENCE_UPPER) {
      return quartile_upper_;
    } else if(interval == INFLUENCE_UCB) {
      double val = quartile_med_ + sqrt(3.0 * log(round_) /
          (2.0 * (alpha_ + beta_)));
      return (val < 1) ? val : 1.0;
    } else if (interval == INFLUENCE_THOMPSON) {
      std::gamma_distribution<double> a(alpha_, 1.0);
      std::gamma_distribution<double> b(beta_, 1.0);
      double x = a(gen_);
      double y = b(gen_);
      return x / (x + y);
    } else { // Case where we shift the distributions by theta stdev (EG)
      double val = quartile_med_ + (interval - (double)THETA_OFFSET - 1.0)
          * quartile_stdev_;
      val = val < 1 ? val : 1.0;
      return val > 0 ? val : 0.0;
    }
    return quartile_med_;
  }

  double sq_error() {
    return (quartile_med_ - original_mean_) * (quartile_med_ - original_mean_);
  }

 private:
  void update_quartiles() {
    boost::math::beta_distribution<> dist(alpha_, beta_);
    quartile_med_ = alpha_ / (alpha_ + beta_);
    quartile_stdev_ = sqrt(alpha_ * beta_ / (alpha_ + beta_ + 1.0))
        / (alpha_ + beta_);
    quartile_upper_ = quantile(dist, 0.75);
  }
};

#endif /* defined(__oim__BetaInfluence__) */


//==================================================================================================
/*!
  @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_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_RSQRT_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object returns the inverse of the square root of the input.

    @par Header <boost/simd/function/rsqrt.hpp>

    Using `rsqrt(x)` is similar to `One(as(x))/sqrt(x)`

    @par Decorators

    - raw_  if full accuracy is not needed gives access on some architectures to faster
    but less accurate version of the function.

    @see sqrt

    @par Example:

      @snippet rsqrt.cpp rsqrt

    @par Possible output:

      @snippet rsqrt.txt rsqrt

  **/
  IEEEValue rsqrt(IEEEValue const& x);
} }
#endif

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

#endif


//---------------------------------------------------------------------------
/// \file   parser.hpp
/// \brief  Construct a constraint from a string
//
// Copyright 2012-2014, nocte@hippie.nu       Released under the MIT License.
//---------------------------------------------------------------------------

#include <unordered_map>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

#include "constraint.hpp"
#include "linear_equation.hpp"
#include "linear_inequality.hpp"

namespace rhea
{

typedef boost::spirit::qi::symbols<char, variable> var_map;

template <typename Iterator>
struct constraint_grammar : boost::spirit::qi::grammar<Iterator, constraint()>
{
    typedef boost::spirit::ascii::space_type space_type;

    constraint_grammar(var_map& vars)
        : constraint_grammar::base_type{constr}
    {
        using namespace boost::spirit;
        namespace phx = boost::phoenix;
        using qi::double_;
        using qi::_val;

        constr = lineq[_val = phx::construct<constraint>(_1)]
                 | linineq[_val = phx::construct<constraint>(_1)];

        lineq = (expr >> '='
                 >> expr)[_val = phx::construct<linear_equation>(_1, _2)];

        linineq
            = (expr >> "<=" >> expr)[_val = phx::construct<linear_inequality>(
                                         _1, relation::leq, _2)]
              | (expr >> ">="
                 >> expr)[_val = phx::construct<linear_inequality>(
                              _1, relation::geq, _2)];

        expr = double_[_val = _1]
               //| lexeme[raw[(ascii::alpha >> *(ascii::alnum | '_'))]] [
               // vars.add, _val = vars[_1] ]
               | vars[_val = _1] | (expr >> '+' >> expr)[_val = _1 + _2]
               | (expr >> '-' >> expr)[_val = _1 - _2]
               | (expr >> '*' >> expr)[_val = _1 * _2]
               | (expr >> '/' >> expr)[_val = _1 / _2];

        constr.name("constraint");
        lineq.name("linear equation");
        linineq.name("linear inequality");
        expr.name("expression");

        qi::on_error<qi::fail>(
            constr, std::cout << phx::val("Error! Expecting ")
                              << _4 // what failed?
                              << phx::val(" here: \"")
                              << phx::construct<std::string>(
                                     _3, _2) // iterators to error-pos, end
                              << phx::val("\"") << std::endl);
    }

    boost::spirit::qi::rule<Iterator, constraint()> constr;
    boost::spirit::qi::rule<Iterator, linear_equation()> lineq;
    boost::spirit::qi::rule<Iterator, linear_inequality()> linineq;
    boost::spirit::qi::rule<Iterator, linear_expression()> expr;
};
}


#ifndef PA_MATH_TENSOR_FIELD_HPP
#define PA_MATH_TENSOR_FIELD_HPP

#include <boost/multi_array.hpp>

#include <pa/math/types.hpp>
#include <pa/export.hpp>

namespace pa
{
struct PA_EXPORT tensor_field
{
  boost::multi_array<matrix3, 3> data    {};
  vector3                        offset  {};
  vector3                        size    {};
  vector3                        spacing {};
};
}

#endif


/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2012, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Ryan Luna */

#include <omplapp/apps/SE2MultiRigidBodyPlanning.h>
#include <omplapp/config.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/base/spaces/TimeStateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include <limits>
#include <boost/math/constants/constants.hpp>

namespace ob = ompl::base;
namespace oc = ompl::control;
using namespace std;

void propagate(const oc::SpaceInformation *si, const ob::State *state,
    const oc::Control* control, const double duration, ob::State *result)
{
    static double timeStep = .01;
    int nsteps = ceil(duration / timeStep);
    double dt = duration / nsteps;
    const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;

    ob::CompoundStateSpace::StateType& s = *result->as<ob::CompoundStateSpace::StateType>();
    ob::SE2StateSpace::StateType& se2 = *s.as<ob::SE2StateSpace::StateType>(0);
    ob::RealVectorStateSpace::StateType& velocity = *s.as<ob::RealVectorStateSpace::StateType>(1);
    // ob::DiscreteStateSpace::StateType& gear = *s.as<ob::DiscreteStateSpace::StateType>(2);
    ob::TimeStateSpace::StateType& timeSpace = *s.as<ob::TimeStateSpace::StateType>(2);

    si->getStateSpace()->copyState(result, state);
    for(int i = 0; i < nsteps; i++)
    {
        se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw()));
        se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw()));
        se2.setYaw(se2.getYaw() + dt * u[0]);
        // velocity.values[0] = velocity.values[0] + dt * (u[1]*gear.value);
        velocity.values[0] = velocity.values[0] + dt * u[1];
        timeSpace.position = timeSpace.position + duration;

        // 'guards' - conditions to change gears
        // if (gear.value > 0)
        // {
        //     if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
        //         gear.value++;
        //     else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
        //         gear.value--;
        // }

        if (!si->satisfiesBounds(result))
            return;
    }
}

bool canPropagateBackward ()
{
    return false;
}

using namespace ompl;

int main()
{
    // plan for two bodies in SE2
    app::SE2MultiRigidBodyPlanning setup(2);

    // load the robot and the environment
    std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
    std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/Maze_planar_env.dae";
    setup.setRobotMesh(robot_fname.c_str());    // The first mesh should use setRobotMesh.
    setup.addRobotMesh(robot_fname.c_str());    // Subsequent robot meshes MUST use addRobotMesh!
    setup.setEnvironmentMesh(env_fname.c_str());

    // constructing start and goal states
    base::ScopedState<base::CompoundStateSpace> start(setup.getSpaceInformation());
    base::ScopedState<base::CompoundStateSpace> goal(setup.getSpaceInformation());

    // define starting state for robot 1
    base::SE2StateSpace::StateType* start1 = start.get()->as<base::SE2StateSpace::StateType>(0);
    start1->setXY(0., 0.);
    start1->setYaw(0.);
    // define goal state for robot 1
    base::SE2StateSpace::StateType* goal1 = goal.get()->as<base::SE2StateSpace::StateType>(0);
    goal1->setXY(26., 0.);
    goal1->setYaw(0.);

    // define starting state for robot 2
    base::SE2StateSpace::StateType* start2 = start.get()->as<base::SE2StateSpace::StateType>(1);
    start2->setXY(26., 0.);
    start2->setYaw(0.);
    // define goal state for robot 2
    base::SE2StateSpace::StateType* goal2 = goal.get()->as<base::SE2StateSpace::StateType>(1);
    goal2->setXY(-30., 0.);
    goal2->setYaw(0.);

    // set the start & goal states
    setup.setStartAndGoalStates(start, goal);

    // use RRTConnect for planning
    setup.setPlanner (base::PlannerPtr(new geometric::RRTConnect(setup.getSpaceInformation())));
    setup.setStatePropagator(boost::bind(&propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));

    setup.setup();
    setup.print(std::cout);
    // attempt to solve the problem, and print it to screen if a solution is found
    if (setup.solve(60))
    {
        setup.simplifySolution();
        setup.getSolutionPath().printAsMatrix(std::cout);
    }

    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <vector>

namespace polyfem
{

///
/// Clip a polygon by a half-plane.
/// https://github.com/alicevision/geogram/blob/cfbc0a5827d71d59f8bcf0369cc1731ef12f82ef/src/examples/graphics/demo_Delaunay2d/main.cpp#L677
///
/// @param[in]  P       { Input polygon }
/// @param[in]  q1      { First endpoint of the clipping line }
/// @param[in]  q2      { Second endpoint of the clipping line }
/// @param[out] result  { Clipped polygon }
///
void clip_polygon_by_half_plane(const Eigen::MatrixXd &P, const Eigen::RowVector2d &q1,
	const Eigen::RowVector2d &q2, Eigen::MatrixXd &result);

///
/// Determine the kernel of the given polygon.
///
/// @param[in]  IV    { #IV x (2|3) vertex positions around the input polygon }
/// @param[out] OV    { #OV x (2|3) vertex positions around the output polygon }
///
void compute_visibility_kernel(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV);

///
/// Determine whether a polygon is star-shaped or not.
///
/// @param[in]  IV    { #IV x (2|3) of vertex positions around the polygon }
/// @param[out] bary  { The barycenter of the kernel }
///
/// @return     True if star shaped, False otherwise.
///
bool is_star_shaped(const Eigen::MatrixXd &IV, Eigen::RowVector3d &bary);

///
/// Compute offset polygon
///
/// @param[in]  IV    { #IV x 2 of vertex positions for the input polygon }
/// @param[out] OV    { #OV x 2 of vertex positions for the offset polygon }
/// @param[in]  eps   { Offset distance }
///
void offset_polygon(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, double eps);

///
/// Compute whether points are inside a polygon
///
/// @param[in]  IV      { #IV x 2 of vertex positions for the input polygon }
/// @param[in]  Q       { #Q x 2 of query point positions }
/// @param[out] inside  { Whether the i-th query point is inside or not }
///
/// @return     Number of points inside
///
int is_inside(const Eigen::MatrixXd &IV, const Eigen::MatrixXd &Q, std::vector<bool> &inside);

///
/// Sample points on a polygon, evenly spaced from each other
///
/// @param[in]  IV           { #IV x 2 vertex positions for the input polygon }
/// @param[in]  num_samples  { Desired number of samples }
/// @param[out] S            { #S x 2 output sample positions }
///
void sample_polygon(const Eigen::MatrixXd &IV, int num_samples, Eigen::MatrixXd &S);

} // namespace polyfem


//==================================================================================================
/*!
  @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_CONSTANT_BITINCREMENT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_BITINCREMENT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generates a value of the chosen type which represents the minimal increment value for @c T.

    @par Header <boost/simd/constant/bitincrement.hpp>

    @par Semantic:

    For any type @c T,

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

    generates a value so that, for any value @c x of type @c T,

    @code
    x + r == simd::nextafter(x, 1);
    @endcode

    evaluates to @c true.

    @return A value of type @c T containing the minimal increment value for @c T

    @see functional::bitincrement
  **/
  template<typename T> T Bitincrement();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generates a value of the chosen type which represents the minimal increment value for @c T.

      @par Semantic:

      For any value @c x of type @c T:
      @code
      T r = simd::functional::bitincrement( boost::simd::as(x));
      @endcode

      is similar to:

      @code
      T r = simd::Bitincrement<T>();
      @endcode

      @return A value of type @c T containing the minimal increment value for @c T

      @see Bitincrement
    **/
    Value Bitincrement();
  }
} }
#endif

#include <boost/simd/constant/scalar/bitincrement.hpp>
#include <boost/simd/constant/simd/bitincrement.hpp>

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_range"

#include <boost/test/unit_test.hpp>
#include "nanocv/math/range.hpp"
#include "nanocv/math/random.hpp"

namespace test
{
        using namespace ncv;

        void check_range(double min, double max, size_t tests)
        {
                range_t<double> range(min, max);

                BOOST_CHECK_LT(min, max);
                BOOST_CHECK_EQUAL(range.min(), min);
                BOOST_CHECK_EQUAL(range.max(), max);

                // check random values
                for (size_t i = 0; i < tests; i ++)
                {
                        random_t<double> rgen(min - 0.1, max + 0.56);

                        const auto val = rgen();
                        if (val < min)
                        {
                                BOOST_CHECK_EQUAL(range.clamp(val), min);
                        }
                        else if (val > max)
                        {
                                BOOST_CHECK_EQUAL(range.clamp(val), max);
                        }
                        else
                        {
                                BOOST_CHECK_EQUAL(range.clamp(val), val);
                        }
                }
        }
}

BOOST_AUTO_TEST_CASE(test_range)
{
        test::check_range(-0.03, 0.005, 32);
        test::check_range(1.03, 13.005, 37);
        test::check_range(-0.54, 0.105, 13);
        test::check_range(-7.03, 10.005, 11);
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_INERTIAMATRIX_HPP
#define RW_MATH_INERTIAMATRIX_HPP

/**
 * @file InertiaMatrix.hpp
 */

#if !defined(SWIG)
#include "Rotation3D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /* @{*/

    /**
     * @brief A 3x3 inertia matrix
     */
    template< class T = double > class InertiaMatrix
    {
      public:
        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< T, 3, 3 > Base;
#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r13 \f$ r_{13} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         * @param r23 \f$ r_{23} \f$
         * @param r31 \f$ r_{31} \f$
         * @param r32 \f$ r_{32} \f$
         * @param r33 \f$ r_{33} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{ccc}
         *  r_{11} & r_{12} & r_{13} \\
         *  r_{21} & r_{22} & r_{23} \\
         *  r_{31} & r_{32} & r_{33}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        InertiaMatrix (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32, T r33)
        {
            _matrix (0, 0) = r11;
            _matrix (0, 1) = r12;
            _matrix (0, 2) = r13;
            _matrix (1, 0) = r21;
            _matrix (1, 1) = r22;
            _matrix (1, 2) = r23;
            _matrix (2, 0) = r31;
            _matrix (2, 1) = r32;
            _matrix (2, 2) = r33;
        }
#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{ccc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}} & \robabx{a}{b}{\mathbf{k}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         * @param k @f$ \robabx{a}{b}{\mathbf{k}} @f$
         */

#endif
        InertiaMatrix (const rw::math::Vector3D< T >& i, const rw::math::Vector3D< T >& j,
                       const rw::math::Vector3D< T >& k)
        {
            _matrix (0, 0) = i[0];
            _matrix (0, 1) = j[0];
            _matrix (0, 2) = k[0];
            _matrix (1, 0) = i[1];
            _matrix (1, 1) = j[1];
            _matrix (1, 2) = k[1];
            _matrix (2, 0) = i[2];
            _matrix (2, 1) = j[2];
            _matrix (2, 2) = k[2];
        }

        /**
         * @brief constructor - where only the diagonal is set
         * @param i [in] m(0,0)
         * @param j [in] m(1,1)
         * @param k [in] m(2,2)
         */
        InertiaMatrix (T i = 0.0, T j = 0.0, T k = 0.0)
        {
            _matrix (0, 0) = i;
            _matrix (0, 1) = 0;
            _matrix (0, 2) = 0;
            _matrix (1, 0) = 0;
            _matrix (1, 1) = j;
            _matrix (1, 2) = 0;
            _matrix (2, 0) = 0;
            _matrix (2, 1) = 0;
            _matrix (2, 2) = k;
        }

        /**
         * @brief Construct an internal matrix from a Eigen::MatrixBase
         * It is the responsibility of the user that 3x3 matrix is indeed an
         * inertia matrix.
         */
        explicit InertiaMatrix (const Base& r) : _matrix (r) {}

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        T& operator() (size_t row, size_t column) { return _matrix (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const T& operator() (size_t row, size_t column) const { return _matrix (row, column); }
#if defined(SWIG)
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Returns reference to the internal 3x3 matrix
         */
        const Base& e () const { return _matrix; }

        /**
         * @brief Returns reference to the internal 3x3 matrix
         */
        Base& e () { return _matrix; }

#if !defined(SWIG)
        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        friend InertiaMatrix operator* (const rw::math::Rotation3D< T >& aRb,
                                        const InertiaMatrix& bRc)
        {
            return InertiaMatrix (aRb.e () * bRc.e ());
        }
#endif
#if !defined(SWIGJAVA)

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
#endif
        InertiaMatrix operator* (const rw::math::Rotation3D< T >& bRc) const
        {
            return InertiaMatrix (this->e () * bRc.e ());
        }

        /**
         * @brief Calculates the addition between the two InertiaMatrices
         */
        InertiaMatrix operator+ (const InertiaMatrix& I2) const
        {
            return InertiaMatrix (this->e () + I2.e ());
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        rw::math::Vector3D< T > operator* (const rw::math::Vector3D< T >& bVc) const
        {
            return rw::math::Vector3D< T > (this->e () * bVc.e ());
        }
#if !defined(SWIG)
        /**
         * @brief Writes rotation matrix to stream
         * @param os [in/out] output stream to use
         * @param r [in] rotation matrix to print
         * @return the updated output stream
         */
        friend std::ostream& operator<< (std::ostream& os, const InertiaMatrix& r)
        {
            return os << r.e ();
        }
#else
        TOSTRING (rw::math::InertiaMatrix< T >);
#endif

        /**
         * @brief Make inertia matrix for a solid sphere.
         * @param mass [in] mass of solid sphere.
         * @param radi [in] radius of sphere.
         * @return the inertia matrix.
         */
        static InertiaMatrix< T > makeSolidSphereInertia (T mass, T radi)
        {
            T tmpV = (T) (2.0 / 5.0) * mass * radi * radi;
            return InertiaMatrix< T > (tmpV, 0, 0, 0, tmpV, 0, 0, 0, tmpV);
        }

        /**
         * @brief Make inertia matrix for a hollow sphere.
         * @param mass [in] mass of hollow sphere.
         * @param radi [in] radius of sphere.
         * @return the inertia matrix.
         */
        static InertiaMatrix< T > makeHollowSphereInertia (T mass, T radi)
        {
            T tmpV = (T) (2.0 / 3.0) * mass * radi * radi;
            return InertiaMatrix< T > (tmpV, 0, 0, 0, tmpV, 0, 0, 0, tmpV);
        }

        /**
         * @brief calculates the inertia of a cuboid where the reference frame is in the
         * center of the cuboid with
         * @param mass
         * @param x
         * @param y
         * @param z
         * @return
         */
        static InertiaMatrix< T > makeCuboidInertia (T mass, T x, T y, T z)
        {
            return InertiaMatrix< T > ((T) (1 / 12.0 * mass * (y * y + z * z)),
                                       0,
                                       0,
                                       0,
                                       (T) (1 / 12.0 * mass * (x * x + z * z)),
                                       0,
                                       0,
                                       0,
                                       (T) (1 / 12.0 * mass * (x * x + y * y)));
        }

      private:
        Base _matrix;
    };
#if !defined(SWIGJAVA)

    /**
     * @brief Calculates the inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */

    #endif
    template< class Q > InertiaMatrix< Q > inverse (const InertiaMatrix< Q >& aRb)
    {
        return InertiaMatrix< Q > (aRb.e ().inverse ());
    }

    /**
     * @brief Casts InertiaMatrix<T> to InertiaMatrix<Q>
     * @param rot [in] InertiaMatrix with type T
     * @return InertiaMatrix with type Q
     */
    template< class Q, class T > InertiaMatrix< Q > cast (const InertiaMatrix< T >& rot)
    {
        InertiaMatrix< Q > res;
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
                res (i, j) = static_cast< Q > (rot (i, j));
        return res;
    }
#if !defined(SWIG)
    extern template class rw::math::InertiaMatrix< double >;
    extern template class rw::math::InertiaMatrix< float >;
#else
    SWIG_DECLARE_TEMPLATE (InertiaMatrixd, rw::math::InertiaMatrix< double >);
    SWIG_DECLARE_TEMPLATE (InertiaMatrixf, rw::math::InertiaMatrix< float >);
#endif

    using InertiaMatrixd = InertiaMatrix< double >;
    using InertiaMatrixf = InertiaMatrix< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void write (const rw::math::InertiaMatrix< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void write (const rw::math::InertiaMatrix< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void read (rw::math::InertiaMatrix< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void read (rw::math::InertiaMatrix< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


// Boost.Polygon library voronoi_predicates_test.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#include <limits>
#include <map>

#define BOOST_TEST_MODULE voronoi_predicates_test
#include <boost/test/test_case_template.hpp>

#include <boost/polygon/detail/voronoi_ctypes.hpp>
#include <boost/polygon/detail/voronoi_predicates.hpp>
#include <boost/polygon/detail/voronoi_structures.hpp>
using namespace boost::polygon::detail;

#include <boost/polygon/voronoi_geometry_type.hpp>
using namespace boost::polygon;

ulp_comparison<double> ulp_cmp;

typedef voronoi_predicates< voronoi_ctype_traits<int> > VP;
typedef point_2d<int> point_type;
typedef site_event<int> site_type;
typedef circle_event<double> circle_type;
VP::event_comparison_predicate<site_type, circle_type> event_comparison;

typedef beach_line_node_key<site_type> key_type;
typedef VP::distance_predicate<site_type> distance_predicate_type;
typedef VP::node_comparison_predicate<key_type> node_comparison_type;
typedef std::map<key_type, int, node_comparison_type> beach_line_type;
typedef beach_line_type::iterator bieach_line_iterator;
distance_predicate_type distance_predicate;
node_comparison_type node_comparison;

typedef VP::circle_existence_predicate<site_type> CEP_type;
typedef VP::mp_circle_formation_functor<site_type, circle_type> MP_CFF_type;
typedef VP::lazy_circle_formation_functor<site_type, circle_type> lazy_CFF_type;
VP::circle_formation_predicate<site_type, circle_type, CEP_type, MP_CFF_type> mp_predicate;
VP::circle_formation_predicate<site_type, circle_type, CEP_type, lazy_CFF_type> lazy_predicate;

#define CHECK_ORIENTATION(P1, P2, P3, R1, R2) \
    BOOST_CHECK_EQUAL(VP::ot::eval(P1, P2, P3) == R1, true); \
    BOOST_CHECK_EQUAL(VP::ot::eval(P1, P3, P2) == R2, true); \
    BOOST_CHECK_EQUAL(VP::ot::eval(P2, P1, P3) == R2, true); \
    BOOST_CHECK_EQUAL(VP::ot::eval(P2, P3, P1) == R1, true); \
    BOOST_CHECK_EQUAL(VP::ot::eval(P3, P1, P2) == R1, true); \
    BOOST_CHECK_EQUAL(VP::ot::eval(P3, P2, P1) == R2, true)

#define CHECK_EVENT_COMPARISON(A, B, R1, R2) \
    BOOST_CHECK_EQUAL(event_comparison(A, B), R1); \
    BOOST_CHECK_EQUAL(event_comparison(B, A), R2)

#define CHECK_DISTANCE_PREDICATE(S1, S2, S3, RES) \
    BOOST_CHECK_EQUAL(distance_predicate(S1, S2, S3), RES)

#define CHECK_NODE_COMPARISON(node, nodes, res, sz) \
    for (int i = 0; i < sz; ++i) { \
      BOOST_CHECK_EQUAL(node_comparison(node, nodes[i]), res[i]); \
      BOOST_CHECK_EQUAL(node_comparison(nodes[i], node), !res[i]); \
    }

#define CHECK_CIRCLE(circle, c_x, c_y, l_x) \
    BOOST_CHECK_EQUAL(ulp_cmp(c1.x(), c_x, 10), ulp_comparison<double>::EQUAL); \
    BOOST_CHECK_EQUAL(ulp_cmp(c1.y(), c_y, 10), ulp_comparison<double>::EQUAL); \
    BOOST_CHECK_EQUAL(ulp_cmp(c1.lower_x(), l_x, 10), ulp_comparison<double>::EQUAL)

#define CHECK_CIRCLE_EXISTENCE(s1, s2, s3, RES) \
  { circle_type c1; \
    BOOST_CHECK_EQUAL(lazy_predicate(s1, s2, s3, c1), RES); }

#define CHECK_CIRCLE_FORMATION_PREDICATE(s1, s2, s3, c_x, c_y, l_x) \
  { circle_type c1, c2; \
    BOOST_CHECK_EQUAL(mp_predicate(s1, s2, s3, c1), true); \
    BOOST_CHECK_EQUAL(lazy_predicate(s1, s2, s3, c2), true); \
    CHECK_CIRCLE(c1, c_x, c_y, l_x); \
    CHECK_CIRCLE(c2, c_x, c_y, l_x); }

BOOST_AUTO_TEST_CASE(orientation_test) {
  int min_int = (std::numeric_limits<int>::min)();
  int max_int = (std::numeric_limits<int>::max)();
  point_type point1(min_int, min_int);
  point_type point2(0, 0);
  point_type point3(max_int, max_int);
  point_type point4(min_int, max_int);
  point_type point5(max_int-1, max_int);
  CHECK_ORIENTATION(point1, point2, point3, VP::ot::COLLINEAR, VP::ot::COLLINEAR);
  CHECK_ORIENTATION(point1, point4, point3, VP::ot::RIGHT, VP::ot::LEFT);
  CHECK_ORIENTATION(point1, point5, point3, VP::ot::RIGHT, VP::ot::LEFT);
}

BOOST_AUTO_TEST_CASE(event_comparison_test1) {
  site_type site(1, 2);
  CHECK_EVENT_COMPARISON(site, site_type(0, 2), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(1, 3), true, false);
  CHECK_EVENT_COMPARISON(site, site_type(1, 2), false, false);
}

BOOST_AUTO_TEST_CASE(event_comparison_test2) {
  site_type site(0, 0, 0, 2);
  CHECK_EVENT_COMPARISON(site, site_type(0, 2), true, false);
  CHECK_EVENT_COMPARISON(site, site_type(0, 0), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, -2, 0, -1), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, -2, 1, 1), true, false);
  CHECK_EVENT_COMPARISON(site, site_type(0, 0, 1, 1), true, false);
}

BOOST_AUTO_TEST_CASE(event_comparison_test3) {
  site_type site(0, 0, 10, 10);
  CHECK_EVENT_COMPARISON(site, site_type(0, 0), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, -1), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, 1), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, 1, 0, 10), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, -10, 0, -1), false, true);
  CHECK_EVENT_COMPARISON(site, site_type(0, 0, 10, 9), true, false);
  CHECK_EVENT_COMPARISON(site, site_type(0, 0, 9, 10), false, true);
}

BOOST_AUTO_TEST_CASE(event_comparison_test4) {
  circle_type circle(1, 2, 3);
  CHECK_EVENT_COMPARISON(circle, circle_type(1, 2, 3), false, false);
  CHECK_EVENT_COMPARISON(circle, circle_type(1, 3, 3), true, false);
  CHECK_EVENT_COMPARISON(circle, circle_type(1, 2, 4), true, false);
  CHECK_EVENT_COMPARISON(circle, circle_type(0, 2, 2), false, true);
  CHECK_EVENT_COMPARISON(circle, circle_type(-1, 2, 3), false, false);
}

BOOST_AUTO_TEST_CASE(event_comparison_test5) {
  circle_type circle(1, 2, 3);
  CHECK_EVENT_COMPARISON(circle, site_type(0, 100), false, true);
  CHECK_EVENT_COMPARISON(circle, site_type(3, 0), false, true);
  CHECK_EVENT_COMPARISON(circle, site_type(3, 2), false, false);
  CHECK_EVENT_COMPARISON(circle, site_type(3, 3), true, false);
  CHECK_EVENT_COMPARISON(circle, site_type(4, 2), true, false);
}

BOOST_AUTO_TEST_CASE(distance_predicate_test1) {
  site_type site1(-5, 0);
  site_type site2(-8, 9);
  site_type site3(-2, 1);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 5), false);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, 5), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 4), false);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, 4), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 6), true);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, 6), true);
}

BOOST_AUTO_TEST_CASE(distance_predicate_test2) {
  site_type site1(-4, 0, -4, 20);
  site_type site2(-2, 10);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 11), false);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 9), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 11), true);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 9), true);
}

BOOST_AUTO_TEST_CASE(disntace_predicate_test3) {
  site_type site1(-5, 5, 2, -2);
  site1.inverse();
  site_type site2(-2, 4);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, -1), false);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, -1), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 1), false);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 1), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 4), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 4), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 5), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 5), false);
}

BOOST_AUTO_TEST_CASE(distance_predicate_test4) {
  site_type site1(-5, 5, 2, -2);
  site_type site2(-2, -4);
  site_type site3(-4, 1);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, 1), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, 1), true);
  CHECK_DISTANCE_PREDICATE(site1, site3, site_type(0, 1), true);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, 1), true);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, -2), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, -2), false);
  CHECK_DISTANCE_PREDICATE(site1, site3, site_type(0, -2), true);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, -2), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, -8), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, -8), false);
  CHECK_DISTANCE_PREDICATE(site1, site3, site_type(0, -8), true);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, -8), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(0, -9), true);
  CHECK_DISTANCE_PREDICATE(site2, site1, site_type(0, -9), false);
  CHECK_DISTANCE_PREDICATE(site1, site3, site_type(0, -9), true);
  CHECK_DISTANCE_PREDICATE(site3, site1, site_type(0, -9), false);
}

BOOST_AUTO_TEST_CASE(disntace_predicate_test5) {
  site_type site1(-5, 5, 2, -2);
  site_type site2 = site1;
  site2.inverse();
  site_type site3(-2, 4);
  site_type site4(-2, -4);
  site_type site5(-4, 1);
  CHECK_DISTANCE_PREDICATE(site3, site2, site_type(0, 1), false);
  CHECK_DISTANCE_PREDICATE(site3, site2, site_type(0, 4), false);
  CHECK_DISTANCE_PREDICATE(site3, site2, site_type(0, 5), false);
  CHECK_DISTANCE_PREDICATE(site3, site2, site_type(0, 7), true);
  CHECK_DISTANCE_PREDICATE(site4, site1, site_type(0, -2), false);
  CHECK_DISTANCE_PREDICATE(site5, site1, site_type(0, -2), false);
  CHECK_DISTANCE_PREDICATE(site4, site1, site_type(0, -8), false);
  CHECK_DISTANCE_PREDICATE(site5, site1, site_type(0, -8), false);
  CHECK_DISTANCE_PREDICATE(site4, site1, site_type(0, -9), false);
  CHECK_DISTANCE_PREDICATE(site5, site1, site_type(0, -9), false);
  CHECK_DISTANCE_PREDICATE(site4, site1, site_type(0, -18), false);
  CHECK_DISTANCE_PREDICATE(site5, site1, site_type(0, -18), false);
  CHECK_DISTANCE_PREDICATE(site4, site1, site_type(0, -1), true);
  CHECK_DISTANCE_PREDICATE(site5, site1, site_type(0, -1), true);
}

BOOST_AUTO_TEST_CASE(distance_predicate_test6) {
  site_type site1(-5, 0, 2, 7);
  site_type site2 = site1;
  site2.inverse();
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(2, 7), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(1, 5), false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(-1, 5), true);
}

BOOST_AUTO_TEST_CASE(distance_predicate_test7) {
  site_type site1(-5, 5, 2, -2);
  site1.inverse();
  site_type site2(-5, 5, 0, 6);
  site_type site3(-2, 4, 0, 4);
  site_type site4(0, 2);
  site_type site5(0, 5);
  site_type site6(0, 6);
  site_type site7(0, 8);
  CHECK_DISTANCE_PREDICATE(site1, site2, site4, false);
  CHECK_DISTANCE_PREDICATE(site1, site2, site5, true);
  CHECK_DISTANCE_PREDICATE(site1, site2, site6, true);
  CHECK_DISTANCE_PREDICATE(site1, site2, site7, true);
  CHECK_DISTANCE_PREDICATE(site1, site3, site4, false);
  CHECK_DISTANCE_PREDICATE(site1, site3, site5, true);
  CHECK_DISTANCE_PREDICATE(site1, site3, site6, true);
  CHECK_DISTANCE_PREDICATE(site1, site3, site7, true);
  site3.inverse();
  CHECK_DISTANCE_PREDICATE(site3, site1, site4, false);
  CHECK_DISTANCE_PREDICATE(site3, site1, site5, false);
  CHECK_DISTANCE_PREDICATE(site3, site1, site6, false);
  CHECK_DISTANCE_PREDICATE(site3, site1, site7, true);
}

BOOST_AUTO_TEST_CASE(distatnce_predicate_test8) {
  site_type site1(-5, 3, -2, 2);
  site1.inverse();
  site_type site2(-5, 5, -2, 2);
  CHECK_DISTANCE_PREDICATE(site1, site2, site_type(-4, 2), false);
}

BOOST_AUTO_TEST_CASE(node_comparison_test1) {
  beach_line_type beach_line;
  site_type site1(0, 0);
  site1.sorted_index(0);
  site_type site2(0, 2);
  site2.sorted_index(1);
  site_type site3(1, 0);
  site3.sorted_index(2);
  beach_line[key_type(site1, site2)] = 2;
  beach_line[key_type(site1, site3)] = 0;
  beach_line[key_type(site3, site1)] = 1;
  int cur_index = 0;
  for (bieach_line_iterator it = beach_line.begin();
       it != beach_line.end(); ++it, ++cur_index) {
    BOOST_CHECK_EQUAL(it->second, cur_index);
  }
}

BOOST_AUTO_TEST_CASE(node_comparison_test2) {
  beach_line_type beach_line;
  site_type site1(0, 1);
  site1.sorted_index(0);
  site_type site2(2, 0);
  site2.sorted_index(1);
  site_type site3(2, 4);
  site3.sorted_index(2);
  beach_line[key_type(site1, site2)] = 0;
  beach_line[key_type(site2, site1)] = 1;
  beach_line[key_type(site1, site3)] = 2;
  beach_line[key_type(site3, site1)] = 3;
  int cur_index = 0;
  for (bieach_line_iterator it = beach_line.begin();
       it != beach_line.end(); ++it, ++cur_index) {
    BOOST_CHECK_EQUAL(it->second, cur_index);
  }
}

BOOST_AUTO_TEST_CASE(node_comparison_test3) {
  key_type node(site_type(1, 0).sorted_index(1), site_type(0, 2).sorted_index(0));
  key_type nodes[] = {
    key_type(site_type(2, -10).sorted_index(2)),
    key_type(site_type(2, -1).sorted_index(2)),
    key_type(site_type(2, 0).sorted_index(2)),
    key_type(site_type(2, 1).sorted_index(2)),
    key_type(site_type(2, 2).sorted_index(2)),
    key_type(site_type(2, 3).sorted_index(2)),
  };
  bool res[] = {false, false, false, false, true, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 6);
}

BOOST_AUTO_TEST_CASE(node_comparison_test4) {
  key_type node(site_type(0, 1).sorted_index(0), site_type(1, 0).sorted_index(1));
  key_type nodes[] = {
    key_type(site_type(2, -3).sorted_index(2)),
    key_type(site_type(2, -2).sorted_index(2)),
    key_type(site_type(2, -1).sorted_index(2)),
    key_type(site_type(2, 0).sorted_index(2)),
    key_type(site_type(2, 1).sorted_index(2)),
    key_type(site_type(2, 3).sorted_index(2)),
  };
  bool res[] = {false, true, true, true, true, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 6);
}

BOOST_AUTO_TEST_CASE(node_comparison_test5) {
  key_type node(site_type(0, 0).sorted_index(0), site_type(1, 2).sorted_index(1));
  key_type nodes[] = {
    key_type(site_type(2, -10).sorted_index(2)),
    key_type(site_type(2, 0).sorted_index(2)),
    key_type(site_type(2, 1).sorted_index(2)),
    key_type(site_type(2, 2).sorted_index(2)),
    key_type(site_type(2, 5).sorted_index(2)),
    key_type(site_type(2, 20).sorted_index(2)),
  };
  bool res[] = {false, false, true, true, true, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 6);
}

BOOST_AUTO_TEST_CASE(node_comparison_test6) {
  key_type node(site_type(1, 1).sorted_index(1), site_type(0, 0).sorted_index(0));
  key_type nodes[] = {
    key_type(site_type(2, -3).sorted_index(2)),
    key_type(site_type(2, -2).sorted_index(2)),
    key_type(site_type(2, 0).sorted_index(2)),
    key_type(site_type(2, 1).sorted_index(2)),
    key_type(site_type(2, 2).sorted_index(2)),
    key_type(site_type(2, 3).sorted_index(2)),
    key_type(site_type(2, 5).sorted_index(2)),
  };
  bool res[] = {false, false, false, false, false, false, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 7);
}

BOOST_AUTO_TEST_CASE(node_comparison_test7) {
  key_type node(site_type(0, 0).sorted_index(0), site_type(0, 2).sorted_index(1));
  key_type nodes[] = {
    key_type(site_type(1, 0).sorted_index(2)),
    key_type(site_type(1, 1).sorted_index(2)),
    key_type(site_type(1, 2).sorted_index(2)),
  };
  bool res[] = {false, false, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 3);
}

BOOST_AUTO_TEST_CASE(node_comparison_test8) {
  key_type node(site_type(0, 0).sorted_index(0), site_type(1, 1).sorted_index(2));
  key_type nodes[] = {
    key_type(site_type(1, 0).sorted_index(1)),
    key_type(site_type(1, 1).sorted_index(2)),
    key_type(site_type(1, 2).sorted_index(3)),
    key_type(site_type(1, 1).sorted_index(2), site_type(0, 0).sorted_index(0)),
  };
  bool res[] = {false, true, true, true};
  CHECK_NODE_COMPARISON(node, nodes, res, 4);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test1) {
  site_type site1(0, 0);
  site_type site2(-8, 0);
  site_type site3(0, 6);
  CHECK_CIRCLE_FORMATION_PREDICATE(site1, site2, site3, -4.0, 3.0, 1.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test2) {
  int min_int = (std::numeric_limits<int>::min)();
  int max_int = (std::numeric_limits<int>::max)();
  site_type site1(min_int, min_int);
  site_type site2(min_int, max_int);
  site_type site3(max_int-1, max_int-1);
  site_type site4(max_int, max_int);
  CHECK_CIRCLE_EXISTENCE(site1, site2, site4, true);
  CHECK_CIRCLE_EXISTENCE(site1, site3, site4, false);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test3) {
  site_type site1(-4, 0);
  site_type site2(0, 4);
  site_type site3(site1.point0(), site2.point0());
  CHECK_CIRCLE_EXISTENCE(site1, site3, site2, false);
  site_type site4(-2, 0);
  site_type site5(0, 2);
  CHECK_CIRCLE_EXISTENCE(site3, site4, site5, false);
  CHECK_CIRCLE_EXISTENCE(site4, site5, site3, false);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test4) {
  site_type site1(-4, 0, -4, 20);
  site_type site2(-2, 10);
  site_type site3(4, 10);
  CHECK_CIRCLE_FORMATION_PREDICATE(site1, site2, site3, 1.0, 6.0, 6.0);
  CHECK_CIRCLE_FORMATION_PREDICATE(site3, site2, site1, 1.0, 14.0, 6.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test5) {
  site_type site1(1, 0, 7, 0);
  site1.inverse();
  site_type site2(-2, 4, 10, 4);
  site_type site3(6, 2);
  site_type site4(1, 0);
  CHECK_CIRCLE_FORMATION_PREDICATE(site3, site1, site2, 4.0, 2.0, 6.0);
  CHECK_CIRCLE_FORMATION_PREDICATE(site4, site2, site1, 1.0, 2.0, 3.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test6) {
  site_type site1(-1, 2, 8, -10);
  site1.inverse();
  site_type site2(-1, 0, 8, 12);
  site_type site3(1, 1);
  CHECK_CIRCLE_FORMATION_PREDICATE(site3, site2, site1, 6.0, 1.0, 11.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test7) {
  site_type site1(1, 0, 6, 0);
  site1.inverse();
  site_type site2(-6, 4, 0, 12);
  site_type site3(1, 0);
  CHECK_CIRCLE_FORMATION_PREDICATE(site3, site2, site1, 1.0, 5.0, 6.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test8) {
  site_type site1(1, 0, 5, 0);
  site1.inverse();
  site_type site2(0, 12, 8, 6);
  site_type site3(1, 0);
  CHECK_CIRCLE_FORMATION_PREDICATE(site3, site2, site1, 1.0, 5.0, 6.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test9) {
  site_type site1(0, 0, 4, 0);
  site_type site2(0, 0, 0, 4);
  site_type site3(0, 4, 4, 4);
  site1.inverse();
  CHECK_CIRCLE_FORMATION_PREDICATE(site1, site2, site3, 2.0, 2.0, 4.0);
}

BOOST_AUTO_TEST_CASE(circle_formation_predicate_test10) {
  site_type site1(1, 0, 41, 30);
  site_type site2(-39, 30, 1, 60);
  site_type site3(1, 60, 41, 30);
  site1.inverse();
  CHECK_CIRCLE_FORMATION_PREDICATE(site1, site2, site3, 1.0, 30.0, 25.0);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/cgs/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost::units;

int main(int argc, char* argv[])
{
	if (argc < 3)
	{
		std::cout << "Usage: ./units vertical_degrees horizontal_degrees" << std::endl;
		return -1;
	}

    // test quantity_cast
    {
    // implicit value_type conversions
    //[conversion_snippet_1
    quantity<si::length>     L1 = quantity<si::length,int>(int(2.5)*si::meters);
    quantity<si::length,int> L2(quantity<si::length,double>(2.5*si::meters));
    //]

    //[conversion_snippet_3
    quantity<si::length,int> L3 = static_cast<quantity<si::length,int> >(L1);
    //]

    //[conversion_snippet_4
    quantity<cgs::length>    L4 = static_cast<quantity<cgs::length> >(L1);
    //]

    quantity<si::length,int> L5(4*si::meters),
                             L6(5*si::meters);
    quantity<cgs::length>    L7(L1);

    swap(L5,L6);

    std::cout << "L1 = " << L1 << std::endl
              << "L2 = " << L2 << std::endl
              << "L3 = " << L3 << std::endl
              << "L4 = " << L4 << std::endl
              << "L5 = " << L5 << std::endl
              << "L6 = " << L6 << std::endl
              << "L7 = " << L7 << std::endl
              << std::endl;
    }

    // test explicit unit system conversion
    {
    //[conversion_snippet_5
    quantity<si::volume>    vs(1.0*pow<3>(si::meter));
    quantity<cgs::volume>   vc(vs);
    quantity<si::volume>    vs2(vc);

    quantity<si::energy>    es(1.0*si::joule);
    quantity<cgs::energy>   ec(es);
    quantity<si::energy>    es2(ec);

    quantity<si::velocity>  v1 = 2.0*si::meters/si::second,
                            v2(2.0*cgs::centimeters/cgs::second);
    //]

    std::cout << "volume (m^3)  = " << vs << std::endl
              << "volume (cm^3) = " << vc << std::endl
              << "volume (m^3)  = " << vs2 << std::endl
              << std::endl;

    std::cout << "energy (joules) = " << es << std::endl
              << "energy (ergs)   = " << ec << std::endl
              << "energy (joules) = " << es2 << std::endl
              << std::endl;

    std::cout << "velocity (2 m/s)  = " << v1 << std::endl
              << "velocity (2 cm/s) = " << v2 << std::endl
              << std::endl;
    }

	{
		// Unit
		const si::plane_angle plane_angle_unit;

		// SI System Reference
		quantity<si::plane_angle> rad(1.0*si::radians); // 1 rad as plane angle
		quantity<si::plane_angle> degree_as_plane_angle(1.0*degree::degrees); // 1 degree as plane angle

		// Trigonometry and Angle System Reference
		quantity<degree::plane_angle> degree(1.0*degree::degrees);

		std::cout << "unit = " << plane_angle_unit << std::endl; // rad
		std::cout << "1 radian = " << rad << std::endl; // 1 rad
		std::cout << "1 degree as plane angle = " << degree_as_plane_angle << std::endl; // 0.0174533 rad
		std::cout << "1 degree using angle system reference = " << degree << std::endl; // 1 deg

		// Conversion from user input
		std::cout << boost::lexical_cast<double>(argv[1]) << " degrees: "
					<< degree_as_plane_angle*boost::lexical_cast<double>(argv[1])
					<< " "
					<< boost::lexical_cast<double>(argv[2]) << " degrees: "
					<< degree_as_plane_angle*boost::lexical_cast<double>(argv[2])
					<< std::endl;

		// Get default radians values for AP and OS cameras
		std:: cout << "AP" << std::endl;

		std::cout << "IRTV - MFOV"
					<< " vertical: "
					<< degree_as_plane_angle*9.0
					<< " horizontal: "
					<< degree_as_plane_angle*14.0 << std::endl;
		std::cout << "IRTV - NFOV"
					<< " vertical: "
					<< degree_as_plane_angle*3.0
					<< " horizontal: "
					<< degree_as_plane_angle*5.0 << std::endl;
		std::cout << "CTV - WFOV"
					<< " vertical: "
					<< degree_as_plane_angle*17.0
					<< " horizontal: "
					<< degree_as_plane_angle*26.0 << std::endl;
		std::cout << "CTV - MFOV"
					<< " vertical: "
					<< degree_as_plane_angle*4.0
					<< " horizontal: "
					<< degree_as_plane_angle*6.0 << std::endl;
		std::cout << "CTV - NFOV"
					<< " vertical: "
					<< degree_as_plane_angle*2.0
					<< " horizontal: "
					<< degree_as_plane_angle*3.0 << std::endl;

		std::cout << "OS" << std::endl;

		std::cout << "IRTV - MFOV"
					<< " vertical: "
					<< degree_as_plane_angle*9.0
					<< " horizontal: "
					<< degree_as_plane_angle*14.0 << std::endl;
		std::cout << "IRTV - NFOV"
					<< " vertical: "
					<< degree_as_plane_angle*3.0
					<< " horizontal: "
					<< degree_as_plane_angle*5.0 << std::endl;
		std::cout << "IRTV - UNFOV"
					<< " vertical: "
					<< degree_as_plane_angle*2.0
					<< " horizontal: "
					<< degree_as_plane_angle*3.0 << std::endl;
		std::cout << "CHDTV - WFOV"
					<< " vertical: "
					<< degree_as_plane_angle*27.0
					<< " horizontal: "
					<< degree_as_plane_angle*40.0 << std::endl;
		std::cout << "CHDTV - MFOV"
					<< " vertical: "
					<< degree_as_plane_angle*10.0
					<< " horizontal: "
					<< degree_as_plane_angle*15.0 << std::endl;
		std::cout << "CHDTV - NFOV"
					<< " vertical: "
					<< degree_as_plane_angle*3.0
					<< " horizontal: "
					<< degree_as_plane_angle*5.0 << std::endl;
		std::cout << "CHDTV - UNFOV"
					<< " vertical: "
					<< degree_as_plane_angle*2.0
					<< " horizontal: "
					<< degree_as_plane_angle*3.0 << std::endl;
		std::cout << "LLLTV - WFOV"
					<< " vertical: "
					<< degree_as_plane_angle*24.0
					<< " horizontal: "
					<< degree_as_plane_angle*36.0 << std::endl;
		std::cout << "LLLTV - MFOV"
					<< " vertical: "
					<< degree_as_plane_angle*9.0
					<< " horizontal: "
					<< degree_as_plane_angle*14.0 << std::endl;
		std::cout << "LLLTV - NFOV"
					<< " vertical: "
					<< degree_as_plane_angle*3.0
					<< " horizontal: "
					<< degree_as_plane_angle*5.0 << std::endl;
	}

    return 0;
}


//==============================================================================
//         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 <nt2/trigonometric/include/functions/rem_pio2_medium.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/pio_4.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/half.hpp>

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

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>

NT2_TEST_CASE_TPL ( rem_pio2_medium_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::rem_pio2_medium;
  using nt2::tag::rem_pio2_medium_;
  typedef typename nt2::meta::as_integer<T>::type iT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<rem_pio2_medium_(T)>::type)
                  , (std::pair<iT,T>)
                  );

  {
    T r1;
    NT2_TEST_EQUAL( rem_pio2_medium(nt2::Pio_2<T>(), r1), nt2::One<iT>());
    NT2_TEST_ULP_EQUAL( r1, nt2::Zero<T>(), 0.5);
    NT2_TEST_EQUAL( rem_pio2_medium(nt2::Pio_4<T>()*nt2::Half<T>(), r1), nt2::Zero<iT>());
    NT2_TEST_ULP_EQUAL( r1, nt2::Pio_4<T>()*nt2::Half<T>(), 0.5);
  }
}


#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <cstdlib>
#include <cassert>
#include <random>
#include <cmath>
#include "Edec.hpp"
static int  n =2;
static int  N =12;
static int q =11158;
//四舍五入取整数
inline int mod(double num)
{
    int dev=(int)std::round(num/q);
    return num-q*dev;
}
boost::numeric::ublas::matrix<int > FinalS(n+1,1);
inline int RandomNum()
{
    std::random_device rd;

    std::mt19937 rng(rd());

    //在这里取到一个范围在0～2q的随机 数字
    //在取一个矩阵的时候mod q 取四舍五入
    std::uniform_int_distribution<int> uni(0,2*q);

    auto random_integer = uni(rng);

    return random_integer;
}
boost::numeric::ublas::matrix<int > GetRandomMatrixS()
{
    using matrix=boost::numeric::ublas::matrix<int>;

    matrix m1(n+1,1);

    m1(0,0)=1;

    for(unsigned int i=1;i<n+1;++i)
    {
      m1(i,0)=mod(RandomNum());
    }

    return m1;
}
boost::numeric::ublas::matrix<int > GetRandomMatrixA()
{
    using matrix=boost::numeric::ublas::matrix<int>;

    matrix m1(N,n);

    for(unsigned int x=0;x<N;++x)
    {
        for(unsigned int y=0;y<n;++y)
        {
            m1(x,y)=mod(RandomNum());
        }
    }

    return m1;
}

boost::numeric::ublas::matrix<int >  GetMatrixB()
{
    using matrix=boost::numeric::ublas::matrix<int>;
    //先取 e == N χ 。
    matrix e(N,1);

    for(int i=0;i<N;++i)
    {
        e(i,0)=RandomNum()%2;

    }
//     std::cout<<e<<"\n";
    matrix b(N,1);

    auto MatrixA=GetRandomMatrixA();

    auto MatrixS=GetRandomMatrixS();

    FinalS=MatrixS;

    matrix MatrixSp(n,1);

    for(unsigned int i=1;i<=n;++i)
    {
        MatrixSp(i-1,0)=MatrixS(i,0);
    }

    auto MatrixB=boost::numeric::ublas::prod(MatrixA, MatrixSp);

    b=( MatrixB+e);

    matrix MatrixAfinal(N,n+1);

    auto col1=boost::numeric::ublas::column(b, 0);

    MatrixA=-MatrixA;

    for(unsigned int i=0;i<N;++i)
    {
        for(unsigned int j=0;j<n+1;++j)
        {
            if(j==0){
                MatrixAfinal(i,0)=b(i,0);

            }

            else
            {
                MatrixAfinal(i,j)=MatrixA(i,j-1);

            }
        }

    }

    return MatrixAfinal;

}

//对消息的加密
//secret为要加密的数字
boost::numeric::ublas::matrix<int > Enc(int secret)
{
    using matrix=boost::numeric::ublas::matrix<int>;
    //密文 c
    matrix c(n+1,1);

    matrix r (N,1);

    auto MatrixA=GetMatrixB();

    for(unsigned int i=0;i<N;++i)
    {
        r(i,0)=RandomNum()%2;
    }

    //倒置矩阵
    MatrixA= boost::numeric::ublas::trans(MatrixA);

    //A倒置T * r
    matrix tmp(n+1,1);

   tmp= boost::numeric::ublas::prod(MatrixA, r);

    //构建加密消息
    matrix m(n+1,1);

    m(0,0)=secret;
    m(1,0)=0;
    m(2,0)=0;
    for(unsigned i=1;i<n+1;++i)
    {
        m(i,0)=m(i,0)*(q/2);
    }
    m(0,0)=m(0,0)*(q/2);

    c=m+tmp;

    return c;
}
inline int InnerProduct(int (&arr1)[3],int (&arr2)[3])
{
    int res=0;
    for(unsigned int i=0;i<n+1;++i)
    {
        res+=arr1[i]*arr2[i];
    }
    return res;
}

int  Dec(int val)
{
    //c为密文
    auto c=Enc(val);

    int arr1[3];
    int arr2[3];

    auto res1=boost::numeric::ublas::column(c, 0);

    auto res2=boost::numeric::ublas::column(FinalS, 0);

    for(int i=0;i<n+1;++i)
    {
        arr1[i]=res1(i);
        arr2[i]=res2(i);
    }
  int   res=InnerProduct(arr1, arr2);

    double qtwo=(double)2/q;

    res=res*qtwo;

    return res%2;
}


///////////////////////////////////////////////////////////////
//  Copyright 2019 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include "../performance_test.hpp"
#if defined(TEST_MPQ)
#include <boost/multiprecision/gmp.hpp>
#endif

void test21()
{
#ifdef TEST_MPQ
   test<boost::multiprecision::mpq_rational>("mpq_rational", 128);
#endif
}


#include <fstream>
#include <CGAL/Real_timer.h>
#include <CGAL/Random.h>
#include <CGAL/Simple_cartesian.h>

#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>

#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>

#include <boost/iterator/function_output_iterator.hpp>

using Kernel = CGAL::Simple_cartesian<double>;
using Point_3 = Kernel::Point_3;
using Vector_3 = Kernel::Vector_3;

using Point_set = CGAL::Point_set_3<Point_3>;
using Point_map = typename Point_set::Point_map;
using Normal_map = typename Point_set::Vector_map;

namespace Shape_detection = CGAL::Shape_detection::Point_set;

using Neighbor_query = Shape_detection::K_neighbor_query
  <Kernel, Point_set, Point_map>;
using Region_type = Shape_detection::Least_squares_cylinder_fit_region
  <Kernel, Point_set, Point_map, Normal_map>;
using Region_growing = CGAL::Shape_detection::Region_growing
  <Point_set, Neighbor_query, Region_type>;

int main (int argc, char** argv)
{
  std::ifstream ifile (argc > 1 ? argv[1] : "data/cube.pwn");
  Point_set points;
  ifile >> points;

  std::cerr << points.size() << " points read" << std::endl;

  // Input should have normals
  assert (points.has_normal_map());

  // Default parameters for data/cube.pwn
  const std::size_t k = 24;
  const double tolerance = 0.05;
  const double max_angle = 5.;
  const std::size_t min_region_size = 200;

  // No constraint on radius
  const double min_radius = 0.;
  const double max_radius = std::numeric_limits<double>::infinity();

  Neighbor_query neighbor_query(points, k, points.point_map());
  Region_type region_type(points, tolerance, max_angle, min_region_size,
                          min_radius, max_radius,
                          points.point_map(), points.normal_map());
  Region_growing region_growing(points, neighbor_query, region_type);

  // Add maps to get colored output
  Point_set::Property_map<unsigned char>
    red = points.add_property_map<unsigned char>("red", 0).first,
    green = points.add_property_map<unsigned char>("green", 0).first,
    blue = points.add_property_map<unsigned char>("blue", 0).first;

  CGAL::Random random;

  std::size_t nb_cylinders = 0;
  CGAL::Real_timer timer;
  timer.start();
  region_growing.detect
    (boost::make_function_output_iterator
     ([&](const std::vector<std::size_t>& region)
      {
        // Assign a random color to each region
        unsigned char r = (unsigned char)(random.get_int(64, 192));
        unsigned char g = (unsigned char)(random.get_int(64, 192));
        unsigned char b = (unsigned char)(random.get_int(64, 192));
        for (const std::size_t& idx : region)
        {
          red[idx] = r;
          green[idx] = g;
          blue[idx] = b;
        }
        ++ nb_cylinders;
      }));
  timer.stop();

  std::cerr << nb_cylinders << " cylinders detected in "
            << timer.time() << " seconds" << std::endl;

  // Save in colored_cylinders.ply
  std::ofstream out ("colored_cylinders.ply");
  CGAL::IO::set_binary_mode (out);
  out << points;

  return EXIT_SUCCESS;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

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

#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>

//#include "sphere.hpp"
//#include "clusterer.hpp"
//#include "sphericalKMeans.hpp"
#include "ddpmeans.hpp"
//#include "dpvMFmeans.hpp"
//#include "dir.hpp"
//#include "cat.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class DDPvMFMeans : public DDPMeans<T>
{
public:
  DDPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
      T lambda, T beta, T Q, boost::mt19937* pRndGen);
  virtual ~DDPvMFMeans();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual void updateLabelsParallel();
  virtual void updateLabels();
  virtual void updateCenters();
  virtual void nextTimeStep(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx);
  virtual void updateState(); // after converging for a single time instant
//  virtual MatrixXu mostLikelyInds(uint32_t n,
//     Matrix<T,Dynamic,Dynamic>& deviates);
//  virtual T avgIntraClusterDeviation();
  virtual uint32_t indOfClosestCluster(int32_t i);

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);

protected:
//  T lambda_;
//  Sphere<T> S_;

  T beta_;
  T Q_; //TODO!

  Matrix<T,Dynamic,Dynamic> xSums_;

  virtual Matrix<T,Dynamic,1> computeSum(uint32_t k);
  virtual void computeSums(void); // updates internal xSums_

  virtual void solveProblem1(T gamma, T age, T& phi, T& theta);
  virtual void solveProblem2(const Matrix<T,Dynamic,1>& xSum, T zeta, T age, T w,
      T& phi, T& theta, T& eta);

  virtual T distToUninstantiated(const Matrix<T,Dynamic,1>& x_i, uint32_t k);
  virtual void reInstantiatedOldCluster(const Matrix<T,Dynamic,1>& xSum, uint32_t k);

};
// --------------------------- impl -------------------------------------------

template<class T>
DDPvMFMeans<T>::DDPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
    T lambda, T beta, T Q, boost::mt19937* pRndGen)
  : DDPMeans<T>(spx,lambda,0.,0.,pRndGen), beta_(beta), Q_(Q)
{
  this->Kprev_ = 0; // so that centers are initialized directly from sample mean
  this->psPrev_ = this->ps_;
  xSums_ = Matrix<T,Dynamic,Dynamic>::Zero(this->D_,1);

  assert(-2.0 < this->lambda_ && this->lambda_ < 0.0);
}

template<class T>
DDPvMFMeans<T>::~DDPvMFMeans()
{}

template<class T>
uint32_t DDPvMFMeans<T>::indOfClosestCluster(int32_t i)
{
  int z_i = this->K_;
  T sim_closest = this->lambda_+1.;
  T sim_k = 0.;
  cout<<"cluster dists "<<i<<": "<< sim_closest;
  for (uint32_t k=0; k<this->K_; ++k)
  {
    if(this->Ns_(k) == 0)
    {// cluster not instantiated yet in this timestep
      sim_k = distToUninstantiated(this->spx_->col(i), k);
    }else{ // cluster instantiated
      sim_k = dist(this->ps_.col(k), this->spx_->col(i));
    }
    cout<<" "<<sim_k;
    if(closer(sim_k, sim_closest))
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  cout<<" => z_i="<<z_i<<endl;
  return z_i;
}

template<class T>
T DDPvMFMeans<T>::distToUninstantiated(const Matrix<T,Dynamic,1>& x_i, uint32_t k)
{

  T phi, theta, eta;
  T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.0),
          (x_i.transpose()*this->psPrev_.col(k))(0))));
  solveProblem2(x_i, zeta, this->ts_[k], this->ws_[k], phi,theta,eta);

//  cout<<" phi = "<<phi<<" age="<<this->ts_[k]<<endl;

  return this->ws_[k]*(cos(theta)-1.)
    +this->ts_[k]*beta_*(cos(phi)-1.)
    +cos(eta) // no minus 1 here cancels with Z(tau) from the two other assignments
    +Q_*this->ts_[k];
//  return cos(theta) + this->ts_[k]*cos(phi) + cos(eta);

  // TODO: this was the old wrong way
//  T phi, theta;
//  solveProblem1( acos( max(static_cast<T>(-1.),min(static_cast<T>(1.),
//            (this->ps_.col(k).transpose()*x_i)(0)))), this->ts_[k], phi, theta);
//  return cos(theta) + beta_*this->ts_[k]*cos(phi);
}

template<class T>
void DDPvMFMeans<T>::updateLabels()
{
// TODO not sure how to parallelize
//#pragma omp parallel for
  for(uint32_t i=0; i<this->N_; ++i)
  {
    uint32_t z_i = indOfClosestCluster(i);
    if(z_i == this->K_)
    { // start a new cluster
      this->ps_.conservativeResize(this->D_,this->K_+1);
      this->Ns_.conservativeResize(this->K_+1);
      this->ps_.col(this->K_) = this->spx_->col(i);
      this->Ns_(z_i) = 1.;
      this->K_ ++;
//      cout<<" added new cluster center at "<<this->spx_->col(i).transpose()<<endl;
    } else {
      if(this->Ns_[z_i] == 0)
      { // instantiated an old cluster
        reInstantiatedOldCluster(this->spx_->col(i), z_i);
      }
      this->Ns_(z_i) ++;
    }
    if(this->z_(i) != this->UNASSIGNED) this->Ns_(this->z_(i)) --;
    this->z_(i) = z_i;
  }

  for(uint32_t k=0; k<this->K_; ++k)
        this->Ns_(k) =0;

#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        this->Ns_(k) ++;
      }
  cout<<" Ns = "<<this->Ns_.transpose()<<endl;
};

template<class T>
void DDPvMFMeans<T>::updateLabelsParallel()
{
// TODO not sure how to parallelize
  Matrix<T,Dynamic,1> psNew(this->D_,1);
  psNew.fill(0);
  int32_t iNew = -1;
  uint32_t reInstantiateOld = 0;
  Matrix<T,Dynamic,1> reInstantiateFrom(this->D_,1);
  reInstantiateFrom.fill(0);
  int32_t iReinstantiate = -1;
  cout<<"::updateLabelsParallel"<<endl;
  uint32_t i0=0;
  do{
    iNew = -1;
    iReinstantiate = -1;
#pragma omp parallel for
    for(uint32_t i=i0; i<this->N_; ++i)
    {
      uint32_t z_i = indOfClosestCluster(i);
#pragma omp critical
      {
        if(z_i == this->K_)
        { // start a new cluster
          if(iNew < 0)
          {
            psNew = this->spx_->col(i);
            iNew = i;
          }
        } else {
          if(this->Ns_[z_i] == 0 && iReinstantiate < 0)
          { // instantiated an old cluster
            reInstantiateOld = z_i;
            reInstantiateFrom =  this->spx_->col(i);
            iReinstantiate = i;
          }
          //        this->Ns_(z_i) ++;
        }
      }
      //    if(this->z_(i) != this->UNASSIGNED) this->Ns_(this->z_(i)) --;
      this->z_(i) = z_i;
    }
    cout<<iReinstantiate<<" "<<iNew<<endl;

    if(iReinstantiate >= 0 && iNew >= 0)
    {
      if(iNew < iReinstantiate)
      {
        this->ps_.conservativeResize(this->D_,this->K_+1);
        this->Ns_.conservativeResize(this->K_+1);
        this->ps_.col(this->K_) = psNew;
        this->Ns_(this->K_) = 1.;
        this->K_ ++;
        i0 = iNew;
      }else{
        reInstantiatedOldCluster(reInstantiateFrom, reInstantiateOld);
        i0 = iReinstantiate;
      }
    }else if(iReinstantiate < 0 && iNew >= 0)
    {
      this->ps_.conservativeResize(this->D_,this->K_+1);
      this->Ns_.conservativeResize(this->K_+1);
      this->ps_.col(this->K_) = psNew;
      this->Ns_(this->K_) = 1.;
      this->K_ ++;
      i0 = iNew;
    }else if(iReinstantiate >= 0 && iNew < 0)
    {
      reInstantiatedOldCluster(reInstantiateFrom, reInstantiateOld);
        i0 = iReinstantiate;
    }
  }while(iReinstantiate >= 0 || iNew >= 0);

  //TODO get counts from GPU
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        this->Ns_(k) ++;
      }

};

template<class T>
void DDPvMFMeans<T>::reInstantiatedOldCluster(const Matrix<T,Dynamic,1>& xSum, uint32_t k)
{
//  cout<<"xSum: "<<xSum.transpose()<<endl;
  T phi, theta, eta;
  T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.0),
          (xSum.transpose()*this->psPrev_.col(k))(0)/xSum.norm())));
  solveProblem2(xSum , zeta, this->ts_[k], this->ws_[k], phi,theta,eta);

  // rotate point from mean_k towards previous mean by angle eta?
  this->ps_.col(k) = rotationFromAtoB<T>(xSum/xSum.norm(),
      this->psPrev_.col(k), eta/(phi*this->ts_[k]+theta+eta))
};

template<class T>
void DDPvMFMeans<T>::computeSums(void)
{
  xSums_ = Matrix<T,Dynamic,Dynamic>::Zero(this->D_, this->K_);
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        xSums_.col(k) += this->spx_->col(i);
      }
}

template<class T>
Matrix<T,Dynamic,1> DDPvMFMeans<T>::computeSum(uint32_t k)
{
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
    }
  return mean_k;
}

template<class T>
void DDPvMFMeans<T>::updateCenters()
{
//  xSums_ = computeSums();
  computeSums();
//#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
  {
//    Matrix<T,Dynamic,1> mean_k = this->computeCenter(k);
    if (this->Ns_(k) > 0)
    { // have data to update kth cluster
      if(k < this->Kprev_)
      { //TODO
        reInstantiatedOldCluster(xSums_.col(k), k);
      }else{
        this->ps_.col(k)= xSums_.col(k)/xSums_.col(k).norm(); //mean_k;
      }
    }
  cout<<this->ps_<<endl;
    assert(this->ps_(0,k) == this->ps_(0,k));
  }
};

template<class T>
void DDPvMFMeans<T>::nextTimeStep(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx)
{
  assert(this->D_ == spx->rows());
  if(this->spx_.get() != spx.get()) this->spx_ = spx; // update the data
  this->N_ = spx->cols();
  this->z_.resize(this->N_);
  this->z_.fill(this->UNASSIGNED);
};

template<class T>
void DDPvMFMeans<T>::updateState()
{
//  xSums_ = computeSums(); // already computed from last updateCenters (and no
//  label changes since)

  for(uint32_t k=0; k<this->K_; ++k)
  {
    if (k<this->ws_.size() && this->Ns_(k) > 0)
    { // instantiated cluster from previous time;
      T phi, theta, eta;
      T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.),
              (xSums_.col(k).transpose()*this->ps_.col(k))(0)
              / xSums_.col(k).norm())));
      solveProblem2(xSums_.col(k), zeta, this->ts_[k], this->ws_[k], phi,theta,eta);
      this->ws_[k] = this->ws_[k]*cos(theta)
        + beta_*this->ts_[k]*cos(phi)
        + xSums_.col(k).norm()*cos(eta);

      cout<<this->ws_[k]<<" : "<<theta<<" "<<phi<<" "<<eta
        <<" "<< xSums_.col(k).norm()<<endl;

//      this->ws_[k] = this->ws_[k]*cos(theta) + beta_*this->ts_[k]*cos(phi)
//        + xSums_.col(k).norm()*cos(eta);
//      this->ws_[k] = 1./(1./this->ws_[k] + this->ts_[k]*tau_) + this->Ns_(k);
      this->ts_[k] = 0; // re-instantiated -> age is 0
    }else if(k >= this->ws_.size()){
      // new cluster
      this->ts_.push_back(0);
      //TODO
      this->ws_.push_back(xSums_.col(k).norm());//this->Ns_(k));
    }

      assert(this->ws_[k] == this->ws_[k]);

    this->ts_[k] ++; // increment all ages
    cout<<"cluster "<<k
      <<"\tN="<<this->Ns_(k)
      <<"\tage="<<this->ts_[k]
      <<"\tweight="<<this->ws_[k]
      <<"\tcenter: "<<this->ps_.col(k).transpose()<<endl;
  }
  this->psPrev_ = this->ps_;
  this->Kprev_ = this->K_;
};

template<class T>
T DDPvMFMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
//  return acos(min(1.0,max(-1.0,(a.transpose()*b)(0)))); // angular similarity
  return a.transpose()*b; // cosine similarity
};

template<class T>
bool DDPvMFMeans<T>::closer(T a, T b)
{
//  return a<b; // if dist a is greater than dist b a is closer than b (angular dist)
  return a>b; // if dist a is greater than dist b a is closer than b (cosine dist)
};

template<class T>
void DDPvMFMeans<T>::solveProblem1(T gamma, T age, T& phi, T& theta)
{
  // solves
  // (1)  sin(phi) beta = sin(theta)
  // (2)  gamma = T phi + theta
  // for phi and theta
  phi = 0.0;

  for (uint32_t i=0; i< 10; ++i)
  {
    T sinPhi = sin(phi);
    T f = - gamma + age*phi + asin(beta_*sinPhi);
    // mathematica
    T df = age + (beta_*cos(phi))/sqrt(1.-beta_*beta_*sinPhi*sinPhi);
    T dPhi = f/df;
    phi = phi - dPhi; // Newton iteration
//    cout<<"@i="<<i<<": "<<phi<<"\t"<<dPhi<<endl;
    if(fabs(dPhi) < 1e-6) break;
  }

  theta = asin(beta_*sin(phi));
};

template<class T>
void DDPvMFMeans<T>::solveProblem2(const Matrix<T,Dynamic,1>& xSum, T zeta,
    T age, T w, T& phi, T& theta, T& eta)
{
  // solves
  // w sin(theta) = beta sin(phi) = ||xSum||_2 sin(eta)
  // eta + T phi + theta = zeta = acos(\mu0^T xSum/||xSum||_2)
  phi = 0.0;

//  cout<<"w="<<w<<" age="<<age<<" zeta="<<zeta<<endl;

  T L2xSum = xSum.norm();
  for (uint32_t i=0; i< 10; ++i)
  {
    T sinPhi = sin(phi);
    T cosPhi = cos(phi);
    T f = - zeta + asin(beta_/L2xSum *sinPhi) + age * phi + asin(beta_/w *sinPhi);
    T df = age + (beta_*cosPhi)/sqrt(L2xSum*L2xSum -
        beta_*beta_*sinPhi*sinPhi) + (beta_*cosPhi)/sqrt(w*w -
        beta_*beta_*sinPhi*sinPhi);

    T dPhi = f/df;

    phi = phi - dPhi; // Newton iteration
//    cout<<"@i="<<i<<": "<<"f="<<f<<" df="<<df<<" phi="<<phi<<"\t"<<dPhi<<endl;
    if(fabs(dPhi) < 1e-6) break;
  }

  theta = asin(beta_/w *sin(phi));
  eta = asin(beta_/L2xSum *sin(phi));
};


#include "gtest/gtest.h"
#include <Eigen/Core>
#include "Face.h"
#include "Point.h"

namespace {
  using namespace Geotree;

  class FaceTest : public ::testing::Test {
  protected:
  };

  TEST_F(FaceTest, Basic)
  {
    Face face(Vector3d(0,0,0), Vector3d(0,0,1), Vector3d(0,0,2), 0);
  }

  TEST_F(FaceTest, intersect)
  {
    Face face(Vector3d(0,0,0), Vector3d(3,0,0), Vector3d(0,3,0), 0);
    Segment segment(Vector3d(1,1,-1), Vector3d(1,1,1), 0);

    std::vector <Point> points;

    face.intersect(segment, points);

    EXPECT_EQ(points.size(), 1);
  }

  TEST_F(FaceTest, split)
  {
    Face face(Vector3d(0,0,0), Vector3d(1,0,0), Vector3d(0,1,0), 0);

    std::vector <std::set<Point>> paths;
    std::set<Point> path;

    Point p0(Vector3d(0.5,0,0));
    Point p1(Vector3d(0,0.5,0));

    path.insert(p0);
    path.insert(p1);
    paths.push_back(path);
    std::vector <Matrix<int, Dynamic, 3>> split = face.split(paths);

    EXPECT_EQ(split.size(), 2);
  }
}


/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _PLAN_CONTAINER_H_
#define _PLAN_CONTAINER_H_

#include <Eigen/Eigen>
#include <vector>
#include <ros/ros.h>

#include <bspline/non_uniform_bspline.h>
#include <traj_utils/polynomial_traj.h>
using std::vector;

namespace fast_planner {

struct PlanParameters {
  /* planning algorithm parameters */
  double max_vel_, max_acc_, max_jerk_;  // physical limits
  double local_traj_len_;                // local replanning trajectory length
  double ctrl_pt_dist;                   // distance between adjacient B-spline
                                         // control points
  double clearance_;
  int dynamic_;
  /* processing time */
  double time_search_ = 0.0;
  double time_optimize_ = 0.0;
  double time_adjust_ = 0.0;
};

struct LocalTrajData {
  /* info of generated traj */

  int traj_id_;
  double duration_;
  ros::Time start_time_;
  Eigen::Vector3d start_pos_, Astar_Local_Target_;
  NonUniformBspline position_traj_, velocity_traj_, acceleration_traj_, jerk_traj_, yaw_traj_, yawdot_traj_,
      yawdotdot_traj_;
  vector<vector<Eigen::Vector3d>> planned_wpts_, sampled_wpts_, motion_primitive_wpts_;
  int best_primitive_index_;
  vector<Eigen::Vector3d> geo_astar_wpts_;
  vector<int> planned_motion_state_list_, sampled_state_list_;
  PolynomialTraj best_traj_;
  double compute_time_, benchmark_compute_time_, benchmark_acc_;
};

class MidPlanData {
public:
  MidPlanData(/* args */) {}
  ~MidPlanData() {}

  vector<Eigen::Vector3d> global_waypoints_;

  // initial trajectory segment
  NonUniformBspline initial_local_segment_;
  vector<Eigen::Vector3d> local_start_end_derivative_;

  // kinodynamic path
  vector<Eigen::Vector3d> kino_path_;

  // visibility constraint
  vector<Eigen::Vector3d> block_pts_;
  Eigen::MatrixXd ctrl_pts_;

  // heading planning
  vector<double> path_yaw_;
  double dt_yaw_;
  double dt_yaw_path_;
};

}  // namespace fast_planner

#endif


#include <cstdint>
#include <memory>
#include <vector>
#include <string>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <omp.h>
#include <stdio.h>
#include <valarray>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

#include <posit/posit>
// Posit Arithmetic FPGA accelerator library
#include <positarith.h>

#include "main.hpp"
#include "defines.hpp"
#include "utils.hpp"
#include "blas.hpp"
#include "vector_utils.hpp"
#include "matrix_utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

posit<NBITS, ES> random_number(float offset, float dev) {
    float num_float;
    posit<NBITS, 2> num_posit_2;
    posit<NBITS, 3> num_posit_3;

    do {
        num_float = offset + (rand() * dev / RAND_MAX);
        num_posit_2 = num_float;
        num_posit_3 = num_float;
    } while (num_posit_2 != num_float || num_posit_3 != num_float);

    return posit<NBITS, ES>(num_float);
}

int main(int argc, char ** argv)
{
    vector<int> lengths = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000, 200000, 300000, 400000, 500000};

    std::string s_dot;
    for(int length : lengths) {
        s_dot = test_dot_product(length);

        ofstream outfile("positarith_dot_es" + std::to_string(ES) + "_" + std::to_string(length) + ".txt", ios::out);
        outfile << s_dot << endl;
        outfile.close();

        remove("top.wdb");
    }

    return 0;
}

std::string test_dot_product(int length) {
    double t_fpga, t_sw, t_float;
    double stop, start;
    int i;
    cpp_dec_float_100 da_sw = 0.0, da_hw = 0.0, da_float = 0.0;

    // Test data set
    std::vector<posit<NBITS,ES>> vec1, vec2;
    // vec1.resize(length); vec2.resize(length);
    for(int i = 0; i < length; i++) {
        posit<NBITS,ES> pos1, pos2;
        pos1 = random_number(0.0, 1.0);
        pos2 = random_number(0.0, 1.0);

        vec1.push_back(pos1);
        vec2.push_back(pos2);
    }

    // Posit HW calculation
    posit<NBITS,ES> res_hw = 0;
    // t_fpga = vector_dot(vec1, vec2, res_hw);

    // Posit SW calculation
    posit<NBITS,ES> res_sw = 0.0;
    start = omp_get_wtime();
    // for(i = 0; i < length; i++) {
    //     res_sw += vec1[i] * vec2[i];
    // }
    res_sw = sw::hprblas::dot(length, vec1, 1, vec2, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    // Float calculation
    float res_float = 0.0;
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        #pragma omp for reduction(+:res_float)
        for(i = 0; i < length; i++) {
            res_float += (float)vec1[i] * (float)vec2[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // Reference calculation
    cpp_dec_float_100 res_dec = 0.0;
    for(int i = 0; i < length; i++) {
        res_dec += (cpp_dec_float_100)((long double)vec1[i]) * (cpp_dec_float_100)((long double)vec2[i]);
    }

    // Calculate decimal accuracy
    da_float = decimal_accuracy(res_dec, (cpp_dec_float_100)res_float);
    da_sw = decimal_accuracy(res_dec, (cpp_dec_float_100)((long double)res_sw));
    da_hw = decimal_accuracy(res_dec, (cpp_dec_float_100)((long double)res_hw));

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float) + "," + to_string_precision(da_hw) + "," + to_string_precision(da_sw) + "," + to_string_precision(da_float);
}


#ifndef YANNQ_GROUNDSTATE_NGDEXACT_HPP
#define YANNQ_GROUNDSTATE_NGDEXACT_HPP
#include <complex>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <tbb/tbb.h>

#include "ED/ConstructSparseMat.hpp"
#include "Utilities/Utility.hpp"

#include "Machines/AmplitudePhase.hpp"

namespace yannq
{

class NGDExact
{
public:
	using Machine = AmplitudePhase;
	using RealScalar = typename Machine::RealScalar;
	using ComplexScalar = typename Machine::ComplexScalar;

	using RealMatrix = typename Machine::RealMatrix;
	using RealVector = typename Machine::RealVector;

	using ComplexMatrix = typename Machine::ComplexMatrix;
	using ComplexVector = typename Machine::ComplexVector;

private:
	const uint32_t n_;
	const AmplitudePhase& qs_;
	tbb::concurrent_vector<uint32_t> basis_;

	Eigen::SparseMatrix<RealScalar> ham_;

	RealMatrix deltasAmp_;
	RealMatrix deltasPhase_;

	RealMatrix deltasAmpPsis_;
	RealMatrix deltasPhasePsis_;

	RealVector olocAmp_;
	RealVector olocPhase_;

	RealVector grad_;

	RealScalar energy_;

	void constructDeltaAmp()
	{
		using Range = tbb::blocked_range<std::size_t>;
		const int N = qs_.getN();
		deltasAmp_.setZero(basis_.size(), qs_.getDimAmp());
		if(basis_.size() >= 32)
		{
			tbb::parallel_for(Range(std::size_t(0u), basis_.size(), 8),
				[&](const Range& r)
			{
				uint32_t start = r.begin();
				uint32_t end = r.end();
				RealMatrix tmp(end-start, qs_.getDimAmp());
				for(uint32_t l = 0; l < end-start; ++l)
				{
					tmp.row(l) =
						qs_.logDerivAmp(qs_.makeAmpData(toSigma(N, basis_[l+start])));
				}
				deltasAmp_.block(start, 0, end-start, qs_.getDimAmp()) = tmp;
			}, tbb::simple_partitioner());
		}
		else
		{
			for(uint32_t k = 0; k < basis_.size(); k++)
			{
				deltasAmp_.row(k) =
					qs_.logDerivAmp(qs_.makeAmpData(toSigma(N, basis_[k])));
			}
		}
	}

	void constructDeltaPhase()
	{
		using Range = tbb::blocked_range<std::size_t>;
		const int N = qs_.getN();
		deltasPhase_.setZero(basis_.size(), qs_.getDimPhase());

		if(basis_.size() >= 32)
		{
			tbb::parallel_for(Range(std::size_t(0u), basis_.size(), 8),
				[&](const Range& r)
			{
				uint32_t start = r.begin();
				uint32_t end = r.end();
				RealMatrix tmp(end-start, qs_.getDimPhase());
				for(uint32_t l = 0; l < end-start; ++l)
				{
					tmp.row(l) =
						qs_.logDerivPhase(qs_.makePhaseData(toSigma(N, basis_[l+start])));
				}
				deltasPhase_.block(start, 0, end-start, qs_.getDimPhase()) = tmp;
			}, tbb::simple_partitioner());
		}
		else
		{
			for(uint32_t k = 0; k < basis_.size(); k++)
			{
				deltasPhase_.row(k) =
					qs_.logDerivPhase(qs_.makePhaseData(toSigma(N, basis_[k])));
			}

		}
	}

public:

	RealScalar getEnergy() const
	{
		return energy_;
	}

	void constructExact()
	{
		ComplexVector st = getPsi(qs_, basis_, true);
		ComplexVector k = ham_*st;

		energy_ = std::real(ComplexScalar(st.adjoint()*k));

		constructDeltaAmp();
		constructDeltaPhase();

		deltasAmpPsis_ = st.cwiseAbs2().asDiagonal()*deltasAmp_;
		olocAmp_ = deltasAmpPsis_.colwise().sum();

		deltasPhasePsis_ = st.cwiseAbs2().asDiagonal()*deltasPhase_;
		olocPhase_ = deltasPhasePsis_.colwise().sum();

		k = st.conjugate().asDiagonal()*k;
		grad_.resize(qs_.getDim());
		grad_.head(qs_.getDimAmp()) =
			2.0*deltasAmp_.transpose()*k.real();
		grad_.head(qs_.getDimAmp()) -= 2.0*energy_*olocAmp_;
		grad_.tail(qs_.getDimPhase())
			= 2.0*deltasPhase_.transpose()*k.imag();
		/*
		grad_.tail(qs_.getDimPhase())
			-= 2.0*energy_*olocPhase_;
			*/
	}

	RealScalar eloc() const
	{
		return energy_;
	}

	const RealVector& olocAmp() const&
	{
		return olocAmp_;
	}

	RealMatrix olocAmp() &&
	{
		return olocAmp_;
	}

	const RealVector& olocPhase() const&
	{
		return olocPhase_;
	}

	RealMatrix olocPhase() &&
	{
		return olocPhase_;
	}

	RealMatrix corrMatAmp() const
	{
		RealMatrix res = deltasAmp_.transpose()*deltasAmpPsis_;
		res -= olocAmp_*olocAmp_.transpose();
		return res;
	}

	RealMatrix corrMatPhase() const
	{
		RealMatrix res = deltasPhase_.transpose()*deltasPhasePsis_;
		res -= olocPhase_*olocPhase_.transpose();
		return res;
	}

	RealVector energyGrad() const
	{
		return grad_;
	}

	template<class Iterable, class ColFunc>
	NGDExact(const AmplitudePhase& qs, Iterable&& basis, ColFunc&& col)
	  : n_{qs.getN()}, qs_(qs)
	{
		tbb::parallel_for_each(basis.begin(), basis.end(),
				[&](uint32_t elt)
		{
			basis_.emplace_back(elt);
		});
		tbb::parallel_sort(basis_.begin(), basis_.end());

		ham_ = edp::constructSubspaceMat(std::forward<ColFunc>(col), basis_);
	}
};
} //namespace yannq
#endif//YANNQ_GROUNDSTATE_NGDEXACT_HPP


//
// Copyright Vardan Akopian 2007
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/ublas_banded.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    namespace detail {
      inline
      void gbtrf (int const n, int const m, int const kl, int const ku,
                  double* ab, int const ldab, int* ipiv, int* info)
      {
        LAPACK_DGBTRF (&n, &m, &kl, &ku, ab, &ldab, ipiv, info);
      }
    }

    template <typename MatrA, typename IVec>
    inline
    int gbtrf (MatrA& a, IVec& ipiv) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::banded_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        traits::row_major_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      int const m = traits::matrix_size2 (a);
      assert (traits::vector_size (ipiv) == (m < n ? m : n));

      // if the matrix has kl lower and ku upper diagonals, then we should have
      // allocated kl lower and kl+ku upper diagonals
      int const kl = traits::matrix_lower_bandwidth (a);
      int const ku = traits::matrix_upper_bandwidth (a) - kl;
      int const ld = traits::leading_dimension (a);

      assert(ku >= 0);

      int info;
      detail::gbtrf (n, m, kl, ku,
                     traits::matrix_storage (a),
		     ld,
                     traits::vector_storage (ipiv),
                     &info);
      return info;
    }

    namespace detail {
      inline
      void gbtrs (char const trans, int const n, int const kl, int const ku, int const m,
                  double const* ab, int const ldab, int const* ipiv,
		  double* b, int const ldb, int* info)
      {
        LAPACK_DGBTRS (&trans, &n, &kl, &ku, &m, ab, &ldab, ipiv, b, &ldb, info);
      }
    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int gbtrs (char const trans, MatrA const& a, IVec const& ipiv, MatrB& b)
    {
      assert (trans == 'N' || trans == 'T' || trans == 'C');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::banded_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (ipiv));

      // if the matrix has kl lower and ku upper diagonals, then we should have
      // allocated kl lower and kl+ku upper diagonals
      int const kl = traits::matrix_lower_bandwidth (a);
      int const ku = traits::matrix_upper_bandwidth (a) - kl;
      int const ld = traits::leading_dimension (a);

      assert(ku >= 0);

      int info;
      detail::gbtrs (trans, n, kl, ku, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
#else
                     traits::matrix_storage_const (a),
#endif
                     ld,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::vector_storage (ipiv),
#else
                     traits::vector_storage_const (ipiv),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

  }

}}}

#endif