// Bring in my package's API, which is what I'm testing
#include <bsplines/BSplinePose.hpp>
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <stdio.h>
// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from libsm
#include <sm/eigen/gtest.hpp>
#include <sm/eigen/NumericalDiff.hpp>

#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>
#include <sm/kinematics/RotationVector.hpp>
#include <sm/kinematics/EulerRodriguez.hpp>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/EulerAnglesZYX.hpp>
#include <stdio.h>

using namespace bsplines;
using namespace sm::kinematics;

struct BSplineTransformationJacobianFunctor
{
  // Necessary for eigen fixed sized type member variables.
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef Eigen::VectorXd input_t;
  typedef Eigen::MatrixXd jacobian_t;
  typedef Eigen::VectorXd value_t;
  typedef double scalar_t;

  BSplineTransformationJacobianFunctor(BSplinePose bs, const Eigen::Vector4d & v, double t) :
    bs_(bs), t_(t), v_(v)
  {

  }

  input_t update(const input_t & x, int c, double delta)
  {
    input_t xnew = x;
    xnew[c] += delta;
    return xnew;
  }

  Eigen::VectorXd operator()(const Eigen::VectorXd & c)
  {
    bs_.setLocalCoefficientVector(t_,c);
    Eigen::Matrix4d T = bs_.transformation(t_);
    return T * v_;
  }

  BSplinePose bs_;
  double t_;
  Eigen::Vector4d v_;
};

struct BSplineInverseTransformationJacobianFunctor
{
  // Necessary for eigen fixed sized type member variables.
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef Eigen::VectorXd input_t;
  typedef Eigen::MatrixXd jacobian_t;
  typedef Eigen::VectorXd value_t;
  typedef double scalar_t;

  BSplineInverseTransformationJacobianFunctor(BSplinePose bs, const Eigen::Vector4d & v, double t) :
    bs_(bs), t_(t), v_(v)
  {

  }

  input_t update(const input_t & x, int c, double delta)
  {
    input_t xnew = x;
    xnew[c] += delta;
    return xnew;
  }

  Eigen::VectorXd operator()(const Eigen::VectorXd & c)
  {
    bs_.setLocalCoefficientVector(t_,c);
    Eigen::Matrix4d T = bs_.inverseTransformation(t_);
    return T * v_;
  }

  BSplinePose bs_;
  double t_;
  Eigen::Vector4d v_;
};

// Check that the Jacobian calculation is correct.
TEST(SplineTestSuite, testBSplineTransformationJacobian)
{
  boost::shared_ptr<RotationalKinematics> rvs[3];

  rvs[0].reset(new EulerAnglesZYX());
  rvs[1].reset(new RotationVector());
  rvs[2].reset(new EulerRodriguez());

  for(int r = 0; r < 3; r++)
  {
      for(int order = 2; order < 10; order++)
	{
	  // Create a two segment spline.
	  BSplinePose bs(order, rvs[r]);
	  bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(Eigen::VectorXd::Random(6)),
			    bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));
	  bs.addPoseSegment(2.0,bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));

	  // Create a random homogeneous vector.
	  Eigen::Vector4d v = Eigen::Vector4d::Random() * 10.0;

	  for(double t = bs.t_min(); t <= bs.t_max(); t+= 0.413)
	    {
	      BSplineTransformationJacobianFunctor f(bs, v, t);
	      sm::eigen::NumericalDiff<BSplineTransformationJacobianFunctor> nd(f);
	      Eigen::MatrixXd estJ = nd.estimateJacobian(bs.localCoefficientVector(t));
	      Eigen::MatrixXd JT;

	      Eigen::Matrix4d T = bs.transformationAndJacobian(t, &JT);

	      //std::cout << "Full jacobian:\n" << JT << std::endl;
	      //std::cout << "boxMinus(v):\n" << sm::kinematics::boxMinus(v) << std::endl;
	      Eigen::MatrixXd J = sm::kinematics::boxMinus(T*v) * JT;

	      sm::eigen::assertNear(J, estJ, 1e-6, SM_SOURCE_FILE_POS);

	      // Try again with the lumped function.
	      Eigen::Vector4d v_n = bs.transformVectorAndJacobian(t,v, &J);
	      sm::eigen::assertNear(v_n, T*v, 1e-6, SM_SOURCE_FILE_POS);
	      sm::eigen::assertNear(J, estJ, 1e-6, SM_SOURCE_FILE_POS);
	    }
	}
    }

}

#if 1
// Check that the Jacobian calculation is correct.
TEST(SplineTestSuite, testBSplineInverseTransformationJacobian)
{
  boost::shared_ptr<RotationalKinematics> rvs[3];

  rvs[0].reset(new EulerAnglesZYX());
  rvs[1].reset(new RotationVector());
  rvs[2].reset(new EulerRodriguez());

  for(int r = 0; r < 3; r++)
  {
      for(int order = 2; order < 10; order++)
	{
	  // Create a two segment spline.
	  BSplinePose bs(order, rvs[r]);
	  bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(Eigen::VectorXd::Random(6)),
			    bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));
	  bs.addPoseSegment(2.0,bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));

	  // Create a random homogeneous vector.
	  Eigen::Vector4d v = Eigen::Vector4d::Random() * 10.0;

	  for(double t = bs.t_min(); t <= bs.t_max(); t+= 0.413)
	    {
	      BSplineInverseTransformationJacobianFunctor f(bs, v, t);
	      sm::eigen::NumericalDiff<BSplineInverseTransformationJacobianFunctor> nd(f);
	      Eigen::MatrixXd estJ = nd.estimateJacobian(bs.localCoefficientVector(t));
	      Eigen::MatrixXd JT;
              Eigen::MatrixXd J;

	      Eigen::Matrix4d T = bs.inverseTransformationAndJacobian(t, &JT);

	      //std::cout << "Full jacobian:\n" << JT << std::endl;
	      //std::cout << "boxMinus(v):\n" << sm::kinematics::boxMinus(v) << std::endl;
	      J = sm::kinematics::boxMinus(T*v) * JT;
              //std::cout << "J(0,0): " << J(0,0) << std::endl;
	      sm::eigen::assertNear(J, estJ, 1e-6, SM_SOURCE_FILE_POS);

	      // Try again with the lumped function.
	      //Eigen::Vector4d v_n = bs.inverseTransformVectorAndJacobian(t,v, &J);
	      //sm::eigen::assertNear(v_n, T*v, 1e-6, SM_SOURCE_FILE_POS);
	      //sm::eigen::assertNear(J, estJ, 1e-6, SM_SOURCE_FILE_POS);
	    }
	}
    }

}

#endif

struct BSplineAccelerationJacobianFunctor
{
  // Necessary for eigen fixed sized type member variables.
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef Eigen::VectorXd input_t;
  typedef Eigen::MatrixXd jacobian_t;
  typedef Eigen::Vector3d value_t;
  typedef double scalar_t;

  BSplineAccelerationJacobianFunctor(BSplinePose bs, double t) :
    bs_(bs), t_(t)
  {

  }

  input_t update(const input_t & x, int c, double delta)
  {
    input_t xnew = x;
    xnew[c] += delta;
    return xnew;
  }

  Eigen::Vector3d operator()(const Eigen::VectorXd & c)
  {
    bs_.setLocalCoefficientVector(t_,c);
    Eigen::Vector3d a = bs_.linearAccelerationAndJacobian(t_,NULL,NULL);
    //std::cout << "a.size(): " << a.size() << std::endl;
    return a;
  }

  BSplinePose bs_;
  double t_;
};

TEST(SplineTestSuite, testBSplineAccelerationJacobian)
{
  boost::shared_ptr<RotationalKinematics> r(new RotationVector());

  for(int order = 2; order < 10; order++)
    {
      // Create a two segment spline.
      BSplinePose bs(order, r);
      bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(Eigen::VectorXd::Random(6)),
			bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));
      bs.addPoseSegment(2.0,bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));

      for(double t = bs.t_min(); t <= bs.t_max(); t+= 0.1)
	{

	  Eigen::MatrixXd J;
	  bs.linearAccelerationAndJacobian(t,&J,NULL);

	  BSplineAccelerationJacobianFunctor f(bs,t);
	  sm::eigen::NumericalDiff<BSplineAccelerationJacobianFunctor> nd(f);
	  Eigen::MatrixXd estJ = nd.estimateJacobian(bs.localCoefficientVector(t));

	  //std::cout << "J\n" << J << "\nestJ:\n" << estJ << std::endl;
	  sm::eigen::assertNear(J, estJ, 1e-6, SM_SOURCE_FILE_POS);

	}
    }
}

TEST(SplineTestSuite, testBSplineCurveQuadraticIntegralSparse) {

    try {
    boost::shared_ptr<RotationalKinematics> rvs;

    rvs.reset(new RotationVector());

        const int length = 50;
        int numSegments = 20;
        double lambda = 0.5;

        for(int order = 2; order < 10; order++)
        {
            // Create a two segment spline.
            BSplinePose bs(order, rvs);
           // bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(Eigen::VectorXd::Random(6)),
           //                   bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));
          //  bs.addPoseSegment(2.0,bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));

            // random positive times
            Eigen::Matrix<double, length, 1> times = Eigen::VectorXd::Random(length) + Eigen::VectorXd::Ones(length);
            // sorted
            std::sort(&times.coeffRef(0), &times.coeffRef(0)+times.size());

            times[length-1] = ceil(times[length-1]);

            Eigen::MatrixXd poses = Eigen::MatrixXd::Random(6,length);

            bs.initPoseSpline3(times,poses,numSegments,lambda);

            // random symmetric matrix:
            Eigen::MatrixXd W = Eigen::MatrixXd::Random(6,6);
            for (int i = 0; i < 6; i++)
                for(int j = 0; j < i; j++)
                    W(j,i) = W(i,j);

            sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q_sparse = bs.curveQuadraticIntegralSparse(W, 0);

            Eigen::MatrixXd Q_dense = bs.curveQuadraticIntegral(W, 0);

            sm::eigen::assertNear(Q_dense, Q_sparse.toDense(), 1e-10, SM_SOURCE_FILE_POS);
        }
    }
    catch(const std::exception &e) {
        FAIL() << e.what();
    }

}

TEST(SplineTestSuite, testBSplineCurveQuadraticIntegralDiagSparse) {

    try {
        boost::shared_ptr<RotationalKinematics> rvs;

        rvs.reset(new RotationVector());
        const int length = 50;
        int numSegments = 20;
        double lambda = 0.5;

        for(int order = 2; order < 10; order++)
        {
            // Create a two segment spline.
            BSplinePose bs(order, rvs);
         //   bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(Eigen::VectorXd::Random(6)),
         //                     bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));
         //   bs.addPoseSegment(2.0,bs.curveValueToTransformation(Eigen::VectorXd::Random(6)));

            // random positive times
            Eigen::Matrix<double, length, 1> times = Eigen::VectorXd::Random(length) + Eigen::VectorXd::Ones(length);
            // sorted
            std::sort(&times.coeffRef(0), &times.coeffRef(0)+times.size());

            times[length-1] = ceil(times[length-1]);

            Eigen::MatrixXd poses = Eigen::MatrixXd::Random(6,length);

            bs.initPoseSpline3(times,poses,numSegments,lambda);

            // random symmetric matrix:
            Eigen::VectorXd W = Eigen::VectorXd::Random(6);

            sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q_sparse = bs.curveQuadraticIntegralDiagSparse(W, 0);

            Eigen::MatrixXd Q_dense = bs.curveQuadraticIntegralDiag(W, 0);

            sm::eigen::assertNear(Q_dense, Q_sparse.toDense(), 1e-10, SM_SOURCE_FILE_POS);
        }
    }
    catch(const std::exception &e) {
        FAIL() << e.what();
    }

}

#include <boost/progress.hpp>

TEST(SplineTestSuite, testInitSpline3Sparse) {

    try {
        boost::shared_ptr<RotationalKinematics> rvs;

        rvs.reset(new RotationVector());

        const bool benchmark = false;

        const int length = benchmark ? 1000 : 100;
        int numSegments = benchmark ? 200 : 20;
        double lambda = 0.1;
      //  int order = 2;
        for(int order = 2; order < 8; order++)
        {
            // Create a two segment spline.
            BSplinePose bs_sparse(order, rvs);
            BSplinePose bs_dense(order, rvs);

            // random positive times
            Eigen::Matrix<double, length, 1> times = Eigen::VectorXd::Random(length) + Eigen::VectorXd::Ones(length);
            // sorted
            std::sort(&times.coeffRef(0), &times.coeffRef(0)+times.size());

            times[length-1] = ceil(times[length-1]);

      //      times = times;

          //  std::cout << "Times:" << std::endl;
          //  std::cout << times << std::endl;

            Eigen::MatrixXd poses = Eigen::MatrixXd::Random(6,length);

            if(benchmark){
              {
                  boost::progress_timer timer;
                  std::cout << "Dense:" << std::endl;
                  bs_dense.initPoseSpline3(times,poses,numSegments,lambda);
              }
              {
                  boost::progress_timer timer;
                  std::cout << "Sparse:" << std::endl;
                  bs_sparse.initPoseSplineSparse(times,poses,numSegments,lambda);
              }
            }

            // diagonals
      //      for (int i = 0; i < Asparse.rows(); i++)
     //           std::cout << Asparse(i,i) << " : " << Adense(i,i) << std::endl;

            Eigen::MatrixXd m1 = bs_sparse.coefficients();
            Eigen::MatrixXd m2 = bs_dense.coefficients();

         /*   std::cout << "m1:" << std::endl;
            std::cout << m1 << std::endl;
            std::cout << "m2:" << std::endl;
            std::cout << m2 << std::endl;
*/
            sm::eigen::assertNear(m1, m2, 1e-8, SM_SOURCE_FILE_POS);

        }
    }
    catch(const std::exception &e) {
        FAIL() << e.what();
    }

}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/exponentbits.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" exponentbits float",  (float))
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::exponentbits;
  using r_t = decltype(exponentbits(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  for(int i=1, k = 0; i < 10; i*= 2, ++k)
  {
  namespace bs = boost::simd;
    STF_EQUAL(1065353216+k*8388608, exponentbits(T(i)));
  }
}

STF_CASE_TPL (" exponentbits double",  (double))
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  using bs::exponentbits;
  using r_t = decltype(exponentbits(T()));

  // return type conformity test
  using r_t = decltype(exponentbits(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  for(int i=1, k = 0; i < 10; i*= 2, ++k)
   {
  namespace bs = boost::simd;
     STF_EQUAL(4607182418800017408ll+k*4503599627370496ll, exponentbits(T(i)));
   }
}


//==============================================================================
//         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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex toolbox - tand/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of tand  components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
///
#include <nt2/include/functions/tand.hpp>
#include <nt2/include/functions/tan.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/include/constants/deginrad.hpp>
#include <complex>

NT2_TEST_CASE_TPL ( tand_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::tand;
  using nt2::tag::tand_;
  typedef std::complex<T> cT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<tand_(cT)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename nt2:: meta::as_complex<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(nt2::Inf<T>())), cT(nt2::Nan<T>()), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(nt2::Minf<T>())), cT(nt2::Nan<T>()), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(1, 1)),std::tan(nt2::Deginrad<T>()*cT(1.0, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(1, 0.3)),std::tan(nt2::Deginrad<T>()*cT(1.0, 0.3)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(0.3, 1)),std::tan(nt2::Deginrad<T>()*cT(0.3, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(0.3, 0.3)),std::tan(nt2::Deginrad<T>()*cT(0.3, 0.3)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(0, 1)),std::tan(nt2::Deginrad<T>()*cT(0.0, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(0, 0.3)),std::tan(nt2::Deginrad<T>()*cT(0.0, 0.3)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tand(cT(0.3, 0)),std::tan(nt2::Deginrad<T>()*cT(0.3, 0.0)), 2);

  const int N = 20;
  cT inputs[N] =
    { cT(nt2::Zero<T>(),nt2::Zero<T>()),cT(nt2::Inf<T>(),nt2::Zero<T>()),cT(nt2::Minf<T>(),nt2::Zero<T>()),cT(nt2::Nan<T>(),nt2::Zero<T>()),
      cT(nt2::Zero<T>(),nt2::Inf<T>()), cT(nt2::Inf<T>(),nt2::Inf<T>()), cT(nt2::Minf<T>(),nt2::Inf<T>()), cT(nt2::Nan<T>(),nt2::Inf<T>()),
      cT(nt2::Zero<T>(),nt2::Minf<T>()),cT(nt2::Inf<T>(),nt2::Minf<T>()),cT(nt2::Minf<T>(),nt2::Minf<T>()),cT(nt2::Nan<T>(),nt2::Minf<T>()),
      cT(nt2::Zero<T>(),nt2::Nan<T>()), cT(nt2::Inf<T>(),nt2::Nan<T>()), cT(nt2::Minf<T>(),nt2::Nan<T>()), cT(nt2::Nan<T>(),nt2::Nan<T>()),
      cT(nt2::Zero<T>(),180), cT(nt2::Inf<T>(),180), cT(nt2::Minf<T>(),180), cT(nt2::Nan<T>(),180),
    };

  for(int i=0; i < N; i++)
   {
     std::cout << "-------------------" << std::endl;
     std::cout << "inputs  "<< inputs[i] << std::endl;
     NT2_TEST_ULP_EQUAL(nt2::tand(-inputs[i]), -nt2::tand(inputs[i]), 3);
     NT2_TEST_ULP_EQUAL(nt2::tand(inputs[i]), nt2::mul_minus_i(nt2::tanh(nt2::mul_i(nt2::multiplies(nt2::Deginrad<T>(), inputs[i])))), 3);
     std::cout << "=================== " << std::endl;
   }

 } // end of test for floating_


#pragma once

#include <boost/operators.hpp>

#include <iterator>
#include <string>
#include <vector>
#include <algorithm>
#include <functional>

#include "defaults.hpp"
#include "exceptions.hpp"
#include "num_io.hpp"
#include "almost_equal.hpp"

namespace gftools {

/** This class describes a point on a grid. It has an index (integer) and a value (pretty much anything) , and it
maps between the two. The index is used for fast access, and the value is used for other things like interpolation/physics/math.*/
template <typename ValueType> class point_base :
    ///dependence on this provides additional comparison operators
    boost::less_than_comparable<point_base<ValueType> >,
    ///dependence on this provides additional comparison operators
    boost::equality_comparable<point_base<ValueType> >
 {
public:
    //there is a small wrapper for integers down below. Here we exclude grids of ints to avoid confusion in the cast operators.
    static_assert(!std::is_same<ValueType,int>::value, "Can't create a grid of ints");

    typedef ValueType value_type;

    ///cast operator to value type
    operator ValueType() const { return val_; }
    ///cast operator to index type
    explicit operator size_t() const { return index_; }
    ///another cast operator to index type
    explicit operator int() const { return index_; }

    ///constructor with a pair of values and indices
    point_base(ValueType val, size_t index):val_(val),index_(index){}

    bool operator==(const point_base &rhs) const {return index_ == rhs.index_;}
    bool operator<(const point_base &rhs) const {return this->index_ < rhs.index_;}

    ValueType value() const { return val_; }
    size_t index() const { return index_; }

protected:
    ///grid point (in physical units)
    ValueType val_;
    ///grid point index
    size_t index_;
};

template<typename T> std::ostream& operator<<(std::ostream& lhs, const point_base<T> &p){
  lhs<<"{"<<p.value()<<"<-["<<p.index()<<"]}"; return lhs;
}

/** A one-dimensional grid, which stores an array of values. Typical examples: grid of real frequencies. Grid of k-points. Grid of Matsubara frequencies. Grid of imaginary times.*/
template <typename ValueType, class Derived>
class grid_base : public boost::equality_comparable<grid_base<ValueType, Derived> > {
public:
    typedef point_base<ValueType> point;
    typedef ValueType value_type;

    /** constructor a grid out of thin air. */
    grid_base();
    /** construct a grid given a vector of points. */
    grid_base(const std::vector<point> & vals);
    /** construct a grid from a vector of values (but not associated indices that would be stored in points). */
    grid_base(const std::vector<ValueType> & vals);
    /** Initialize the values from an external function that maps the integer values to the ValueType values. */
    grid_base(int min, int max, std::function<ValueType (int)> f);

    /** Returns a value at given index. */
    point operator[](size_t in) const;
    /** Returns all values. */
    const std::vector<point> & points() const;
    /** Returns values of all points. (computed on the fly, slow)*/
    std::vector<ValueType> values() const;
    /** Checks if a point is present in a grid. */
    bool check_point(point in, real_type tolerance = std::numeric_limits<real_type>::epsilon()) const;
    /** Returns size of grid. */
    size_t size() const;

    /** Returns the closest point to the given value. */
    point find_nearest(ValueType in) const;
    /** Get a value of an object at the given point, which is defined on a grid. */
    template <class Obj> auto eval(Obj &&in, point x) const ->decltype(in[0]);

    /** Shift a point by the given value. */
    point shift(point in, ValueType shift_arg) const;
    ValueType shift(ValueType in, ValueType shift_arg) const;
    point shift (point in, point shift_arg) const;

    // CRTP forwards: TODO: these will need to be cleaned for C++ and documented.
    /** Get a value of an object at the given coordinate, which is defined on a grid. */
    template <class Obj>
        auto eval(Obj &in, ValueType x) const ->decltype(in[0])
        { return static_cast<const Derived*>(this)->eval(in,x); };
    /// Make the object printable.
    template <typename ValType, class Derived2> friend std::ostream& operator<<(std::ostream& lhs, const grid_base<ValType,Derived2> &gr);
    /// Compare 2 grids
    bool operator==(const grid_base &rhs) const;

    class ex_wrong_index : public gftools_exception { public:
        ex_wrong_index(int i, int l):index_(i),l_(l),
        msg("grid_base : index " + std::to_string(index_) + " is out of bounds >" + std::to_string(l_) + ".")
        {}
        virtual const char* what() const throw(){return msg.c_str();}
        int index_; int l_;
        std::string msg;
    };

    class ex_not_found : public gftools_exception { public:
        ex_not_found(value_type x, grid_base const &y):x_(x), y_(y)
        {
            value_type b1(y_[0]), b2(y_[y_.size()-1]);
            msg = "grid_base : " + make_num_io(x_).to_string() + " is not found in the grid [" +
                  make_num_io(b1).to_string() + "; " + make_num_io(b2).to_string() + "].";
        }
        virtual const char* what() const throw(){return msg.c_str();}
        value_type x_;
        grid_base const& y_;
        std::string msg;
    };

protected:
    std::vector<point> vals_;
};

namespace extra {
/// A small helper struct to decorate a function object with [] method.
template <typename F, typename Grid>
struct function_proxy {
    F f_;
    const Grid& grid_;
    function_proxy(F f, Grid const& grid):f_(f),grid_(grid){}
    typedef typename std::result_of<F(typename Grid::value_type)>::type value_type;
    value_type operator[](int i) const {return f_((grid_.points()[i]).value()); }
};
}

//
// grid_base implementation
//

template <typename ValueType, class Derived>
grid_base<ValueType,Derived>::grid_base()
{};

template <typename ValueType, class Derived>
grid_base<ValueType,Derived>::grid_base(const std::vector<point> &vals):vals_(vals)
{
};

template <typename ValueType, class Derived>
grid_base<ValueType,Derived>::grid_base(const std::vector<ValueType> &vals)
{
    vals_.reserve(vals.size());
    for (size_t i=0; i<vals.size(); ++i) { vals_.emplace_back(point(vals[i], i)); };
};

template <typename ValueType, class Derived>
grid_base<ValueType,Derived>::grid_base(int min, int max, std::function<ValueType (int)> f)
{
    if (max<min) std::swap(min,max);
    size_t n_points = max-min;
    vals_.reserve(n_points);
    for (size_t i=0; i<n_points; ++i) vals_.emplace_back(f(min+i), i) ;
}

template <typename ValueType, class Derived>
inline typename grid_base<ValueType,Derived>::point grid_base<ValueType,Derived>::operator[](size_t index) const
{
    #ifndef NDEBUG
    if (index>vals_.size()) throw ex_wrong_index(index,this->size());
    #endif
    return vals_[index];
}

template <typename ValueType, class Derived>
inline const std::vector<typename grid_base<ValueType,Derived>::point> & grid_base<ValueType,Derived>::points() const
{
    return vals_;
}

template <typename ValueType, class Derived>
inline std::vector<ValueType> grid_base<ValueType,Derived>::values() const
{
    std::vector<ValueType> out;
    out.reserve(vals_.size());
    for (const auto& x : vals_) out.emplace_back(x.value());
    return out;
}

template <typename ValueType, class Derived>
inline size_t grid_base<ValueType,Derived>::size() const
{
    return vals_.size();
}

template <typename ValueType, class Derived>
inline bool grid_base<ValueType,Derived>::check_point(point in, real_type tolerance) const
{
    return (in.index() < vals_.size() && std::abs(in.value() - vals_[in.index()].value()) < tolerance);
}

template <typename ValueType, class Derived>
template <class Obj>
inline auto grid_base<ValueType,Derived>::eval(Obj &&in, point x) const ->decltype(in[0])
{
    if (check_point(x)) return in[x.index()];
    else throw ex_wrong_index(x.index(), this->size());
}

template <typename ValueType, class Derived>
inline typename grid_base<ValueType,Derived>::point grid_base<ValueType,Derived>::find_nearest(ValueType in) const
{
    static_assert(std::is_same<bool,decltype(std::declval<ValueType>() < std::declval<ValueType>())>::value,
        "Default find_nearest is written only for less-comparable types");
    auto nearest_iter = std::lower_bound(vals_.begin(), vals_.end(), in, [](ValueType x, ValueType y){return x<y;});
    size_t dist = std::distance(vals_.begin(), nearest_iter);
    if (dist > 0 && std::abs(complex_type(vals_[dist].value()) - complex_type(in)) > std::abs(complex_type(vals_[dist-1].value()) - complex_type(in)) ) dist--;
    return vals_[dist];
}

template <typename ValueType, class Derived>
inline typename grid_base<ValueType,Derived>::point grid_base<ValueType,Derived>::shift(point in, ValueType shift_arg) const
{
    if (almost_equal(shift_arg, 0.0)) return in;
    ValueType out(in.value());
    out = static_cast<const Derived*>(this)->shift(ValueType(in),shift_arg);
    point p1 = static_cast<const Derived*>(this)->find_nearest(out);
    if (!almost_equal(p1.value(), out, std::abs(p1.value() - ((p1.index()!=0)?vals_[p1.index() - 1]:vals_[p1.index()+1]).value())/10.)) {
#ifndef NDEBUG
      ERROR("Couldn't shift point" <<  in << " by " << shift_arg << " got " << out);
#endif
      throw gftools::ex_generic("grid_base::shift : Couldn't shift point");
    }
    else return p1;
}

template <typename ValueType, class Derived>
inline ValueType grid_base<ValueType,Derived>::shift(ValueType in, ValueType shift_arg) const
{
    return in+shift_arg;
}

template <typename ValueType, class Derived>
inline typename grid_base<ValueType,Derived>::point grid_base<ValueType,Derived>::shift(point in, point shift_arg) const
{
    size_t index = (in.index() + shift_arg.index())%vals_.size();
    #ifndef NDEBUG
    ValueType val = static_cast<const Derived*>(this)->shift(in.value(), shift_arg.value());
    if (!almost_equal(val, vals_[index].value())) throw gftools::ex_generic("grid_base::shift : almost equal failed.");
    #endif
    return vals_[index];

}

template <typename ValueType, class Derived>
inline bool grid_base<ValueType,Derived>::operator==(const grid_base &rhs) const
{
    bool out = (this->size() == rhs.size());
    for (size_t i=0; i<vals_.size() && out; i++) {
        out = out && almost_equal(vals_[i].value(), rhs.vals_[i].value(), num_io<double>::tolerance());
    }
    return out;
}

template <typename ValueType, class Derived>
std::ostream& operator<<(std::ostream& lhs, const grid_base<ValueType,Derived> &gr)
{
    lhs << "{";
    //lhs << gr.vals_;
    std::ostream_iterator<ValueType> out_it (lhs,", ");
    std::transform(gr.vals_.begin(),gr.vals_.end(), out_it, [](const typename grid_base<ValueType,Derived>::point &x){return ValueType(x);});
    lhs << "}";
    return lhs;
}

} // end :: namespace gftools


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 <mpi.h>
#include <iostream>
#include <rokko/rokko.hpp>
#include <rokko/collective.hpp>
#include <rokko/utility/frank_matrix.hpp>
#include <boost/lexical_cast.hpp>

typedef rokko::matrix_col_major matrix_major;
// typedef rokko::matrix_row_major matrix_major;

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  unsigned int dim = 10;
  std::string solver_name(rokko::parallel_dense_solver::default_solver());
  if (argc >= 2) solver_name = argv[1];
  if (argc >= 3) dim = boost::lexical_cast<unsigned int>(argv[2]);

  rokko::grid g;
  if (g.get_myrank() == 0) std::cout << "dimension = " << dim << std::endl;
  std::cout << std::flush;
  MPI_Barrier(g.get_comm());

  rokko::parallel_dense_solver solver(solver_name);
  solver.initialize(argc, argv);
  rokko::distributed_matrix<double, matrix_major> mat(dim, dim, g, solver);
  rokko::frank_matrix::generate(mat);
  mat.print();

  rokko::localized_matrix<double, matrix_major> lmat(dim, dim);
  for (int proc = 0; proc < g.get_nprocs(); ++proc) {
    rokko::gather(mat, lmat, proc);
    if (g.get_myrank() == proc) {
      std::cout << "root = " << proc << std::endl;
      std::cout << "lmat:" << std::endl << lmat << std::endl;
    }
    std::cout << std::flush;
    MPI_Barrier(g.get_comm());
  }

  solver.finalize();
  MPI_Finalize();
  return 0;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#include <iostream>

#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"
#include "pinocchio/spatial/spatial-axis.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_SE3 )
{
  using namespace pinocchio;
  typedef SE3::HomogeneousMatrixType HomogeneousMatrixType;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix<double,4,1> Vector4;

  const SE3 identity = SE3::Identity();

  typedef SE3::Quaternion Quaternion;
  typedef SE3::Vector4 Vector4;
  Quaternion quat(Vector4::Random().normalized());

  SE3 m_from_quat(quat,Vector3::Random());

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  HomogeneousMatrixType aMb = amb;
  HomogeneousMatrixType bMc = bmc;

  // Test internal product
  HomogeneousMatrixType aMc = amc;
  BOOST_CHECK(aMc.isApprox(aMb*bMc));

  HomogeneousMatrixType bMa = amb.inverse();
  BOOST_CHECK(bMa.isApprox(aMb.inverse()));

  // Test point action
  Vector3 p = Vector3::Random();
  Vector4 p4; p4.head(3) = p; p4[3] = 1;

  Vector3 Mp = (aMb*p4).head(3);
  BOOST_CHECK(amb.act(p).isApprox(Mp));

  Vector3 Mip = (aMb.inverse()*p4).head(3);
  BOOST_CHECK(amb.actInv(p).isApprox(Mip));

  // Test action matrix
  ActionMatrixType aXb = amb;
  ActionMatrixType bXc = bmc;
  ActionMatrixType aXc = amc;
  BOOST_CHECK(aXc.isApprox(aXb*bXc));

  ActionMatrixType bXa = amb.inverse();
  BOOST_CHECK(bXa.isApprox(aXb.inverse()));

  ActionMatrixType X_identity = identity.toActionMatrix();
  BOOST_CHECK(X_identity.isIdentity());

  ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse();
  BOOST_CHECK(X_identity_inverse.isIdentity());

  // Test dual action matrix
  BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix()));

  // Test isIdentity
  BOOST_CHECK(identity.isIdentity());

  // Test isApprox
  BOOST_CHECK(identity.isApprox(identity));

  // Test cast
  typedef SE3Tpl<float> SE3f;
  SE3f::Matrix3 rot_float(amb.rotation().cast<float>());
  SE3f amb_float = amb.cast<float>();
  BOOST_CHECK(amb_float.isApprox(amb.cast<float>()));

  // Test actInv
  const SE3 M = SE3::Random();
  const SE3 Minv = M.inverse();

  BOOST_CHECK(Minv.actInv(Minv).isIdentity());
  BOOST_CHECK(M.actInv(identity).isApprox(Minv));
}

BOOST_AUTO_TEST_CASE ( test_Motion )
{
  using namespace pinocchio;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef Motion::Vector6 Vector6;

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  Motion bv = Motion::Random();
  Motion bv2 = Motion::Random();

  typedef MotionBase<Motion> Base;

  Vector6 bv_vec = bv;
  Vector6 bv2_vec = bv2;

  // std::stringstream
  std::stringstream ss;
  ss << bv << std::endl;
  BOOST_CHECK(!ss.str().empty());

  // Test .+.
  Vector6 bvPbv2_vec = bv+bv2;
  BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec+bv2_vec));

  Motion bplus = static_cast<Base &>(bv) + static_cast<Base &>(bv2);
  BOOST_CHECK((bv+bv2).isApprox(bplus));

  Motion v_not_zero(Vector6::Ones());
  BOOST_CHECK(!v_not_zero.isZero());

  Motion v_zero(Vector6::Zero());
  BOOST_CHECK(v_zero.isZero());

  // Test == and !=
  BOOST_CHECK(bv == bv);
  BOOST_CHECK(!(bv != bv));

  // Test -.
  Vector6 Mbv_vec = -bv;
  BOOST_CHECK( Mbv_vec.isApprox(-bv_vec));

  // Test .+=.
  Motion bv3 = bv; bv3 += bv2;
  BOOST_CHECK( bv3.toVector().isApprox(bv_vec+bv2_vec));

  // Test .=V6
  bv3 = bv2_vec;
  BOOST_CHECK( bv3.toVector().isApprox(bv2_vec));

  // Test scalar*M6
  Motion twicebv(2.*bv);
  BOOST_CHECK(twicebv.isApprox(Motion(2.*bv.toVector())));

  // Test M6*scalar
  Motion bvtwice(bv*2.);
  BOOST_CHECK(bvtwice.isApprox(twicebv));

  // Test M6/scalar
  Motion bvdividedbytwo(bvtwice/2.);
  BOOST_CHECK(bvdividedbytwo.isApprox(bv));

  // Test constructor from V6
  Motion bv4(bv2_vec);
  BOOST_CHECK( bv4.toVector().isApprox(bv2_vec));

  // Test action
  ActionMatrixType aXb = amb;
  BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb*bv_vec));

  // Test action inverse
  ActionMatrixType bXc = bmc;
  BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));

  // Test double action
  Motion cv = Motion::Random();
  bv = bmc.act(cv);
  BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector()));

  // Simple test for cross product vxv
  Motion vxv = bv.cross(bv);
  BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));

  // Test Action Matrix
  Motion v2xv = bv2.cross(bv);
  Motion::ActionMatrixType actv2 = bv2.toActionMatrix();

  BOOST_CHECK(v2xv.toVector().isApprox(actv2*bv.toVector()));

  // Test Dual Action Matrix
  Force f(bv.toVector());
  Force v2xf = bv2.cross(f);
  Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix();

  BOOST_CHECK(v2xf.toVector().isApprox(dualactv2*f.toVector()));
  BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));

  // Simple test for cross product vxf
  Force vxf = bv.cross(f);
  BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear())));
  BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));

  // Test frame change for vxf
  Motion av = Motion::Random();
  Force af = Force::Random();
  bv = amb.actInv(av);
  Force bf = amb.actInv(af);
  Force avxf = av.cross(af);
  Force bvxf = bv.cross(bf);
  BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector()));

  // Test frame change for vxv
  av = Motion::Random();
  Motion aw = Motion::Random();
  bv = amb.actInv(av);
  Motion bw = amb.actInv(aw);
  Motion avxw = av.cross(aw);
  Motion bvxw = bv.cross(bw);
  BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector()));

  // Test isApprox
  bv.toVector().setOnes();
  const double eps = 1e-6;
  BOOST_CHECK(bv == bv);
  BOOST_CHECK(bv.isApprox(bv));
  Motion bv_approx(bv);
  bv_approx.linear()[0] += eps;
  BOOST_CHECK(bv_approx.isApprox(bv,eps));

  // Test ref() method
  {
    Motion a(Motion::Random());
    BOOST_CHECK(a.ref().isApprox(a));

    const Motion b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }

  // Test cast
  {
    typedef MotionTpl<float> Motionf;
    Motion a(Motion::Random());
    Motionf a_float = a.cast<float>();
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
  }
}

BOOST_AUTO_TEST_CASE (test_motion_ref)
{
  using namespace pinocchio;
  typedef Motion::Vector6 Vector6;

  typedef MotionRef<Vector6> MotionV6;

  Motion v_ref(Motion::Random());
  MotionV6 v(v_ref.toVector());

  BOOST_CHECK(v_ref.isApprox(v));

  MotionV6::MotionPlain v2(v*2.);
  Motion v2_ref(v_ref*2.);

  BOOST_CHECK(v2_ref.isApprox(v2));

  v2 = v_ref + v;
  BOOST_CHECK(v2_ref.isApprox(v2));

  v = v2;
  BOOST_CHECK(v2.isApprox(v));

  v2 = v - v;
  BOOST_CHECK(v2.isApprox(Motion::Zero()));

  SE3 M(SE3::Identity());
  v2 = M.act(v);
  BOOST_CHECK(v2.isApprox(v));

  v2 = M.actInv(v);
  BOOST_CHECK(v2.isApprox(v));

  Motion v3(Motion::Random());
  v_ref.setRandom();
  v = v_ref;
  v2 = v.cross(v3);
  v2_ref = v_ref.cross(v3);

  BOOST_CHECK(v2.isApprox(v2_ref));

  v.setRandom();
  v.setZero();
  BOOST_CHECK(v.isApprox(Motion::Zero()));

  // Test ref() method
  {
    Vector6 v6(Vector6::Random());
    MotionV6 a(v6);
    BOOST_CHECK(a.ref().isApprox(a));

    const Motion b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }

}

BOOST_AUTO_TEST_CASE(test_motion_zero)
{
  using namespace pinocchio;
  Motion v((MotionZero()));

  BOOST_CHECK(v.toVector().isZero());
  BOOST_CHECK(MotionZero() == Motion::Zero());

  // SE3.act
  SE3 m(SE3::Random());
  BOOST_CHECK(m.act(MotionZero()) == Motion::Zero());
  BOOST_CHECK(m.actInv(MotionZero()) == Motion::Zero());

  // Motion.cross
  Motion v2(Motion::Random());
  BOOST_CHECK(v2.cross(MotionZero()) == Motion::Zero());
}

BOOST_AUTO_TEST_CASE ( test_Force )
{
  using namespace pinocchio;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef Force::Vector6 Vector6;

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  Force bf = Force::Random();
  Force bf2 = Force::Random();

  Vector6 bf_vec = bf;
  Vector6 bf2_vec = bf2;

  // std::stringstream
  std::stringstream ss;
  ss << bf << std::endl;
  BOOST_CHECK(!ss.str().empty());

  // Test .+.
  Vector6 bfPbf2_vec = bf+bf2;
  BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec+bf2_vec));

  // Test -.
  Vector6 Mbf_vec = -bf;
  BOOST_CHECK(Mbf_vec.isApprox(-bf_vec));

  // Test .+=.
  Force bf3 = bf; bf3 += bf2;
  BOOST_CHECK(bf3.toVector().isApprox(bf_vec+bf2_vec));

  // Test .= V6
  bf3 = bf2_vec;
  BOOST_CHECK(bf3.toVector().isApprox(bf2_vec));

  // Test constructor from V6
  Force bf4(bf2_vec);
  BOOST_CHECK(bf4.toVector().isApprox(bf2_vec));

  // Test action
  ActionMatrixType aXb = amb;
  BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));

  // Test action inverse
  ActionMatrixType bXc = bmc;
  BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));

  // Test double action
  Force cf = Force::Random();
  bf = bmc.act(cf);
  BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector()));

  // Simple test for cross product
  // Force vxv = bf.cross(bf);
  // ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector()));

  Force f_not_zero(Vector6::Ones());
  BOOST_CHECK(!f_not_zero.isZero());

  Force f_zero(Vector6::Zero());
  BOOST_CHECK(f_zero.isZero());

  // Test isApprox

  BOOST_CHECK(bf == bf);
  bf.setRandom();
  bf2.setZero();
  BOOST_CHECK(bf == bf);
  BOOST_CHECK(bf != bf2);
  BOOST_CHECK(bf.isApprox(bf));
  BOOST_CHECK(!bf.isApprox(bf2));

  const double eps = 1e-6;
  Force bf_approx(bf);
  bf_approx.linear()[0] += eps/2.;
  BOOST_CHECK(bf_approx.isApprox(bf,eps));

  // Test ref() method
  {
    Force a(Force::Random());
    BOOST_CHECK(a.ref().isApprox(a));

    const Force b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }

  // Test cast
  {
    typedef ForceTpl<float> Forcef;
    Force a(Force::Random());
    Forcef a_float = a.cast<float>();
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
  }

  // Test scalar multiplication
  const double alpha = 1.5;
  Force b(Force::Random());
  Force alpha_f = alpha * b;
  Force f_alpha = b * alpha;

  BOOST_CHECK(alpha_f == f_alpha);
}

BOOST_AUTO_TEST_CASE (test_force_ref)
{
  using namespace pinocchio;
  typedef Force::Vector6 Vector6;

  typedef ForceRef<Vector6> ForceV6;

  Force f_ref(Force::Random());
  ForceV6 f(f_ref.toVector());

  BOOST_CHECK(f_ref.isApprox(f));

  ForceV6::ForcePlain f2(f*2.);
  Force f2_ref(f_ref*2.);

  BOOST_CHECK(f2_ref.isApprox(f2));

  f2 = f_ref + f;
  BOOST_CHECK(f2_ref.isApprox(f2));

  f = f2;
  BOOST_CHECK(f2.isApprox(f));

  f2 = f - f;
  BOOST_CHECK(f2.isApprox(Force::Zero()));

  SE3 M(SE3::Identity());
  f2 = M.act(f);
  BOOST_CHECK(f2.isApprox(f));

  f2 = M.actInv(f);
  BOOST_CHECK(f2.isApprox(f));

  Motion v(Motion::Random());
  f_ref.setRandom();
  f = f_ref;
  f2 = v.cross(f);
  f2_ref = v.cross(f_ref);

  BOOST_CHECK(f2.isApprox(f2_ref));

  f.setRandom();
  f.setZero();
  BOOST_CHECK(f.isApprox(Force::Zero()));

  // Test ref() method
  {
    Vector6 v6(Vector6::Random());
    ForceV6 a(v6);
    BOOST_CHECK(a.ref().isApprox(a));

    const Force b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }
}

BOOST_AUTO_TEST_CASE ( test_Inertia )
{
  using namespace pinocchio;
  typedef Inertia::Matrix6 Matrix6;

  Inertia aI = Inertia::Random();
  Matrix6 matI = aI;
  BOOST_CHECK_EQUAL(matI(0,0), aI.mass());
  BOOST_CHECK_EQUAL(matI(1,1), aI.mass());
  BOOST_CHECK_EQUAL(matI(2,2), aI.mass()); // 1,1 before unifying

  BOOST_CHECK_SMALL((matI-matI.transpose()).norm(),matI.norm()); //previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) );
  BOOST_CHECK_SMALL((matI.topRightCorner<3,3>()*aI.lever()).norm(),
            aI.lever().norm()); //previously ensure that( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );

  Inertia I1 = Inertia::Identity();
  BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity()));

  // Test motion-to-force map
  Motion v = Motion::Random();
  Force f = I1 * v;
  BOOST_CHECK(f.toVector().isApprox(v.toVector()));

  // Test Inertia group application
  SE3 bma = SE3::Random();
  Inertia bI = bma.act(aI);
  Matrix6 bXa = bma;
  BOOST_CHECK((bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose())
                               .isApprox(bI.inertia().matrix()));
  BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse())
                              .isApprox(bI.matrix()));

  // Test inverse action
  BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa)
                              .isApprox(bma.actInv(bI).matrix()));

  // Test vxIv cross product
  v = Motion::Random();
  f = aI*v;
  Force vxf = v.cross(f);
  Force vxIv = aI.vxiv(v);
  BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector()));

  // Test operator+
  I1 = Inertia::Random();
  Inertia I2 = Inertia::Random();
  BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix()));

  // operator +=
  Inertia I12 = I1;
  I12 += I2;
  BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12.matrix()));

  // Test operator vtiv
  double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
  double kinetic = aI.vtiv(v);
  BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);

  // Test constructor (Matrix6)
  Inertia I1_bis(I1.matrix());
  BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix()));

  // Test Inertia from ellipsoid
  I1 = Inertia::FromEllipsoid(2., 3., 4., 5.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
          16.4, 0., 13.6, 0., 0., 10.).matrix()));

  // Test Inertia from Cylinder
  I1 = Inertia::FromCylinder(2., 4., 6.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
        14., 0., 14., 0., 0., 16.).matrix()));

  // Test Inertia from Box
  I1 = Inertia::FromBox(2., 6., 12., 18.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
        78., 0., 60., 0., 0., 30.).matrix()));

  // Copy operator
  Inertia aI_copy(aI);
  BOOST_CHECK(aI_copy == aI);

  // Test isZero
  Inertia I_not_zero = Inertia::Identity();
  BOOST_CHECK(!I_not_zero.isZero());

  Inertia I_zero = Inertia::Zero();
  BOOST_CHECK(I_zero.isZero());

  // Test isApprox
  const double eps = 1e-6;
  BOOST_CHECK(aI == aI);
  BOOST_CHECK(aI.isApprox(aI));
  Inertia aI_approx(aI);
  aI_approx.mass() += eps/2.;
  BOOST_CHECK(aI_approx.isApprox(aI,eps));

  // Test Variation
  Inertia::Matrix6 aIvariation = aI.variation(v);

  Motion::ActionMatrixType vAction = v.toActionMatrix();
  Motion::ActionMatrixType vDualAction = v.toDualActionMatrix();

  Inertia::Matrix6 aImatrix = aI.matrix();
  Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;

  BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
  BOOST_CHECK(vxIv.isApprox(Force(aIvariation*v.toVector())));

  // Test vxI operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());

    const Matrix6 M_ref(v.toDualActionMatrix()*I.matrix());
    Matrix6 M; Inertia::vxi(v,I,M);

    BOOST_CHECK(M.isApprox(M_ref));
    BOOST_CHECK(I.vxi(v).isApprox(M_ref));
  }

  // Test Ivx operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());

    const Matrix6 M_ref(I.matrix()*v.toActionMatrix());
    Matrix6 M; Inertia::ivx(v,I,M);

    BOOST_CHECK(M.isApprox(M_ref));
    BOOST_CHECK(I.ivx(v).isApprox(M_ref));
  }

  // Test variation against vxI - Ivx operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());

    Matrix6 Ivariation = I.variation(v);

    Matrix6 M1; Inertia::vxi(v,I,M1);
    Matrix6 M2; Inertia::ivx(v,I,M2);
    Matrix6 M3(M1-M2);

    BOOST_CHECK(M3.isApprox(Ivariation));
  }

  // Test dynamic parameters
  {
    Inertia I(Inertia::Random());

    Inertia::Vector10 v = I.toDynamicParameters();

    BOOST_CHECK_CLOSE(v[0], I.mass(), 1e-12);

    BOOST_CHECK(v.segment<3>(1).isApprox(I.mass()*I.lever()));

    Eigen::Matrix3d I_o = I.inertia() + I.mass()*skew(I.lever()).transpose()*skew(I.lever());
    Eigen::Matrix3d I_ov;
    I_ov << v[4], v[5], v[7],
            v[5], v[6], v[8],
            v[7], v[8], v[9];

    BOOST_CHECK(I_o.isApprox(I_ov));

    Inertia I2 = Inertia::FromDynamicParameters(v);
    BOOST_CHECK(I2.isApprox(I));

  }

}

BOOST_AUTO_TEST_CASE(cast_inertia)
{
  using namespace pinocchio;
  Inertia Y(Inertia::Random());

  BOOST_CHECK(Y.cast<double>() == Y);
  BOOST_CHECK(Y.cast<long double>().cast<double>() == Y);
}

BOOST_AUTO_TEST_CASE ( test_ActOnSet )
{
  using namespace pinocchio;
  const int N = 20;
  typedef Eigen::Matrix<double,6,N> Matrix6N;
  SE3 jMi = SE3::Random();
  Motion v = Motion::Random();

  // Forcet SET
  Matrix6N iF = Matrix6N::Random(),jF,jFinv,jF_ref,jFinv_ref;

  // forceSet::se3Action
  forceSet::se3Action(jMi,iF,jF);
  for( int k=0;k<N;++k )
    BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k)));

  jF_ref = jMi.toDualActionMatrix()*iF;
  BOOST_CHECK(jF_ref.isApprox(jF));

  forceSet::se3ActionInverse(jMi.inverse(),iF,jFinv);
  BOOST_CHECK(jFinv.isApprox(jF));

  Matrix6N iF2 = Matrix6N::Random();
  jF_ref += jMi.toDualActionMatrix() * iF2;

  forceSet::se3Action<ADDTO>(jMi,iF2,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));

  Matrix6N iF3 = Matrix6N::Random();
  jF_ref -= jMi.toDualActionMatrix() * iF3;

  forceSet::se3Action<RMTO>(jMi,iF3,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));

  // forceSet::se3ActionInverse
  forceSet::se3ActionInverse(jMi,iF,jFinv);
  jFinv_ref = jMi.inverse().toDualActionMatrix() * iF;
  BOOST_CHECK(jFinv_ref.isApprox(jFinv));

  jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2;
  forceSet::se3ActionInverse<ADDTO>(jMi,iF2,jFinv);
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));

  jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3;
  forceSet::se3ActionInverse<RMTO>(jMi,iF3,jFinv);
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));

  // forceSet::motionAction
  forceSet::motionAction(v,iF,jF);
  for( int k=0;k<N;++k )
    BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k)));

  jF_ref = v.toDualActionMatrix() * iF;
  BOOST_CHECK(jF.isApprox(jF_ref));

  jF_ref += v.toDualActionMatrix() * iF2;
  forceSet::motionAction<ADDTO>(v,iF2,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));

  jF_ref -= v.toDualActionMatrix() * iF3;
  forceSet::motionAction<RMTO>(v,iF3,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));

  // Motion SET
  Matrix6N iV = Matrix6N::Random(),jV,jV_ref,jVinv,jVinv_ref;

  // motionSet::se3Action
  motionSet::se3Action(jMi,iV,jV);
  for( int k=0;k<N;++k )
    BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));

  jV_ref = jMi.toActionMatrix()*iV;
  BOOST_CHECK(jV.isApprox(jV_ref));

  motionSet::se3ActionInverse(jMi.inverse(),iV,jVinv);
  BOOST_CHECK(jVinv.isApprox(jV));

  Matrix6N iV2 = Matrix6N::Random();
  jV_ref += jMi.toActionMatrix()*iV2;
  motionSet::se3Action<ADDTO>(jMi,iV2,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  Matrix6N iV3 = Matrix6N::Random();
  jV_ref -= jMi.toActionMatrix()*iV3;
  motionSet::se3Action<RMTO>(jMi,iV3,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  // motionSet::se3ActionInverse
  motionSet::se3ActionInverse(jMi,iV,jVinv);
  jVinv_ref = jMi.inverse().toActionMatrix() * iV;
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));

  jVinv_ref += jMi.inverse().toActionMatrix()*iV2;
  motionSet::se3ActionInverse<ADDTO>(jMi,iV2,jVinv);
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));

  jVinv_ref -= jMi.inverse().toActionMatrix()*iV3;
  motionSet::se3ActionInverse<RMTO>(jMi,iV3,jVinv);
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));

  // motionSet::motionAction
  motionSet::motionAction(v,iV,jV);
  for( int k=0;k<N;++k )
    BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));

  jV_ref = v.toActionMatrix()*iV;
  BOOST_CHECK(jV.isApprox(jV_ref));

  jV_ref += v.toActionMatrix()*iV2;
  motionSet::motionAction<ADDTO>(v,iV2,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  jV_ref -= v.toActionMatrix()*iV3;
  motionSet::motionAction<RMTO>(v,iV3,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  // motionSet::inertiaAction
  const Inertia I(Inertia::Random());
  motionSet::inertiaAction(I,iV,jV);
  for( int k=0;k<N;++k )
    BOOST_CHECK((I*(Motion(iV.col(k)))).toVector().isApprox(jV.col(k)));

  jV_ref = I.matrix()*iV;
  BOOST_CHECK(jV.isApprox(jV_ref));

  jV_ref += I.matrix()*iV2;
  motionSet::inertiaAction<ADDTO>(I,iV2,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  jV_ref -= I.matrix()*iV3;
  motionSet::inertiaAction<RMTO>(I,iV3,jV);
  BOOST_CHECK(jV.isApprox(jV_ref));

  // motionSet::act
  Force f = Force::Random();
  motionSet::act(iV,f,jF);
  for( int k=0;k<N;++k )
    BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k)));

  for( int k=0;k<N;++k )
    jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector();
  BOOST_CHECK(jF.isApprox(jF_ref));

  for( int k=0;k<N;++k )
    jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector();
  motionSet::act<ADDTO>(iV2,f,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));

  for( int k=0;k<N;++k )
    jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector();
  motionSet::act<RMTO>(iV3,f,jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
}

BOOST_AUTO_TEST_CASE(test_skew)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Motion::Vector6 Vector6;

  Vector3 v3(Vector3::Random());
  Vector6 v6(Vector6::Random());

  Vector3 res1 = unSkew(skew(v3));
  BOOST_CHECK(res1.isApprox(v3));

  Vector3 res2 = unSkew(skew(v6.head<3>()));
  BOOST_CHECK(res2.isApprox(v6.head<3>()));

  Vector3 res3 = skew(v3)*v3;
  BOOST_CHECK(res3.isZero());

  Vector3 rhs(Vector3::Random());
  Vector3 res41 = skew(v3)*rhs;
  Vector3 res42 = v3.cross(rhs);

  BOOST_CHECK(res41.isApprox(res42));

}

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

  Vector3 v(Vector3::Random());
  Matrix3 M(Matrix3::Random());
  Matrix3 Mcopy(M);

  addSkew(v,M);
  Matrix3 Mref = Mcopy + skew(v);
  BOOST_CHECK(M.isApprox(Mref));

  Mref += skew(-v);
  addSkew(-v,M);
  BOOST_CHECK(M.isApprox(Mcopy));

  M.setZero();
  addSkew(v,M);
  BOOST_CHECK(M.isApprox(skew(v)));
}

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

  Vector3 u(Vector3::Random());
  Vector3 v(Vector3::Random());

  Matrix3 ref = skew(u) * skew(v);

  Matrix3 res = skewSquare(u,v);

  BOOST_CHECK(res.isApprox(ref));
}

template<int axis>
struct test_scalar_multiplication_cartesian_axis
{
  typedef pinocchio::CartesianAxis<axis> Axis;
  typedef double Scalar;
  typedef Eigen::Matrix<Scalar,3,1> Vector3;

  static void run()
  {
    const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX);
    const Vector3 r1 = Axis() * alpha;
    const Vector3 r2 = alpha * Axis();

    BOOST_CHECK(r1.isApprox(r2));

    for(int k = 0; k < Axis::dim; ++k)
    {
      if(k==axis)
      {
        BOOST_CHECK(r1[k] == alpha);
        BOOST_CHECK(r2[k] == alpha);
      }
      else
      {
        BOOST_CHECK(r1[k] == Scalar(0));
        BOOST_CHECK(r2[k] == Scalar(0));
      }
    }
  }
};

BOOST_AUTO_TEST_CASE(test_cartesian_axis)
{
  using namespace Eigen;
  using namespace pinocchio;
  Vector3d v(Vector3d::Random());
  const double alpha = 3;
  Vector3d v2(alpha*v);

  BOOST_CHECK(AxisX::cross(v).isApprox(Vector3d::Unit(0).cross(v)));
  BOOST_CHECK(AxisY::cross(v).isApprox(Vector3d::Unit(1).cross(v)));
  BOOST_CHECK(AxisZ::cross(v).isApprox(Vector3d::Unit(2).cross(v)));
  BOOST_CHECK(AxisX::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2)));
  BOOST_CHECK(AxisY::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2)));
  BOOST_CHECK(AxisZ::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2)));

  test_scalar_multiplication_cartesian_axis<0>::run();
  test_scalar_multiplication_cartesian_axis<1>::run();
  test_scalar_multiplication_cartesian_axis<2>::run();
}

template<int axis>
struct test_scalar_multiplication
{
  typedef pinocchio::SpatialAxis<axis> Axis;
  typedef double Scalar;
  typedef pinocchio::MotionTpl<Scalar> Motion;

  static void run()
  {
    const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX);
    const Motion r1 = Axis() * alpha;
    const Motion r2 = alpha * Axis();

    BOOST_CHECK(r1.isApprox(r2));

    for(int k = 0; k < Axis::dim; ++k)
    {
      if(k==axis)
      {
        BOOST_CHECK(r1.toVector()[k] == alpha);
        BOOST_CHECK(r2.toVector()[k] == alpha);
      }
      else
      {
        BOOST_CHECK(r1.toVector()[k] == Scalar(0));
        BOOST_CHECK(r2.toVector()[k] == Scalar(0));
      }
    }
  }
};

BOOST_AUTO_TEST_CASE(test_spatial_axis)
{
  using namespace pinocchio;

  Motion v(Motion::Random());
  Force f(Force::Random());

  Motion vaxis;
  vaxis << AxisVX();
  BOOST_CHECK(AxisVX::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVX()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVX::cross(f).isApprox(vaxis.cross(f)));

  vaxis << AxisVY();
  BOOST_CHECK(AxisVY::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVY()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVY::cross(f).isApprox(vaxis.cross(f)));

  vaxis << AxisVZ();
  BOOST_CHECK(AxisVZ::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVZ()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVZ::cross(f).isApprox(vaxis.cross(f)));

  vaxis << AxisWX();
  BOOST_CHECK(AxisWX::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWX()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWX::cross(f).isApprox(vaxis.cross(f)));

  vaxis << AxisWY();
  BOOST_CHECK(AxisWY::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWY()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWY::cross(f).isApprox(vaxis.cross(f)));

  vaxis << AxisWZ();
  BOOST_CHECK(AxisWZ::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWZ()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWZ::cross(f).isApprox(vaxis.cross(f)));

  // Test operation Axis * Scalar
  test_scalar_multiplication<0>::run();
  test_scalar_multiplication<1>::run();
  test_scalar_multiplication<2>::run();
  test_scalar_multiplication<3>::run();
  test_scalar_multiplication<4>::run();
  test_scalar_multiplication<5>::run();

  // Operations of Constraint on forces Sxf
  typedef Motion::ActionMatrixType ActionMatrixType;
  typedef ActionMatrixType::ColXpr ColType;
  typedef ForceRef<ColType> ForceRefOnColType;
  typedef MotionRef<ColType> MotionRefOnColType;
  ActionMatrixType Sxf,Sxf_ref;
  ActionMatrixType S(ActionMatrixType::Identity());

  SpatialAxis<0>::cross(f,ForceRefOnColType(Sxf.col(0)));
  SpatialAxis<1>::cross(f,ForceRefOnColType(Sxf.col(1)));
  SpatialAxis<2>::cross(f,ForceRefOnColType(Sxf.col(2)));
  SpatialAxis<3>::cross(f,ForceRefOnColType(Sxf.col(3)));
  SpatialAxis<4>::cross(f,ForceRefOnColType(Sxf.col(4)));
  SpatialAxis<5>::cross(f,ForceRefOnColType(Sxf.col(5)));

  for(int k = 0; k < 6; ++k)
  {
    MotionRefOnColType Scol(S.col(k));
    ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(f);
  }

  BOOST_CHECK(Sxf.isApprox(Sxf_ref));
}

BOOST_AUTO_TEST_SUITE_END ()


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

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <simple_moveit_wrapper/industrial_robot.h>
#include "arf_trajectory/trajectory.h"
#include "arf_graph/graph.h"
#include "arf_graph/util.h"

namespace rvt = rviz_visual_tools;
namespace smw = simple_moveit_wrapper;

class Rviz
{
public:
  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
  Rviz()
  {
    visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("base_link", "/visualization_marker_array"));
    visual_tools_->loadRobotStatePub("/display_robot_state");
  }

  void plotPose(Eigen::Affine3d pose);
  void clear();
};

class Demo1
{
  std::vector<std::vector<std::vector<double>>> graph_data_;
  std::vector<arf::TrajectoryPoint> ee_trajectory_;
  std::vector<std::vector<double>> shortest_path_;

public:
  void createAndShowTrajectory(Rviz& rviz);
  void createGraphData(smw::Robot& robot);
  void calculateShortestPath(smw::Robot& robot);
  void showShortestPath(smw::Robot& robot, Rviz& rviz);
};

std::vector<arf::TrajectoryPoint> createPath();

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_moveit_wrapper");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(2);
  spinner.start();

  // arf::Robot robot;
  smw::IndustrialRobot robot;
  Rviz rviz;
  Demo1 demo1;
  demo1.createAndShowTrajectory(rviz);
  demo1.createGraphData(robot);
  demo1.calculateShortestPath(robot);
  demo1.showShortestPath(robot, rviz);
  ros::shutdown();
  return 0;
}

std::vector<arf::TrajectoryPoint> createPath()
{
  const int num_path_points = 5;
  const std::vector<double> p1 = { 1, 0, 0.4 };
  const std::vector<double> p2 = { 1.5, 0, 0.5 };

  std::vector<arf::TrajectoryPoint> path;

  double s = 0;
  double ds = 1 / (num_path_points - 1);
  for (int i = 0; i < num_path_points; ++i)
  {
    arf::Number x(p1[0] * (1 - s) + p2[0] * s);
    arf::Number y(p1[1] * (1 - s) + p2[1] * s);
    arf::Number z(p1[2] * (1 - s) + p2[2] * s);
    arf::Number rx(0), ry(M_2_PI), rz(0);
    arf::TrajectoryPoint tp(x, y, z, rx, ry, rz);
    path.push_back(tp);
    s += ds;
  }
  return path;
}

void Rviz::plotPose(Eigen::Affine3d pose)
{
  Eigen::Isometry3d pose_temp(pose.matrix());
  visual_tools_->publishAxis(pose_temp, rvt::LARGE);
  visual_tools_->trigger();
  ros::Duration(0.1).sleep();
}

void Rviz::clear()
{
  visual_tools_->deleteAllMarkers();
  visual_tools_->trigger();
  ros::Duration(0.1).sleep();
}

void Demo1::createAndShowTrajectory(Rviz& rviz)
{
  for (int i = 0; i < 10; ++i)
  {
    arf::TolerancedNumber x(0.5, 0.45, 0.55, 5);
    arf::Number y, z(0.5 + static_cast<double>(i) / 20);
    arf::Number rx, ry(M_PI_2), rz;
    arf::TrajectoryPoint tp(x, y, z, rx, ry, rz);
    ee_trajectory_.push_back(tp);
  }
  for (auto tp : ee_trajectory_)
  {
    rviz.plotPose(tp.getNominalPose());
    // tp.plot(rviz.visual_tools_);
  }
}

void Demo1::createGraphData(smw::Robot& robot)
{
  for (auto tp : ee_trajectory_)
  {
    std::vector<std::vector<double>> new_data;
    for (auto pose : tp.getGridSamples())
    {
      for (auto q_sol : robot.ik(pose))
      {
        if (!robot.isColliding(q_sol))
          new_data.push_back(q_sol);
      }
    }
    graph_data_.push_back(new_data);
  }
}

void Demo1::calculateShortestPath(smw::Robot& robot)
{
  arf::Graph demo_graph(graph_data_);
  demo_graph.runMultiSourceDijkstra();
  std::vector<arf::Node*> sp = demo_graph.getShortestPath();
  std::cout << "Shortest path \n";
  for (auto node : sp)
  {
    std::cout << (*node) << std::endl;
    shortest_path_.push_back(*(*node).jv);
  }
}

void Demo1::showShortestPath(smw::Robot& robot, Rviz& rviz)
{
  for (auto q : shortest_path_)
  {
    robot.plot(rviz.visual_tools_, q);
    ros::Duration(0.5).sleep();
  }
}


/// 786

/// This file is subject to the terms and conditions defined in
/// file 'LICENSE', which is part of this source code package.

/// Author: inumanag

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

#include <time.h>
#include <sstream>
#include <unordered_map>

#include <sys/types.h>
#include <sys/stat.h>

#include <boost/math/tools/roots.hpp>
#include <boost/math/distributions/binomial.hpp>

#include "common.h"

using namespace std;

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

mode_t stat_file(const string &path)
{
	struct stat path_stat;
	int s = stat(path.c_str(), &path_stat);
	assert(s == 0);
	return path_stat.st_mode;
}

vector<string> split(const string &s, char delim)
{
	vector<string> elems;
	stringstream ss(s);
	string item;
	while(getline(ss, item, delim)) {
		elems.push_back(item);
	}
	return elems;
}

string rc(const string &s)
{
	auto r = s;
	reverse(r.begin(), r.end());
	transform(r.begin(), r.end(), r.begin(), rev_dna);
	return r;
}

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

double tau(double edit_error, int kmer_size)
{
	const double ERROR_RATIO = (Globals::Search::MAX_ERROR - Globals::Search::MAX_EDIT_ERROR) / Globals::Search::MAX_EDIT_ERROR;
   double gap_error = std::min(1.0, ERROR_RATIO * edit_error);
   double a = (1 - gap_error) / (1 + gap_error);
   double b = 1 / (2 * std::exp(kmer_size * edit_error) - 1);
   return a * b;
}

double solve_inverse_jaccard(int j, int kmer_size)
{
	if (j == 0) return 1;
	if (j == 1) return 0;
	return boost::math::tools::newton_raphson_iterate([j, kmer_size](double d){
		const double ERROR_RATIO = (Globals::Search::MAX_ERROR - Globals::Search::MAX_EDIT_ERROR) / Globals::Search::MAX_EDIT_ERROR;
		double E = exp(d * kmer_size);
		return make_tuple(
			((1 - d * ERROR_RATIO) / (1 + d * ERROR_RATIO)) * (1.0 / (2 * E - 1)) - j,
			2 * (- kmer_size * E + ERROR_RATIO - 2 * ERROR_RATIO * E + E * kmer_size * pow(d * ERROR_RATIO, 2)) /
				pow((2 * E - 1) * (1 + d * ERROR_RATIO), 2)
		);
	}, 0.10, 0.0, 1.0, numeric_limits<double>::digits);
}

int relaxed_jaccard_estimate(int s, int kmer_size, unordered_map<int, int> &mm)
{
	double result = -1;
	auto it = mm.find(s);
	if (it != mm.end()) result = it->second;
	if (result != -1) return result;

	using namespace boost::math;
	const double CI = 0.75;
	const double Q2 = (1.0 - CI) / 2; // one side interval probability

	result = ceil(s * tau(Globals::Search::MAX_EDIT_ERROR, kmer_size));
	for (; result >= 0; result--) {
		double d = solve_inverse_jaccard(result / s, kmer_size); // returns edit error
		double x = quantile(complement(binomial(s, tau(d, kmer_size)), Q2)); // inverse binomial
		double low_d = solve_inverse_jaccard(x / s, kmer_size);
		if (100 * (1 - low_d) < Globals::Search::MAX_EDIT_ERROR) {
			result++;
			break;
		}
	}
	result = max(result, 0.0);
	mm[s] = result;
	return result;
}