//
// Copyright (c) 2018 Sho Hirose
//
// Distributed under the MIT License. (See accompanying file LICENSE.txt or copy
// at https://opensource.org/licenses/MIT)
//
#ifndef IS_FIXED_SIZE_VECTORIZABLE_HPP
#define IS_FIXED_SIZE_VECTORIZABLE_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <type_traits>

namespace foo {

/// Checks wether if T is an Eigen object of fixed-size vectorizable type.
/// Please refer to <a
/// href=http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html>
/// Fixed-size vectorizable Eigen objects </a>
template <typename T>
struct is_fixed_size_vectorizable : std::false_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Vector2d> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Vector4d> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Matrix2d> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Matrix2f> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Matrix4d> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Matrix4f> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Affine3d> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Affine3f> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Quaterniond> : std::true_type {};

template <>
struct is_fixed_size_vectorizable<Eigen::Quaternionf> : std::true_type {};

}  // end namespace foo

#endif


//==============================================================================
//         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
//==============================================================================
#ifndef NT2_INTEGRATION_FUNCTIONS_COMMON_ROMBERG_HPP_INCLUDED
#define NT2_INTEGRATION_FUNCTIONS_COMMON_ROMBERG_HPP_INCLUDED

#include <nt2/integration/functions/romberg.hpp>
#include <nt2/integration/output.hpp>
#include <nt2/integration/options.hpp>
#include <nt2/integration/waypoints.hpp>
#include <nt2/integration/fudge.hpp>
#include <nt2/integration/order.hpp>
#include <nt2/integration/split.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/eps.hpp>
#include <nt2/include/functions/exp2.hpp>
#include <nt2/include/functions/fast_ldexp.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/include/functions/diff.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <boost/mpl/bool.hpp>

namespace nt2 { namespace details
{
  template<class T, class V> class romberg_impl
  {
  public :
    typedef T                                                                input_t;
    typedef typename meta::as_logical<input_t>::type                            bi_t;
    typedef V                                                                value_t;
    typedef typename meta::is_complex<value_t>::type                     v_is_cplx_t;
    typedef typename boost::mpl::if_<v_is_cplx_t,value_t,input_t>::type     result_t;
    typedef typename meta::as_real<value_t>::type                             real_t;
    typedef details::integration_settings<T, V, tag::romberg_>                   o_t;
    typedef container::table<value_t>                                         vtab_t;
    typedef container::table<input_t>                                         itab_t;
    typedef container::table<real_t>                                          rtab_t;
    typedef container::table<result_t>                                      restab_t;
    typedef container::table<bi_t>                                           bitab_t;

    romberg_impl() :  errbnd_(Nan<real_t>()),
                      fcnt_(0),
                      maxfcnt_(Valmax<size_t>()),
                      warn_(0),
                      res_(){}
    ~romberg_impl() {}

    size_t nbeval()           const { return fcnt_;             }
    real_t lasterror()        const { return err_;              }
    bool   ok()               const { return warn_ == 0;        }
    const restab_t & result() const { return res_;              }
    void setwarn(size_t w)          { if(w > warn_) warn_ =  w; }
    size_t  warn()            const { return warn_;             }
    size_t  steps()           const { return steps_;            }

    template < class FUNC, class X>
    void compute( const FUNC& f, const X & x, const o_t & o)
    {
      init(o, x);
      real_t tmptol = tol_;
      BOOST_AUTO_TPL(dif, nt2::abs(nt2::diff(nt2::rowvect(wpts_))));
      real_t tol1= tol_/nt2::globalasum1(dif);
      size_t l = numel(dif);
      res_.resize(extent(wpts_));
      res_(1) = nt2::Zero<value_t>();
      tol_ = tol1*dif(1);
      res_(2) = compute<true>(f, wpts_(1), wpts_(2));
      err_= nt2::abs(dint_);
      for(size_t i=2; i < l; ++i)
      {
        tol_ = tol1*dif(i);
        res_(i+1) = res_(i)+compute<false>(f, wpts_(i), wpts_(i+1));
        err_+= nt2::abs(dint_);
      }
      if (l >= 2)
      {
        tol_ = tol1*dif(l);
        res_(l+1) = res_(l)+compute<true>(f, wpts_(l), wpts_(l+1));
        err_+= nt2::abs(dint_);
      }

      tol_ = tmptol;
      if (!o.return_waypoints)
      {
        res_(begin_) = res_(end_);
        res_.resize(nt2::of_size(1, 1));
      }
    }

  private :
    real_t      errbnd_;
    real_t         err_;
    size_t        fcnt_;
    size_t     maxfcnt_;
    size_t        warn_;
    size_t       steps_;
    real_t         tol_;
    vtab_t         res_;
    real_t        hmin_;
    itab_t        wpts_;
    size_t    nbextrap_;
    size_t     maxstep_;
    bool    singular_a_;
    bool    singular_b_;
    value_t int_, dint_, oldint_;
  private:
    template < class X >
    void init( const o_t & o, const X&x)
    {
      details::prepare_waypoints(o, x, wpts_);
      warn_ = 0;
      fcnt_ = 0;
      maxfcnt_ = o.maxfunccnt;
      err_ = nt2::Zero<real_t>();
      tol_ = fast_ldexp(o.abstol, -4);//give a 16 factor to tolerance
      nbextrap_ = o.nbextrap;
      maxstep_ = o.maxstep;
      singular_a_ = o.singular_a;
      singular_b_ = o.singular_b;
    }

    template <bool test, class FUNC>
    value_t compute(const  FUNC &f, const input_t& a, const input_t& b)
    {
      typedef typename details::fudge<FUNC,1,vtab_t,input_t,test> fudge1;
      typedef typename details::fudge<FUNC,0,vtab_t,input_t,test> fudgelast;
      input_t d =  b-a;
      real_t e = nt2::eps(d);
      input_t s = e*nt2::sign(d);
      size_t n = (1 << (nbextrap_-1))+1;
      itab_t points = nt2::linspace(a, b, n);
      vtab_t y = f(points);
      fudge1::fdg(f, y, fcnt_, singular_a_, a,  s); // Fudge a to avoid infinities.
      fudgelast::fdg(f, y, fcnt_, singular_b_, b, -s); // Fudge b to avoid infinities.
      fcnt_ = n;
      vtab_t t(of_size(1,nbextrap_+maxstep_));
      rtab_t h(extent(t));
      value_t extra = nt2::average(y(begin_), y(end_));
      h(nt2::_(1, nbextrap_)) = nt2::exp2(-nt2::_(Zero<real_t>(), nbextrap_-1));
      size_t pas = nt2::minusone(numel(y));
      for(size_t i=1; i <= nbextrap_; i++)
      {
        t(i)=d*h(i)*(nt2::globalsum(y(nt2::_(begin_, pas, end_)))-extra);
        pas >>= 1;
      }
      input_t h0 = h(nbextrap_)*d;
      h = nt2::sqr(h);
      int_ = zeroneville(h(nt2::_(1, nbextrap_)),t(nt2::_(1, nbextrap_)));
      oldint_ = Nan<value_t>();
      for (size_t i = nbextrap_-1;  i <= maxstep_+nbextrap_-1; i++)
      {
        real_t e1 = nt2::max(nt2::abs(dint_), (nt2::abs(int_-oldint_)));
//        if ( (nt2::abs(dint_) <= tol_*nt2::abs(int_)) && (nt2::abs(int_-oldint_) <= tol_))
        if ( (e1  <= tol_*nt2::max(nt2::abs(int_), One<real_t>())))
        {
         steps_ = i-nbextrap_+2;
         return int_;
        }
        h0 *= nt2::Half<real_t>();
        h(i+2)=h(i+1)*nt2::Quarter<real_t>();
        points = linspace(a+h0, b-h0, 1 << i);
        t(i+2) = (nt2::Half<real_t>()*t(i+1) + h0*globalsum(f(points)));
        fcnt_ += nt2::numel(points);
        oldint_ = int_;
        int_ = zeroneville(h(nt2::_(i-nbextrap_+2, i+2)),t(nt2::_(i-nbextrap_+2, i+2)));
      }
      setwarn(3);
      steps_ = maxstep_+1;
      return int_;
    }
    template < class XPR1, class XPR2>
    value_t zeroneville(const XPR1& h, const XPR2& s)
    // ZERONEVILLE    Extrapolation de neville en zero
    //                [y, dint_] = ZeroNeville( h, s)
    //                Calcule la valeur en zero (y) du polynome qui aux points h(i)
    //                prend les valeurs s(i)
    //                dint_ est de plus une évaluation de l'erreur absolue en ce point.
    {
      vtab_t  c(s),  d(s);
      dint_ = Zero<value_t>();
      size_t nearest = numel(s);
      size_t nbextrap = nearest;
      value_t rint = s(nbextrap);
      nearest--;
      for(size_t m = 1;  m <= nbextrap_-1; m++)
      {
        rtab_t ho = h(nt2::_(1, nbextrap-m));
        rtab_t hp = h(nt2::_(1+m, nbextrap));
        rtab_t den = ho-hp;
        for(size_t i = 1; i <= nbextrap-m;  i++)
        {
          value_t w  = c(i+1)-d(i);
          value_t deno = w/den(i);
          d(i) = hp(i)*deno;
          c(i) = ho(i)*deno;
        }
        if (2*nearest < nbextrap-m)
        {
          dint_ = c(nearest+1);
        }
        else
        {
          dint_ = d(nearest);
          nearest--;
        }
        rint += dint_;
      }
      return rint;
    }
  };
} }

namespace nt2 { namespace ext
                {
                  BOOST_DISPATCH_IMPLEMENT  ( romberg_, tag::cpu_
                                              , (F)(X)(O)
                                              , (unspecified_< F >)
                                              ((ast_<X, nt2::container::domain>))
                                              (unspecified_<O>)
    )
  {
    typedef typename O::value_t                                             value_t;
    typedef typename O::input_t                                             input_t;
    typedef typename O::real_t                                               real_t;
    typedef typename O::result_t                                           result_t;
    typedef typename O::restab_t                                           restab_t;
    typedef typename boost::is_same<input_t,real_t>::type           input_is_real_t;
    typedef nt2::integration::output<restab_t,real_t>                   result_type;

    result_type operator()(F f, X const& x, O const& o)
    {
      details::romberg_impl<input_t, value_t> q;
      q.compute(f, x, o);
      result_type that =  {q.result(), q.lasterror(),q.nbeval(),q.ok(),q.warn()};
      return that;
    }
  };
} }
#endif


/*
 * MatchRefiner.cpp
 *
 * Copyright(C) 2014, University of Oulu, all rights reserved.
 * Copyright(C) 2014, NVIDIA Corporation, all rights reserved.
 * Third party copyrights are property of their respective owners.
 * Contact: Daniel Herrera C. (dherrera@ee.oulu.fi),
 *          Kihwan Kim(kihwank@nvidia.com)
 * Author : Daniel Herrera C.
 */

#include "MatchRefiner.h"
#include <opencv2/imgproc.hpp>
#include <Eigen/Eigen>
#include "PatchWarper.h"
#include "Profiler.h"

namespace dtslam
{

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This version uses manual loops. It doesn't seem optimal but it's the fastest so far.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MatchRefiner::MatchRefiner()
{
	mRefJacobian.create(kPatchSize, kPatchSize);
}

void MatchRefiner::setRefPatch(const cv::Mat1b *refPatch)
{
	assert(refPatch->isContinuous());
	assert(refPatch->step[0] == kPatchSize);

	//Copy
	mRefPatch = refPatch;

	//Build jacobian and hessian
	cv::Vec6i hessianTriangle(0, 0, 0, 0, 0, 0);
	const int rowStride = kPatchSize;

	const unsigned char *patchPtr = (*mRefPatch)[0] + rowStride + 1;
	cv::Vec2i *jacPtr = mRefJacobian[0] + rowStride + 1;

	for (int j = 1; j < kPatchSize - 1; j++)
	{
		for (int i = 1; i < kPatchSize - 1; i++)
		{
			//Jacobian
			cv::Vec2i &jac = *jacPtr;
			jac[0] = ((int)patchPtr[1] - (int)patchPtr[-1]) / 2; //dx
			jac[1] = ((int)patchPtr[rowStride] - (int)patchPtr[-rowStride]) / 2; //dy

			//Lower left triangle of Hessian (=J'*J)
			//Note that jac[2]==1
			hessianTriangle[0] += jac[0] * jac[0];
			hessianTriangle[1] += jac[0] * jac[1];
			hessianTriangle[2] += jac[1] * jac[1];
			hessianTriangle[3] += jac[0];
			hessianTriangle[4] += jac[1];
			hessianTriangle[5] += 1;

			patchPtr++;
			jacPtr++;
		}
		patchPtr += 2;
		jacPtr += 2;
	}

	//Build hessian
	cv::Matx33f hessian;
	for (int v = 0, j = 0; j < 3; j++)
	{
		for (int i = 0; i <= j; i++)
		{
			hessian(j, i) = hessian(i, j) = (float)hessianTriangle[v++];
		}
	}
	mRefHessianInv = hessian.inv(cv::DECOMP_CHOLESKY);

	//Calculate variance of patch
	//    const uchar *refPtr = (*mRefPatch)[0];
	//    uint32_t refSumSq=0;
	//    uint32_t refSum=0;
	//
	//    for (int j = 0; j < kPatchSize; j++)
	//    {
	//        for (int i = 0; i < kPatchSize; i++)
	//        {
	//            refSumSq += refPtr[0]*refPtr[0];
	//            refSum += refPtr[0];
	//
	//            //Advance col
	//            refPtr++;
	//        }
	//        //Advance row
	//    }
	//    uint32_t refSqSum = refSum*refSum;
	//	mRefVar = (refSumSq >> (kPatchSizeBits*2)) - (refSqSum >> (kPatchSizeBits*4));
}

void MatchRefiner::setImg(const cv::Mat1b *img)
{
	mImg = img;
}

bool MatchRefiner::refine()
{
	//const float kConvergenceLimit = 0.03f;
	const float kConvergenceLimit = 0.05f;

	//Prepare patch matrix
	mImgPatch.release();
	mImgPatch.create(kPatchSize, kPatchSize);
	assert(mImgPatch.isContinuous());

	//Initial transform
	mMeanDiff = 0;

	//Iterate
	for (mIterationCount = 0; mIterationCount < kMaxIterations; ++mIterationCount)
	{
		//Refine
		float updateSq = refineIteration();

		//Check bounds
		if (mOrigin.x < 0 || mOrigin.y < 0 || mOrigin.x >= mImg->cols - 1 - kPatchSize || mOrigin.y >= mImg->rows - 1 - kPatchSize)
		{
			mOrigin.x = 0;
			mOrigin.y = 0;
			return false;
		}

		//Check convergence
		if (updateSq < kConvergenceLimit)
			return true;
	}

	return true;
}

float MatchRefiner::refineIteration()
{
	if (!buildImgPatch())
		return 0;

	//bool log=false;
	//if(log)
	//{
	//	DTSLAM_LOG << "Iteration=" << mIterationCount << "\n";
	//	DTSLAM_LOG << "img=" << mImgPatch << "\n";
	//}

	//Loop over pixels
	const size_t rowStride = kPatchSize;

	const uchar *refPtr = (*mRefPatch)[0] + rowStride + 1;
	const uchar *imgPtr = mImgPatch[0] + rowStride + 1;
	const cv::Vec2i *refJacPtr = mRefJacobian[0] + rowStride + 1;

	cv::Vec3i accum(0, 0, 0); // sum(J(p)*diff)

	for (int j = 1; j < kPatchSize - 1; j++)
	{
		for (int i = 1; i < kPatchSize - 1; i++)
		{
			const cv::Vec2i &jac = *refJacPtr;

			int imgValue = *imgPtr;
			int refValue = *refPtr;
			int diff = imgValue - refValue + mMeanDiff;

			accum[0] += diff * jac[0];
			accum[1] += diff * jac[1];
			accum[2] += diff;

			//Advance col
			refJacPtr++;
			refPtr++;
			imgPtr++;
		}

		//Advance row
		refJacPtr += 2;
		refPtr += 2;
		imgPtr += 2;
	}

	//Calculate update
	const cv::Vec3f update = mRefHessianInv * (cv::Vec3f)accum;
	mOrigin.x -= update[0];
	mOrigin.y -= update[1];
	mMeanDiff -= (int)update[2];

	//float updateSquared = cvutils::PointDistSq(lastOrigin, mOrigin);
	float updateSquared = abs(update[0]) + abs(update[1]);
	return updateSquared;
}

bool MatchRefiner::buildImgPatch()
{
	cv::Point2i iorigin = cv::Point2i((int)mOrigin.x, (int)mOrigin.y);
	if (iorigin.x < 0 || iorigin.y < 0 || iorigin.x >= mImg->cols - 1 - kPatchSize || iorigin.y >= mImg->rows - 1 - kPatchSize)
		return false;

	const float dx = mOrigin.x - iorigin.x;
	const float dy = mOrigin.y - iorigin.y;
	const float mixTL = (1.0f - dx) * (1.0f - dy);
	const float mixTR = (dx)* (1.0f - dy);
	const float mixBL = (1.0f - dx) * (dy);
	const float mixBR = (dx)* (dy);

	const size_t imgRowStride = mImg->step[0];

	uchar *patchPtr = mImgPatch[0];

	//Make patch
	for (int j = 0; j < kPatchSize; j++)
	{
		const uchar *imgPtr = (*mImg)[iorigin.y + j] + (int)iorigin.x;
		for (int i = 0; i < kPatchSize; i++)
		{
			float value = mixTL * imgPtr[0] + mixTR * imgPtr[1] + mixBL * imgPtr[imgRowStride]
				+ mixBR * imgPtr[imgRowStride + 1];
			*patchPtr = (uchar)value;

			//Advance col
			patchPtr++;
			imgPtr++;
		}
	}
	return true;
}

int MatchRefiner::getScore()
{
	//Check image bounds
	const cv::Point2f maxOrigin((float)mImg->cols - kPatchSize - 1, (float)mImg->rows - kPatchSize - 1);
	if (mOrigin.x < 0 || mOrigin.x >= maxOrigin.x
		|| mOrigin.y < 0 || mOrigin.y >= maxOrigin.y)
		return std::numeric_limits<int>::max();

	if (mImgPatch.empty())
	{
		mImgPatch.create(kPatchSize, kPatchSize);
		buildImgPatch();
	}

	unsigned int score = 0;

	//    //Debug: This calculates the mean of each patch
	//    float meanImg=0;
	//    float meanRef=0;
	//    {
	//        const uchar *refPtr = (*mRefPatch)[0];
	//        const uchar *imgPtr = mImgPatch[0];
	//
	//        for (int j = 0; j < kPatchSize; j++)
	//        {
	//            for (int i = 0; i < kPatchSize; i++)
	//            {
	//                meanImg += (*imgPtr);
	//                meanRef += (*refPtr);
	//
	//                //Advance col
	//                refPtr++;
	//                imgPtr++;
	//            }
	//
	//            //Advance row
	//        }
	//        meanImg /= kPatchSize*kPatchSize;
	//        meanRef /= kPatchSize*kPatchSize;
	//    }

	const uchar *refPtr = (*mRefPatch)[0];
	const uchar *imgPtr = mImgPatch[0];
	int iMeanDiff = mMeanDiff;

	for (int j = 0; j < kPatchSize; j++)
	{
		for (int i = 0; i < kPatchSize; i++)
		{
			int imgValue = *imgPtr;
			int refValue = *refPtr;
			int diff = imgValue - refValue + iMeanDiff;

			//Debug: this is the real measure, using mMeanDiff is a shortcut but accurate enough
			//float diff = (*imgPtr) - meanImg - (*refPtr) + meanRef;

			score += diff*diff;

			//Advance col
			refPtr++;
			imgPtr++;
		}

		//Advance row
	}

	//score >>= 2*kPatchSizeBits;

	//    ZssdComparer c;
	//    c.setRefPatch(*mRefPatch);
	//    auto res = c.compare(mImgPatch);
	//    DTSLAM_LOG << "Score according to zssdcompare=" << res << "\n";

	//score = (score<<6) / mRefVar;
	//DTSLAM_LOG << "Score according to refiner=" << scorei << "\n";

	//DTSLAM_LOG << "ref=" << *mRefPatch << "\n";
	//DTSLAM_LOG << "img=" << mImgPatch << "\n";
	return score;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This version uses Eigen. I was hoping it'd be faster than manual loops but it turned out to be slower
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//MatchRefiner::MatchRefiner()
//{
//}
//
//void MatchRefiner::setRefPatch(const cv::Mat1b *refPatch)
//{
//	assert(refPatch->isContinuous());
//	assert(refPatch->step[0] == kPatchSize);
//
//	//Copy
//	Eigen::Map<Eigen::Array<unsigned char, kPatchSize, kPatchSize, Eigen::RowMajor>> refPatchEigen(refPatch->data);
//	mRefPatch = refPatchEigen.cast<short>();
//
//	//Jacobian
//	mRefJacobianX.setZero();
//	mRefJacobianY.setZero();
//	mRefJacobianX.block<kPatchSize - 2, kPatchSize - 2>(1, 1) = (mRefPatch.block<kPatchSize - 2, kPatchSize - 2>(1, 2) - mRefPatch.block<kPatchSize - 2, kPatchSize - 2>(1, 0)).cast<int>() / 2;
//	mRefJacobianY.block<kPatchSize - 2, kPatchSize - 2>(1, 1) = (mRefPatch.block<kPatchSize - 2, kPatchSize - 2>(2, 1) - mRefPatch.block<kPatchSize - 2, kPatchSize - 2>(0, 1)).cast<int>() / 2;
//
//	//Hessian
//	cv::Vec6i hessianTriangle(0, 0, 0, 0, 0, 0);
//	hessianTriangle[0] = (mRefJacobianX * mRefJacobianX).sum(); //sum(dx*dx)
//	hessianTriangle[1] = (mRefJacobianX * mRefJacobianY).sum(); //sum(dx*dy)
//	hessianTriangle[2] = (mRefJacobianY * mRefJacobianY).sum(); //sum(dy*dy)
//	hessianTriangle[3] = mRefJacobianX.sum(); //sum(dx)
//	hessianTriangle[4] = mRefJacobianY.sum(); //sum(dy)
//	hessianTriangle[5] = (kPatchSize - 2)*(kPatchSize - 2);
//
//    //Build hessian
//	Eigen::Matrix<float, 3, 3, Eigen::RowMajor> hessian;
//	for (int v = 0, j = 0; j < 3; j++)
//    {
//        for(int i = 0; i <= j; i++)
//        {
//            hessian(j, i) = hessian(i, j) = (float)hessianTriangle[v++];
//        }
//    }
//
//	mRefHessianInv = hessian.inverse();
//}
//
//void MatchRefiner::setImg(const cv::Mat1b *img)
//{
//	mImg = img;
//}
//
//bool MatchRefiner::refine()
//{
//	//const float kConvergenceLimit = 0.03f;
//	const float kConvergenceLimit = 0.05f;
//
//    //Prepare patch matrix
//	mValidImgPatch = false;
//
//    //Initial transform
//    mMeanDiff = 0;
//
//    //Iterate
//    for(mIterationCount = 0; mIterationCount < kMaxIterations; ++mIterationCount)
//    {
//    	//Refine
//    	float updateSq = refineIteration();
//
//		//Check bounds
//		if (mOrigin.x < 0 || mOrigin.y < 0 || mOrigin.x >= mImg->cols - 1 - kPatchSize || mOrigin.y >= mImg->rows - 1 - kPatchSize)
//		{
//			mOrigin.x = 0;
//			mOrigin.y = 0;
//			return false;
//		}
//
//    	//Check convergence
//    	if(updateSq < kConvergenceLimit)
//    		return true;
//    }
//
//    return true;
//}
//
//float MatchRefiner::refineIteration()
//{
//    if(!buildImgPatch())
//    	return 0;
//
//	Eigen::Array<int, kPatchSize, kPatchSize, Eigen::RowMajor> diff;
//	diff = (mImgPatch - mRefPatch + mMeanDiff).cast<int>();
//
//	Eigen::Vector3i accum;
//	accum[0] = (diff * mRefJacobianX).sum(); //sum(diff*dx)
//	accum[1] = (diff * mRefJacobianY).sum(); //sum(diff*dy)
//	accum[2] = diff.sum(); //sum(diff)
//
//    //Calculate update
//	Eigen::Vector3f update = mRefHessianInv * accum.cast<float>();
//    mOrigin.x -= update[0];
//    mOrigin.y -= update[1];
//    mMeanDiff -= (int)update[2];
//
//    //float updateSquared = cvutils::PointDistSq(lastOrigin, mOrigin);
//	float updateSquared = abs(update[0])+abs(update[1]);
//    return updateSquared;
//}
//
//bool MatchRefiner::buildImgPatch()
//{
//    cv::Point2i iorigin = cv::Point2i((int)mOrigin.x, (int)mOrigin.y);
//	if (iorigin.x < 0 || iorigin.y < 0 || iorigin.x >= mImg->cols - 1 - kPatchSize || iorigin.y >= mImg->rows - 1 - kPatchSize)
//	{
//		mValidImgPatch = false;
//		return false;
//	}
//
//	Eigen::Map<Eigen::Array<uchar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> imgMap(mImg->data, mImg->rows, mImg->cols);
//
//    const float dx = mOrigin.x - iorigin.x;
//    const float dy = mOrigin.y - iorigin.y;
//
//	auto blockLeft = imgMap.block<kPatchSize+1, kPatchSize>(iorigin.y, iorigin.x);
//	auto blockRight = imgMap.block<kPatchSize+1, kPatchSize>(iorigin.y, iorigin.x + 1);
//
//	Eigen::Array<uchar, kPatchSize+1, kPatchSize, Eigen::RowMajor> pass1;
//	pass1 = (blockLeft.cast<float>()*(1 - dx) + blockRight.cast<float>()*dx).cast<uchar>();
//
//	auto blockUp = pass1.block<kPatchSize, kPatchSize>(0,0);
//	auto blockDown = pass1.block<kPatchSize, kPatchSize>(1,0);
//	mImgPatch = (blockUp.cast<float>()*(1 - dy) + blockDown.cast<float>()*dy).cast<short>();
//
//	mValidImgPatch = true;
//    return true;
//}
//
//int MatchRefiner::getScore()
//{
//    //Check image bounds
//	const cv::Point2f maxOrigin((float)mImg->cols - kPatchSize - 1, (float)mImg->rows - kPatchSize - 1);
//	if(mOrigin.x < 0 || mOrigin.x >= maxOrigin.x
//		|| mOrigin.y < 0 || mOrigin.y >= maxOrigin.y)
//		return std::numeric_limits<int>::max();
//
//	if(!mValidImgPatch)
//	{
//		buildImgPatch();
//	}
//
//	Eigen::Array<int, kPatchSize, kPatchSize, Eigen::RowMajor> diff;
//	diff = (mImgPatch - mRefPatch + mMeanDiff).cast<int>();
//	unsigned int score = (diff*diff).sum(); //sum(diff*diff)
//
//	return score;
//}

} /* namespace dtslam */


///////////////////////////////////////////////////////////////////////////////
// statistics::model::algorithm::log_likelihoods.hpp                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOODS_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOODS_HPP_ER_2009
#include <algorithm>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/statistics/model/wrap/aggregate/model_dataset.hpp>
#include <boost/statistics/model/functional/log_likelihood_evaluator.hpp>

namespace boost{
namespace statistics{
namespace model{

// Evaluates the log-likelihood for each parameter in [b_p,e_p), given
// model and data md
template<typename T,
    typename M,typename Rx,typename Ry,typename ItP,typename ItLw>
ItLw log_likelihoods(
    model_dataset_<M,Rx,Ry> md,
    ItP b_p,
    ItP e_p,
    ItLw o_lw
){
    typedef log_likelihood_evaluator<T,M,Rx,Ry> eval_;
    return std::transform(
        b_p,
        e_p,
        o_lw,
        eval_(md)
    );
}

// Maps the input log-pdf to the ratio-log-pdf
template<
    typename T,
    typename M,
    typename Rx,
    typename Ry,
    typename ItP,
    typename ItLw
>
ItLw
log_likelihoods(
    model_dataset_<M,Rx,Ry> md,
    ItP b_p,
    ItP e_p,
    ItLw b_lw,
    ItLw o_lw   // lw <- (log_likelihod - lw)
){
    typedef log_likelihood_evaluator<T,M,Rx,Ry> eval_;
    eval_ e(md);
    return std::transform(
        b_p,
        e_p,
        b_lw,
        o_lw,
        boost::lambda::bind(e,boost::lambda::_2) - boost::lambda::_1
    );
}

}// model
}// statistics
}// boost

#endif


// OffAxisSource.h
// created by Kuangdai on 11-Nov-2017
// base class of off-axis source

#include "OffAxisSource.h"
#include "Geodesy.h"
#include "Quad.h"
#include "Domain.h"
#include "Element.h"
// TODO: implement this in core
// #include "OffAxisSourceTerm.h"
#include "Mesh.h"
#include "XMath.h"
#include "SpectralConstants.h"
#include "XMPI.h"
#include "MultilevelTimer.h"

OffAxisSource::OffAxisSource(double depth, double lat, double lon,
    double srcLat, double srcLon, double srcDep):
mDepth(depth), mLatitude(lat), mLongitude(lon) {
    // handle singularity at poles
    if (std::abs(mLatitude - 90.) < tinyDouble) {
        mLatitude = 90.;
        mLongitude = 0.;
    }
    if (std::abs(mLatitude + 90.) < tinyDouble) {
        mLatitude = -90.;
        mLongitude = 0.;
    }
    // compute theta and phi in source-centered coordinate system
    RDCol3 rtpG, rtpS;
    rtpG(0) = 1.;
    rtpG(1) = Geodesy::lat2Theta_d(mLatitude, mDepth);
    rtpG(2) = Geodesy::lon2Phi(mLongitude);
    rtpS = Geodesy::rotateGlob2Src(rtpG, srcLat, srcLon, srcDep);
    mThetaSrc = rtpS(1);
    mPhiSrc = rtpS(2);
}

void OffAxisSource::release(Domain &domain, const Mesh &mesh) const {
    MultilevelTimer::begin("Locate Off-axis Source", 2);
    // locate local
    int myrank = XMPI::nproc();
    int locTag = -1;
    RDColP interpFactXii, interpFactEta;
    if (locate(mesh, locTag, interpFactXii, interpFactEta)) {
        myrank = XMPI::rank();
    }

    // min recRank
    int myrank_min = XMPI::min(myrank);
    if (myrank_min == XMPI::nproc()) {
        throw std::runtime_error("OffAxisSource::release || Error locating off-axis source.");
    }
    MultilevelTimer::end("Locate Off-axis Source", 2);

    MultilevelTimer::begin("Compute Off-axis Source", 2);
    // release to me
    if (myrank_min == XMPI::rank()) {
        // compute OffAxisSource term
        vec_arPP_CMatX3 fouriers;
        const Quad *myQuad = mesh.getQuad(locTag);
        computeSourceFourier(*myQuad, interpFactXii, interpFactEta, mPhiSrc, fouriers);
        // add to domain
        Element *myElem = domain.getElement(myQuad->getElementTag());
        // TODO: implement this in core
        //       the existing class SourceTerm is not general enough
        //       because the source time funciton varies for each source
        // domain.addOffAxisSourceTerm(new OffAxisSourceTerm(myElem, fouriers));
    }
    MultilevelTimer::end("Compute Off-axis Source", 2);
}

bool OffAxisSource::locate(const Mesh &mesh, int &locTag,
    RDColP &interpFactXii, RDColP &interpFactEta) const {
    MultilevelTimer::begin("R OffAxisSource", 3);
    RDCol2 srcCrds = RDCol2::Zero();
    double r = mesh.computeRadiusRef(mDepth, mLatitude, mLongitude);
    srcCrds(0) = r * sin(mThetaSrc);
    srcCrds(1) = r * cos(mThetaSrc);
    MultilevelTimer::end("R OffAxisSource", 3);

    // check range of subdomain
    if (srcCrds(0) > mesh.sMax() + tinySingle || srcCrds(0) < mesh.sMin() - tinySingle) {
        return false;
    }
    if (srcCrds(1) > mesh.zMax() + tinySingle || srcCrds(1) < mesh.zMin() - tinySingle) {
        return false;
    }
    // find host element
    RDCol2 srcXiEta;
    for (int iloc = 0; iloc < mesh.getNumQuads(); iloc++) {
        const Quad *quad = mesh.getQuad(iloc);
        if (quad->isFluid() || !quad->nearMe(srcCrds(0), srcCrds(1))) {
            continue;
        }
        if (quad->invMapping(srcCrds, srcXiEta)) {
            if (std::abs(srcXiEta(0)) <= 1.000001 && std::abs(srcXiEta(1)) <= 1.000001) {
                locTag = iloc;
                XMath::interpLagrange(srcXiEta(0), nPntEdge,
                    (quad->isAxial() ? SpectralConstants::getP_GLJ().data()
                                     : SpectralConstants::getP_GLL().data()),
                    interpFactXii.data());
                XMath::interpLagrange(srcXiEta(1), nPntEdge,
                    SpectralConstants::getP_GLL().data(), interpFactEta.data());
                return true;
            }
        }
    }
    return false;
}

#include "Parameters.h"
// #include "OffAxisPointForce.h"
#include <fstream>
#include <boost/algorithm/string.hpp>

void OffAxisSource::buildInparam(std::vector<OffAxisSource> *&offsrc,
    const Parameters &par, int verbose) {
    // TODO:
    // read location of off-axis sources
    // read source-time function
    // build off-axis source objects
}


#include <iomanip>

#include <Eigen/Dense>
#include <glog/logging.h>
#include <maplab-common/file-logger.h>
#include <minkindr_conversions/kindr_tf.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/transform_datatypes.h>

#include "simulation/magnetometer-simulator.h"

// This demo class subscribes to NavSatFix and global attitude topic and
// simulates
// a magnetometer measurement which can then be compared to real magnetometer
// measurements.
// Used to validate the magnetometer simulator with real data.
class MagnetometerSimulatorDemo {
 public:
  explicit MagnetometerSimulatorDemo(const simulation::YearMonthDay& utc_time) {
    magnetometer_simulator_.reset(
        new simulation::MagnetometerSimulator(utc_time));

    const std::string filename = "/tmp/simulated_magnetometer.txt";
    logger_.reset(new common::FileLogger(filename));
  }

  void runAndJoin() const {
    ros::spin();
    ros::waitForShutdown();
  }

  void registerSubscriberAndPublisher() {
    // Subscribe to NavSatFix topic.
    const std::string topic_name_llh = "/mavros/global_position/global";
    LOG(INFO) << "Subscribed to topic " << topic_name_llh;
    boost::function<void(const sensor_msgs::NavSatFixConstPtr& msg)>
        llh_callback =
            boost::bind(&MagnetometerSimulatorDemo::llhCallback, this, _1);
    sub_llh_ = node_handle_.subscribe(topic_name_llh, 1000, llh_callback);

    // Subscribe to global attitude topic.
    const std::string topic_name_attitude = "/mavros/imu/data";
    LOG(INFO) << "Subscribed to topic " << topic_name_attitude;
    boost::function<void(const sensor_msgs::ImuConstPtr& msg)>
        attitude_callback =
            boost::bind(&MagnetometerSimulatorDemo::attitudeCallback, this, _1);
    sub_attitude_ =
        node_handle_.subscribe(topic_name_attitude, 1000, attitude_callback);
  }

  void llhCallback(const sensor_msgs::NavSatFixConstPtr& msg) {
    aslam::Quaternion trafo_kindr;
    quaternionTFToKindr(quaternion_, &trafo_kindr);

    Eigen::Matrix3d R_B_G = trafo_kindr.getRotationMatrix().inverse();
    Eigen::Vector3d llh(msg->latitude, msg->longitude, msg->altitude);

    Eigen::Vector3d simulated_magnetic_field_tesla;
    magnetometer_simulator_->calculateMagneticField(
        llh, R_B_G, &simulated_magnetic_field_tesla);

    // Save to file.
    *logger_ << std::setprecision(15) << simulated_magnetic_field_tesla(0)
             << " " << simulated_magnetic_field_tesla(1) << " "
             << simulated_magnetic_field_tesla(2) << std::endl;
  }

  void attitudeCallback(const sensor_msgs::ImuConstPtr& msg) {
    quaternion_ = tf::Quaternion(
        msg->orientation.x, msg->orientation.y, msg->orientation.z,
        msg->orientation.w);
  }

 private:
  std::unique_ptr<simulation::MagnetometerSimulator> magnetometer_simulator_;
  std::unique_ptr<common::FileLogger> logger_;
  ros::NodeHandle node_handle_;
  ros::Subscriber sub_llh_;
  ros::Subscriber sub_attitude_;
  tf::Quaternion quaternion_;
};

int main(int argc, char** argv) {
  // Initialize google logging and flags.
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  // Initialize ros node handle.
  ros::init(argc, argv, "magnetometer_simulator_demo");

  // Set the time of the measurement.
  simulation::YearMonthDay utc_date;
  utc_date.year = 2015;
  utc_date.month = 5;
  utc_date.day = 1;

  MagnetometerSimulatorDemo magnetometer_simulator_demo(utc_date);
  magnetometer_simulator_demo.registerSubscriberAndPublisher();
  magnetometer_simulator_demo.runAndJoin();

  return 0;
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <fstream>
#include <tuple>

// boost
#define BOOST_TEST_MODULE QPSolverTest
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// Eigen
#include <unsupported/Eigen/Polynomials>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include <RBDyn/EulerIntegration.h>
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>
#include <RBDyn/ID.h>

// sch
#include <sch/CD/CD_Pair.h>
#include <sch/S_Object/S_Box.h>
#include <sch/S_Object/S_Sphere.h>

// Tasks
#include "Tasks/Bounds.h"
#include "Tasks/QPConstr.h"
#include "Tasks/QPContactConstr.h"
#include "Tasks/QPMotionConstr.h"
#include "Tasks/QPSolver.h"
#include "Tasks/QPTasks.h"

// Arms
#include "arms.h"

BOOST_AUTO_TEST_CASE(FrictionConeTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  double angle = cst::pi<double>() / 4.;
  double mu = std::tan(angle);

  qp::FrictionCone cone(Matrix3d::Identity(), 4, mu);
  for(Vector3d v : cone.generators)
  {
    // check cone equation x^2 + y^2 = z^2(tan(angle)^2)
    BOOST_CHECK_SMALL(std::pow(v.x(), 2) + std::pow(v.y(), 2) - std::pow(v.z(), 2) * std::pow(mu, 2), 0.00001);
  }

  Matrix3d rep;
  rep << 0., 1., 0., 0., 0., 1., 1., 0., 0.;
  qp::FrictionCone cone2(rep, 4, mu);
  for(Vector3d v : cone2.generators)
  {
    // check cone equation in rep frame z^2 + y^2 = x^2(tan(angle)^2)
    BOOST_CHECK_SMALL(std::pow(v.z(), 2) + std::pow(v.y(), 2) - std::pow(v.x(), 2) * std::pow(mu, 2), 0.00001);
  }
}

// TODO contacts Test

BOOST_AUTO_TEST_CASE(QPTaskTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs(1);

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  qp::QPSolver solver;

  solver.nrVars(mbs, {}, {});
  BOOST_CHECK_EQUAL(solver.nrVars(), 3);

  solver.updateConstrSize();

  Vector3d posD = Vector3d(0.707106, 0.707106, 0.);
  qp::PositionTask posTask(mbs, 0, "b3", posD);
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);

  // Test addTask
  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test PositionTask
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(posTask.eval().norm(), 0.00001);

  // test removeTask
  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  qp::OrientationTask oriTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.));
  qp::SetPointTask oriTaskSp(mbs, 0, &oriTask, 10., 1.);

  // Test addTask
  solver.addTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test OrientatioTask
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(oriTask.eval().norm(), 0.00001);

  // test removeTask
  solver.removeTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  qp::PostureTask postureTask(mbs, 0, {{}, {0.2}, {0.4}, {-0.8}}, 10., 1.);
  postureTask.jointsStiffness(mbs, {{"j2", 10.}});
  solver.addTask(&postureTask);

  // Test PostureTask
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(postureTask.task().eval().norm(), 0.00001);

  solver.removeTask(&postureTask);

  Vector3d comD(RotZ(cst::pi<double>() / 4.) * rbd::computeCoM(mb, mbcInit));
  qp::CoMTask comTask(mbs, 0, comD);
  qp::SetPointTask comTaskSp(mbs, 0, &comTask, 10., 1.);

  solver.addTask(&comTaskSp);

  // Test CoMTask
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(comTask.eval().norm(), 0.00001);

  solver.removeTask(&comTaskSp);

  qp::LinVelocityTask linVelocityTask(mbs, 0, "b3", -Vector3d::UnitX() * 0.005);
  qp::SetPointTask linVelocityTaskSp(mbs, 0, &linVelocityTask, 1000., 10000.);

  solver.addTask(&linVelocityTaskSp);

  // Test LinVelocityTask
  mbcs[0] = mbcInit;
  for(int i = 0; i < 4000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  // error is huge, why ?
  BOOST_CHECK_SMALL(linVelocityTask.eval().norm(), 0.001);

  solver.removeTask(&linVelocityTaskSp);
}

BOOST_AUTO_TEST_CASE(QPConstrTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb, mbEnv;
  MultiBodyConfig mbcInit, mbcEnv;

  std::tie(mb, mbcInit) = makeZXZArm();
  std::tie(mbEnv, mbcEnv) = makeEnv();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);
  forwardKinematics(mbEnv, mbcEnv);
  forwardVelocity(mbEnv, mbcEnv);

  std::vector<MultiBody> mbs = {mb, mbEnv};
  std::vector<MultiBodyConfig> mbcs = {mbcInit, mbcEnv};

  qp::QPSolver solver;

  std::vector<qp::UnilateralContact> contVec = {
      qp::UnilateralContact(0, 1, "b3", "b0", {Vector3d::Zero()}, Matrix3d::Identity(), sva::PTransformd::Identity(), 3,
                            std::tan(cst::pi<double>() / 4.))};

  Vector3d posD = Vector3d(0.707106, 0.707106, 0.);
  qp::PositionTask posTask(mbs, 0, "b3", posD);
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);

  qp::OrientationTask oriTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.));
  qp::SetPointTask oriTaskSp(mbs, 0, &oriTask, 10., 1.);

  qp::ContactAccConstr contCstrAcc;

  // Test addEqualityConstraint
  mbcs[0] = mbcInit;
  contCstrAcc.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrEqualityConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, contVec, {});
  solver.updateConstrSize();

  // check vars number is 3 dof + 3 lambda
  BOOST_CHECK_EQUAL(solver.nrVars(), 3 + 3);

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);
  solver.addTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 2);

  solver.data().computeNormalAccB(mbs, mbcs);
  posTask.update(mbs, mbcs, solver.data());
  oriTask.update(mbs, mbcs, solver.data());
  Vector3d evalPos = posTask.eval();
  Vector3d evalOri = oriTask.eval();

  // Test ContactAccConstr
  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL((posTask.eval() - evalPos).norm(), 0.00001);
  BOOST_CHECK_SMALL((oriTask.eval() - evalOri).norm(), 0.00001);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);
  solver.removeTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test removeEqualityConstraint
  contCstrAcc.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);

  // Test ContactSpeedConstr
  mbcs[0] = mbcInit;
  qp::ContactSpeedConstr contCstrSpeed(0.001);

  // Test addEqualityConstraint
  contCstrSpeed.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrEqualityConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, contVec, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);
  solver.addTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 2);

  solver.data().computeNormalAccB(mbs, mbcs);
  posTask.update(mbs, mbcs, solver.data());
  oriTask.update(mbs, mbcs, solver.data());
  evalPos = posTask.eval();
  evalOri = oriTask.eval();

  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL((posTask.eval() - evalPos).norm(), 0.00001);
  BOOST_CHECK_SMALL((oriTask.eval() - evalOri).norm(), 0.00001);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);
  solver.removeTask(&oriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test removeEqualityConstraint
  solver.removeEqualityConstraint(&contCstrSpeed);
  BOOST_CHECK_EQUAL(solver.nrEqualityConstraints(), 0);
  solver.removeConstraint(&contCstrSpeed);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);

  // MotionConstr test
  mbcs[0] = mbcInit;
  contVec = {};
  double Inf = std::numeric_limits<double>::infinity();
  std::vector<std::vector<double>> torqueMin = {{}, {-Inf}, {-Inf}, {-Inf}};
  std::vector<std::vector<double>> torqueMax = {{}, {Inf}, {Inf}, {Inf}};
  std::vector<std::vector<double>> torqueDtMin = {{}, {-Inf}, {-Inf}, {-Inf}};
  std::vector<std::vector<double>> torqueDtMax = {{}, {Inf}, {Inf}, {Inf}};
  qp::MotionConstr motionCstr(mbs, 0, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005);
  qp::PositiveLambda plCstr;

  motionCstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  plCstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 2);

  solver.nrVars(mbs, contVec, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test MotionConstr
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }
  motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec());
  motionCstr.torque(mbs, mbcs);

  BOOST_CHECK_SMALL(posTask.eval().norm(), 5e-05);

  MultiBodyConfig mbcTest(mbcs[0]);
  mbcTest.jointTorque = {{}, {0.}, {0.}, {0.}};
  InverseDynamics id(mb);
  id.inverseDynamics(mb, mbcTest);

  BOOST_CHECK_SMALL((dofToVector(mb, mbcs[0].jointTorque) - dofToVector(mb, mbcTest.jointTorque)).norm(), 0.00001);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test removeEqualityConstraint
  motionCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  // MotionConstr test with contact
  mbcs[0] = mbcInit;
  contVec = {qp::UnilateralContact(0, 1, "b3", "b0", {Vector3d::Zero()}, Matrix3d::Identity(),
                                   sva::PTransformd::Identity(), 3, std::tan(cst::pi<double>() / 4.))};

  solver.addGenInequalityConstraint(&motionCstr);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 1);
  solver.addConstraint(&motionCstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 2);

  solver.nrVars(mbs, contVec, {});
  solver.updateConstrSize();

  // check vars number is 3 dof + 3 lambda
  BOOST_CHECK_EQUAL(solver.nrVars(), 3 + 3);

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test MotionConstr
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL(posTask.eval().norm(), 5e-5);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test removeEqualityConstraint
  motionCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  plCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);
}

BOOST_AUTO_TEST_CASE(QPJointLimitsTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<rbd::MultiBody> mbs = {mb};
  std::vector<rbd::MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  qp::PositionTask posTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.) * mbcInit.bodyPosW[bodyI].translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);

  double inf = std::numeric_limits<double>::infinity();
  std::vector<std::vector<double>> lBound = {{}, {-cst::pi<double>() / 4.}, {-inf}, {-inf}};
  std::vector<std::vector<double>> uBound = {{}, {cst::pi<double>() / 4.}, {inf}, {inf}};

  qp::JointLimitsConstr jointConstr(mbs, 0, {lBound, uBound}, 0.001);

  // Test add*Constraint
  solver.addBoundConstraint(&jointConstr);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 1);
  solver.addConstraint(&jointConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test JointLimitsConstr
  mbcs[0] = mbcInit;
  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE_GT(mbcs[0].q[1][0], -cst::pi<double>() / 4. - 0.01);
  }

  posTask.position(RotZ(-cst::pi<double>() / 2.) * mbcInit.bodyPosW[bodyI].translation());
  mbcs[0] = mbcInit;
  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE_LT(mbcs[0].q[1][0], cst::pi<double>() / 4. + 0.01);
  }

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test remove*Constraint
  solver.removeBoundConstraint(&jointConstr);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 0);
  solver.removeConstraint(&jointConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);
}

BOOST_AUTO_TEST_CASE(QPDamperJointLimitsTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<rbd::MultiBody> mbs = {mb};
  std::vector<rbd::MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  qp::PositionTask posTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.) * mbcInit.bodyPosW[bodyI].translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);

  double inf = std::numeric_limits<double>::infinity();
  std::vector<std::vector<double>> lBound = {{}, {-cst::pi<double>() / 4.}, {-inf}, {-inf}};
  std::vector<std::vector<double>> uBound = {{}, {cst::pi<double>() / 4.}, {inf}, {inf}};
  std::vector<std::vector<double>> lVel = {{}, {-inf}, {-inf}, {-inf}};
  std::vector<std::vector<double>> uVel = {{}, {inf}, {inf}, {inf}};

  qp::DamperJointLimitsConstr dampJointConstr(mbs, 0, {lBound, uBound}, {lVel, uVel}, 0.125, 0.025, 1., 0.001);

  // Test add*Constraint
  dampJointConstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test JointLimitsConstr
  mbcs[0] = mbcInit;
  for(int i = 0; i < 2000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE_GT(mbcs[0].q[1][0], -(cst::pi<double>() / 4.) * (1. - 0.025) - 0.01);
  }

  posTask.position(RotZ(-cst::pi<double>() / 2.) * mbcInit.bodyPosW[bodyI].translation());
  mbcs[0] = mbcInit;
  for(int i = 0; i < 2000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE_LT(mbcs[0].q[1][0], (cst::pi<double>() / 4.) * (1. - 0.025) + 0.01);
  }

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test remove*Constraint
  dampJointConstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);
}

BOOST_AUTO_TEST_CASE(QPMimicJointTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  /* Make a very simple gripper */
  MultiBodyGraph mbg;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d::Zero();

  RBInertiad rbi(mass, h, I);

  Body b0(rbi, "b0");
  Body b1(rbi, "b1");
  Body b2(rbi, "b2");
  Body b3(rbi, "b3");

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  Joint j0(Joint::RevZ, true, "j0");
  Joint j1(Joint::RevZ, true, "j1");
  j1.makeMimic("j0", -1.0, 0.0);
  Joint j2(Joint::RevZ, true, "j2");
  j2.makeMimic("j0", 0.0, 0.5);

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  PTransformd from = sva::PTransformd::Identity();

  mbg.linkBodies("b0", from, "b1", from, "j0");
  mbg.linkBodies("b1", from, "b2", from, "j1");
  mbg.linkBodies("b2", from, "b3", from, "j2");

  MultiBody mb = mbg.makeMultiBody("b0", true, from);

  MultiBodyConfig mbcInit(mb);
  mbcInit.zero(mb);
  BOOST_CHECK_EQUAL(mbcInit.q[3][0], 0.5);

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<rbd::MultiBody> mbs = {mb};
  std::vector<rbd::MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  tasks::qp::PostureTask pt(mbs, 0, mbcInit.q, 100.0, 10.0);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&pt);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test mimic joint in mbc
  mbcs[0] = mbcInit;
  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE(mbcs[0].q[1][0] == -mbcs[0].q[2][0]);
    BOOST_REQUIRE(mbcs[0].q[3][0] == 0.5);
  }
  BOOST_CHECK_SMALL(mbcs[0].q[1][0], 1e-4);

  mbcs[0] = mbcInit;
  pt.posture({{}, {0.2}, {-0.2}, {0.5}});
  for(int i = 0; i < 2000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    BOOST_REQUIRE(mbcs[0].q[1][0] == -mbcs[0].q[2][0]);
    BOOST_REQUIRE(mbcs[0].q[3][0] == 0.5);
  }
  BOOST_CHECK_SMALL(mbcs[0].q[1][0] - 0.2, 1e-4);

  solver.removeTask(&pt);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);
}

BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<rbd::MultiBody> mbs = {mb};
  std::vector<rbd::MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  qp::PositionTask posTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.) * mbcInit.bodyPosW[bodyI].translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);

  std::vector<std::vector<double>> lBound = {{}, {-30.}, {-30.}, {-30.}};
  std::vector<std::vector<double>> uBound = {{}, {30.}, {30.}, {30.}};
  std::vector<std::vector<double>> lBoundDt = {{}, {-500.}, {-500.}, {-500.}};
  std::vector<std::vector<double>> uBoundDt = {{}, {500.}, {500.}, {500.}};

  constexpr double dt = 0.001;

  qp::MotionConstr motionCstr(mbs, 0, {lBound, uBound}, {lBoundDt, uBoundDt}, dt);
  qp::PositiveLambda plCstr;

  // Test add*Constraint

  solver.addGenInequalityConstraint(&motionCstr);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 1);
  solver.addConstraint(&motionCstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  plCstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 2);

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  // Test MotionConstr torque limits
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], dt);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec());
    motionCstr.torque(mbs, mbcs);
    for(int i = 0; i < 3; ++i)
    {
      BOOST_REQUIRE_GT(mbcs[0].jointTorque[i + 1][0], lBound[i + 1][0] - 0.001);
      BOOST_REQUIRE_LT(mbcs[0].jointTorque[i + 1][0], uBound[i + 1][0] + 0.001);
    }
  }

  posTask.position(mbcInit.bodyPosW[bodyI].translation());
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], dt);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec());
    motionCstr.torque(mbs, mbcs);
    for(int i = 0; i < 3; ++i)
    {
      BOOST_REQUIRE_GT(mbcs[0].jointTorque[i + 1][0], lBound[i + 1][0] - 0.001);
      BOOST_REQUIRE_LT(mbcs[0].jointTorque[i + 1][0], uBound[i + 1][0] + 0.001);
    }
  }

  motionCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  Eigen::VectorXd lpoly(2), upoly(2);
  Eigen::VectorXd null;
  lpoly << -30, 1.;
  upoly << 30, 1.;
  std::vector<std::vector<Eigen::VectorXd>> lBoundPoly = {{null}, {lpoly}, {lpoly}, {lpoly}};
  std::vector<std::vector<Eigen::VectorXd>> uBoundPoly = {{null}, {upoly}, {upoly}, {upoly}};
  qp::MotionPolyConstr motionPolyCstr(mbs, 0, {lBoundPoly, uBoundPoly});

  motionPolyCstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 2);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  // Test MotionPolyConstr torque limits
  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    std::vector<std::vector<double>> oldQ = mbcs[0].q;
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    motionPolyCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec());
    motionPolyCstr.torque(mbs, mbcs);
    for(int i = 0; i < 3; ++i)
    {
      BOOST_REQUIRE_GT(mbcs[0].jointTorque[i + 1][0], Eigen::poly_eval(lBoundPoly[i + 1][0], oldQ[i + 1][0]));
      BOOST_REQUIRE_LT(mbcs[0].jointTorque[i + 1][0], Eigen::poly_eval(uBoundPoly[i + 1][0], oldQ[i + 1][0]));
    }
  }

  posTask.position(mbcInit.bodyPosW[bodyI].translation());
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    std::vector<std::vector<double>> oldQ = mbcs[0].q;
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
    motionPolyCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec());
    motionPolyCstr.torque(mbs, mbcs);
    for(int i = 0; i < 3; ++i)
    {
      BOOST_REQUIRE_GT(mbcs[0].jointTorque[i + 1][0], Eigen::poly_eval(lBoundPoly[i + 1][0], oldQ[i + 1][0]));
      BOOST_REQUIRE_LT(mbcs[0].jointTorque[i + 1][0], Eigen::poly_eval(uBoundPoly[i + 1][0], oldQ[i + 1][0]));
    }
  }

  motionPolyCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  plCstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrBoundConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);
}

BOOST_AUTO_TEST_CASE(QPAutoCollTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  qp::PositionTask posTask(mbs, 0, "b3", mbcInit.bodyPosW[bodyI].translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 50., 1.);

  sch::S_Sphere b0(0.25), b3(0.25);
  sch::CD_Pair pair(&b0, &b3);

  PTransformd I = PTransformd::Identity();
  qp::CollisionConstr autoCollConstr(mbs, 0.001);
  int collId1 = 10;
  autoCollConstr.addCollision(mbs, collId1, 0, "b0", &b0, I, 0, "b3", &b3, I, 0.01, 0.005, 1.);
  BOOST_CHECK_EQUAL(autoCollConstr.nrCollisions(), 1);

  // Test addInequalityConstraint
  solver.addInequalityConstraint(&autoCollConstr);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 1);
  solver.addConstraint(&autoCollConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  mbcs[0] = mbcInit;
  std::ofstream distHard("selfDistHard.py");
  distHard << "dist = [";
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    sch::Point3 pb1Tmp, pb2Tmp;
    double dist = pair.getClosestPoints(pb1Tmp, pb2Tmp);
    dist = dist >= 0. ? std::sqrt(dist) : -std::sqrt(-dist);
    distHard << dist << ", ";
    BOOST_REQUIRE_GT(dist, 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }
  distHard << "]" << std::endl;

  autoCollConstr.rmCollision(collId1);
  BOOST_CHECK_EQUAL(autoCollConstr.nrCollisions(), 0);

  // test automatic damping computation
  autoCollConstr.addCollision(mbs, collId1, 0, "b0", &b0, I, 0, "b3", &b3, I, 0.1, 0.01, 0., 0.1);
  BOOST_CHECK_EQUAL(autoCollConstr.nrCollisions(), 1);
  posTask.position(mbcInit.bodyPosW[bodyI].translation());

  mbcs[0] = mbcInit;
  std::ofstream distSoft("selfDistSoft.py");
  distSoft << "dist = [";
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    sch::Point3 pb1Tmp, pb2Tmp;
    double dist = pair.getClosestPoints(pb1Tmp, pb2Tmp);
    dist = dist >= 0. ? std::sqrt(dist) : -std::sqrt(-dist);
    distSoft << dist << ", ";
    BOOST_REQUIRE_GT(dist, 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }
  distSoft << "]" << std::endl;

  autoCollConstr.rmCollision(collId1);
  BOOST_CHECK_EQUAL(autoCollConstr.nrCollisions(), 0);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test remove*Constraint
  solver.removeInequalityConstraint(&autoCollConstr);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 0);
  solver.removeConstraint(&autoCollConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);
}

BOOST_AUTO_TEST_CASE(QPStaticEnvCollTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb, mbEnv;
  MultiBodyConfig mbcInit, mbcEnv;

  std::tie(mb, mbcInit) = makeZXZArm();
  std::tie(mbEnv, mbcEnv) = makeEnv();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);
  forwardKinematics(mbEnv, mbcEnv);
  forwardVelocity(mbEnv, mbcEnv);

  std::vector<MultiBody> mbs = {mb, mbEnv};
  std::vector<MultiBodyConfig> mbcs = {mbcInit, mbcEnv};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  qp::PositionTask posTask(mbs, 0, "b3", mbcInit.bodyPosW[bodyI].translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 50., 1.);

  sch::S_Sphere b0(0.25), b3(0.25);
  sch::CD_Pair pair(&b0, &b3);

  b0.setTransformation(qp::tosch(mbcInit.bodyPosW[0]));

  PTransformd I = PTransformd::Identity();
  qp::CollisionConstr seCollConstr(mbs, 0.001);
  int collId1 = 10;
  seCollConstr.addCollision(mbs, collId1, 0, "b3", &b3, I, 1, "b0", &b0, I, 0.01, 0.005, 1.);
  BOOST_CHECK_EQUAL(seCollConstr.nrCollisions(), 1);

  // Test addInequalityConstraint
  solver.addInequalityConstraint(&seCollConstr);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 1);
  solver.addConstraint(&seCollConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  mbcs[0] = mbcInit;
  std::ofstream distHard("staticDistHard.py");
  distHard << "dist = [";
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    sch::Point3 pb1Tmp, pb2Tmp;
    double dist = pair.getClosestPoints(pb1Tmp, pb2Tmp);
    dist = dist >= 0 ? std::sqrt(dist) : -std::sqrt(-dist);
    distHard << dist << ", ";
    BOOST_REQUIRE_GT(dist, 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }
  distHard << "]" << std::endl;

  seCollConstr.rmCollision(collId1);
  BOOST_CHECK_EQUAL(seCollConstr.nrCollisions(), 0);

  // test damping computation
  seCollConstr.addCollision(mbs, collId1, 0, "b3", &b3, I, 1, "b0", &b0, I, 0.1, 0.01, 0., 0.1);
  BOOST_CHECK_EQUAL(seCollConstr.nrCollisions(), 1);
  posTask.position(mbcInit.bodyPosW[bodyI].translation());

  mbcs[0] = mbcInit;
  std::ofstream distSoft("staticDistSoft.py");
  distSoft << "dist = [";
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    sch::Point3 pb1Tmp, pb2Tmp;
    double dist = pair.getClosestPoints(pb1Tmp, pb2Tmp);
    dist = dist >= 0 ? std::sqrt(dist) : -std::sqrt(-dist);
    distSoft << dist << ", ";
    BOOST_REQUIRE_GT(dist, 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }
  distSoft << "]" << std::endl;

  seCollConstr.rmCollision(collId1);
  BOOST_CHECK_EQUAL(seCollConstr.nrCollisions(), 0);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);

  // Test remove*Constraint
  solver.removeInequalityConstraint(&seCollConstr);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 0);
  solver.removeConstraint(&seCollConstr);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);
}

BOOST_AUTO_TEST_CASE(QPBilatContactTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb, mbEnv;
  MultiBodyConfig mbcInit, mbcEnv;

  std::tie(mb, mbcInit) = makeZXZArm(false);
  std::tie(mbEnv, mbcEnv) = makeEnv();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);
  forwardKinematics(mbEnv, mbcEnv);
  forwardVelocity(mbEnv, mbcEnv);

  std::vector<MultiBody> mbs = {mb, mbEnv};
  std::vector<MultiBodyConfig> mbcs = {mbcInit, mbcEnv};

  qp::QPSolver solver;

  double Inf = std::numeric_limits<double>::infinity();
  std::vector<std::vector<double>> torqueMin = {{0., 0., 0., 0., 0., 0.}, {-Inf}, {-Inf}, {-Inf}};
  std::vector<std::vector<double>> torqueMax = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}};
  std::vector<std::vector<double>> torqueDtMin = {{0., 0., 0., 0., 0., 0.}, {-Inf}, {-Inf}, {-Inf}};
  std::vector<std::vector<double>> torqueDtMax = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}};
  qp::MotionConstr motionCstr(mbs, 0, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005);
  qp::PositiveLambda plCstr;
  qp::ContactAccConstr contCstrAcc;

  solver.addGenInequalityConstraint(&motionCstr);
  solver.addConstraint(&motionCstr);

  solver.addEqualityConstraint(&contCstrAcc);
  solver.addConstraint(&contCstrAcc);

  plCstr.addToSolver(solver);

  std::vector<Eigen::Vector3d> points = {Vector3d(0.1, 0.1, 0.), Vector3d(-0.1, 0.1, 0.), Vector3d(-0.1, -0.1, 0.),
                                         Vector3d(0.1, -0.1, 0.)};

  std::vector<Eigen::Matrix3d> biFrames = {
      sva::RotY((0. * cst::pi<double>()) / 2.),
      sva::RotY((1. * cst::pi<double>()) / 2.),
      sva::RotY((2. * cst::pi<double>()) / 2.),
      sva::RotY((3. * cst::pi<double>()) / 2.),
  };

  std::vector<qp::UnilateralContact> uni = {
      qp::UnilateralContact(0, 1, "b0", "b0", points, Matrix3d::Identity(), sva::PTransformd::Identity(), 3, 0.7)};
  std::vector<qp::BilateralContact> bi = {
      qp::BilateralContact(0, 1, "b0", "b0", points, biFrames, sva::PTransformd::Identity(), 3, 0.7)};

  solver.nrVars(mbs, uni, {});
  solver.updateConstrSize();
  BOOST_CHECK_EQUAL(solver.nrVars(), 9 + 4 * 3);

  // This stance with unilateral contac is impossible so the solver must fail
  // The forces are apply on the Z axis and the gravity come frome the Y axis
  mbcs[0] = mbcInit;
  BOOST_REQUIRE(!solver.solve(mbs, mbcs));

  // We test it again with bilateral contact to check that the stance is now
  // valid
  // the forces are apply on the Z, Y, -Z and -Y axis
  solver.nrVars(mbs, {}, bi);
  solver.updateConstrSize();
  BOOST_CHECK_EQUAL(solver.nrVars(), 9 + 4 * 3);

  mbcs[0] = mbcInit;
  for(int i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  plCstr.removeFromSolver(solver);

  solver.removeEqualityConstraint(&contCstrAcc);
  solver.removeConstraint(&contCstrAcc);

  solver.removeConstraint(&motionCstr);
  solver.removeGenInequalityConstraint(&motionCstr);
}

Eigen::Vector6d compute6dError(const sva::PTransformd & b1, const sva::PTransformd & b2)
{
  Eigen::Vector6d error;
  error.head<3>() = sva::rotationError(b1.rotation(), b2.rotation());
  error.tail<3>() = b1.translation() - b2.translation();
  return error;
}

Eigen::Vector6d compute6dErrorInB1(const sva::PTransformd & b1, const sva::PTransformd & b2)
{
  sva::MotionVecd error;
  error.angular() = sva::rotationError(b1.rotation(), b2.rotation());
  error.linear() = b1.translation() - b2.translation();
  return (sva::PTransformd(b1.rotation()) * error).vector();
}

double computeDofError(const sva::PTransformd & b1, const sva::PTransformd & b2, const Eigen::MatrixXd & dof)
{
  return (dof * compute6dError(b1, b2)).norm();
}

double computeDofErrorInB1(const sva::PTransformd & b1, const sva::PTransformd & b2, const Eigen::MatrixXd & dof)
{
  return (dof * compute6dErrorInB1(b1, b2)).norm();
}

BOOST_AUTO_TEST_CASE(QPDofContactsTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb, mbEnv;
  MultiBodyConfig mbcInit, mbcEnv;

  std::tie(mb, mbcInit) = makeZXZArm(false);
  std::tie(mbEnv, mbcEnv) = makeEnv();

  Vector4d quat(Vector4d::Random().normalized());
  mbcInit.q[0] = {quat.w(), quat.x(), quat.y(), quat.z(), 0., 0., 0.};

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);
  forwardKinematics(mbEnv, mbcEnv);
  forwardVelocity(mbEnv, mbcEnv);

  std::vector<MultiBody> mbs = {mb, mbEnv};
  std::vector<MultiBodyConfig> mbcs = {mbcInit, mbcEnv};

  qp::QPSolver solver;

  sva::PTransformd X_b1_cf(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());

  qp::ContactPosConstr contCstrSpeed(0.005);
  // target in cf coordinate is transform into world frame
  qp::PositionTask posTask(mbs, 0, "b0",
                           (X_b1_cf * mbcInit.bodyPosW[0]).rotation().transpose() * Vector3d(1., 1., -1.));
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 10., 1.);
  // Rotation in cf coordinate
  qp::OrientationTask oriTask(mbs, 0, "b0",
                              (X_b1_cf * mbcInit.bodyPosW[0]).rotation().transpose() * sva::RotY(0.1) * sva::RotX(0.5));
  qp::SetPointTask oriTaskSp(mbs, 0, &oriTask, 10., 1.);

  contCstrSpeed.addToSolver(solver);
  solver.addTask(&posTaskSp);

  std::vector<Eigen::Vector3d> points = {Vector3d(0.1, 0.1, 0.), Vector3d(-0.1, 0.1, 0.), Vector3d(-0.1, -0.1, 0.),
                                         Vector3d(0.1, -0.1, 0.)};

  sva::PTransformd X_b1_b2(mbcEnv.bodyPosW[0] * mbcInit.bodyPosW[0].inv());
  std::vector<qp::UnilateralContact> uni = {
      qp::UnilateralContact(0, 1, "b0", "b0", points, Matrix3d::Identity(), X_b1_b2, 3, 0.7, X_b1_cf)};

  // contactDof must be provide in r1BodyId frame
  MatrixXd contactDof(5, 6);
  contactDof.setZero();
  contactDof(0, 0) = 1.;
  contactDof(1, 1) = 1.;
  contactDof(2, 2) = 1.;
  contactDof(3, 3) = 1.;
  contactDof(4, 4) = 1.;

  // test Z free
  contCstrSpeed.addDofContact({0, 1, "b0", "b0"}, contactDof);
  solver.nrVars(mbs, uni, {});
  solver.updateConstrSize();

  mbcs[0] = mbcInit;
  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.005);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL(computeDofErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0], contactDof),
                    1e-6);
  BOOST_CHECK_GT(std::pow(compute6dErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0])(5), 2), 0.1);

  // test Y Free and updateDofContacts
  contactDof.setZero();
  contactDof(0, 0) = 1.;
  contactDof(1, 1) = 1.;
  contactDof(2, 2) = 1.;
  contactDof(3, 3) = 1.;
  contactDof(4, 5) = 1.;
  contCstrSpeed.resetDofContacts();
  contCstrSpeed.addDofContact({0, 1, "b0", "b0"}, contactDof);
  contCstrSpeed.updateDofContacts();
  mbcs[0] = mbcInit;
  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.005);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL(computeDofErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0], contactDof),
                    1e-6);
  BOOST_CHECK_GT(std::pow(compute6dErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0])(4), 2), 0.1);

  // test WX Free and updateDofContacts
  contactDof.setZero();
  contactDof(0, 1) = 1.;
  contactDof(1, 2) = 1.;
  contactDof(2, 3) = 1.;
  contactDof(3, 4) = 1.;
  contactDof(4, 5) = 1.;
  contCstrSpeed.resetDofContacts();
  contCstrSpeed.addDofContact({0, 1, "b0", "b0"}, contactDof);
  contCstrSpeed.updateDofContacts();

  // add the orientation task in cf coordinate
  solver.addTask(&oriTaskSp);
  solver.updateTasksNrVars(mbs);
  mbcs[0] = mbcInit;
  for(int i = 0; i < 1000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.005);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  BOOST_CHECK_SMALL(computeDofErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0], contactDof),
                    1e-4);
  BOOST_CHECK_GT(std::abs(compute6dErrorInB1(X_b1_cf * mbcs[0].bodyPosW[0], X_b1_cf * mbcInit.bodyPosW[0])(0)), 0.1);
}

BOOST_AUTO_TEST_CASE(QPBoundedSpeedTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;
  solver.solver("QLD");

  std::string bodyName("b3");
  int bodyIndex = mb.bodyIndexByName(bodyName);
  sva::PTransformd bodyPoint(Vector3d(0., 0.1, 0.));

  qp::BoundedSpeedConstr constSpeed(mbs, 0, 0.005);
  qp::PostureTask postureTask(mbs, 0, {{}, {0.}, {0.}, {0.}}, 1., 0.01);
  qp::PositionTask posTask(mbs, 0, bodyName, Vector3d(1., -1., 1.), bodyPoint.translation());
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 20., 1.);
  MatrixXd dof(1, 6);
  VectorXd speed(1);

  // X body axis must have 0 velocity
  dof << 0., 0., 0., 1., 0., 0.;
  speed << 0.;
  constSpeed.addBoundedSpeed(mbs, bodyName, bodyPoint.translation(), dof, speed);
  BOOST_CHECK_EQUAL(constSpeed.nrBoundedSpeeds(), 1);

  constSpeed.addToSolver(solver);
  solver.addTask(&postureTask);
  solver.addTask(&posTaskSp);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  mbcs[0] = mbcInit;
  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.005);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  sva::PTransformd initPos(bodyPoint * mbcInit.bodyPosW[bodyIndex]);
  sva::PTransformd finalPos(bodyPoint * mbcs[0].bodyPosW[bodyIndex]);
  BOOST_CHECK_SMALL(computeDofError(finalPos, initPos, dof), 1e-6);
  BOOST_CHECK_GT(std::pow(compute6dError(finalPos, initPos)(4), 2), 0.1);

  // same test but with Z axis
  BOOST_CHECK(constSpeed.removeBoundedSpeed(bodyName));
  constSpeed.updateBoundedSpeeds();
  BOOST_CHECK_EQUAL(constSpeed.nrBoundedSpeeds(), 0);
  BOOST_CHECK_EQUAL(constSpeed.maxGenInEq(), 0);

  // must resize constraint matrix since nrMaxIneq has changed
  solver.updateConstrSize();
  dof << 0., 0., 0., 0., 0., 1.;
  constSpeed.addBoundedSpeed(mbs, bodyName, bodyPoint.translation(), dof, speed);
  constSpeed.updateBoundedSpeeds();
  BOOST_CHECK_EQUAL(constSpeed.nrBoundedSpeeds(), 1);
  BOOST_CHECK_EQUAL(constSpeed.maxGenInEq(), 1);
  solver.updateConstrSize();

  mbcs[0] = mbcInit;
  for(int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.005);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);
  }

  initPos = bodyPoint * mbcInit.bodyPosW[bodyIndex];
  finalPos = bodyPoint * mbcs[0].bodyPosW[bodyIndex];
  BOOST_CHECK_SMALL(computeDofError(finalPos, initPos, dof), 1e-6);
  BOOST_CHECK_GT(std::pow(compute6dError(finalPos, initPos)(4), 2), 0.1);
}

BOOST_AUTO_TEST_CASE(MomentumTask)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  sva::ForceVecd momTarget(Vector3d(1., 1., 1.), Vector3d(0., 0., 0.));

  qp::MomentumTask momTask(mbs, 0, momTarget);
  qp::SetPointTask momTaskSp(mbs, 0, &momTask, 10., 1.);

  // Test addTask
  solver.addTask(&momTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test MomentumTask
  BOOST_REQUIRE(solver.solve(mbs, mbcs));
}

BOOST_AUTO_TEST_CASE(SurfaceOrientationTask)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  qp::SurfaceOrientationTask surfOriTask(mbs, 0, "b3", RotZ(cst::pi<double>() / 2.),
                                         sva::PTransformd(Vector3d(0., 0., 0.)));
  qp::SetPointTask surfOriTaskSp(mbs, 0, &surfOriTask, 10., 1.);

  // Test addTask
  solver.addTask(&surfOriTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test MomentumTask
  BOOST_REQUIRE(solver.solve(mbs, mbcs));
}

BOOST_AUTO_TEST_CASE(QPCoMPlaneTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm();

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  int bodyI = mb.bodyIndexByName("b3");
  Eigen::Vector3d initPos(mbcInit.bodyPosW[bodyI].translation());
  qp::PositionTask posTask(mbs, 0, "b3", initPos);
  qp::SetPointTask posTaskSp(mbs, 0, &posTask, 50., 1.);
  Eigen::Vector3d n1(0., 0., -1.);
  Eigen::Vector3d n2(0., 0., 1.);
  Eigen::Vector3d p1(0., 0., 0.1);
  Eigen::Vector3d p2(0., 0., -0.1);
  double offset1 = -n1.dot(p1);
  double offset2 = -n2.dot(p2);

  qp::CoMIncPlaneConstr comPlaneConstr(mbs, 0, 0.001);
  int planeId1 = 10;
  int planeId2 = 20;
  comPlaneConstr.addPlane(planeId1, n1, offset1, 0.01, 0.005, 0., 0.1);
  comPlaneConstr.addPlane(planeId2, n2, offset2, 0.01, 0.005, 0.1, 0.);
  BOOST_CHECK_EQUAL(comPlaneConstr.nrPlanes(), 2);

  comPlaneConstr.addToSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 1);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 1);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  solver.addTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  mbcs[0] = mbcInit;
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);

    // check if the CoM is on the good side of each plane
    Eigen::Vector3d com = rbd::computeCoM(mbs[0], mbcs[0]);
    double dist1 = n1.dot(com) + offset1;
    double dist2 = n2.dot(com) + offset2;
    BOOST_REQUIRE_GT(dist1, 0.005);
    BOOST_REQUIRE_GT(dist2, 0.005);
  }

  // inverse rotation side
  mbcs[0] = mbcInit;
  posTask.position(initPos);
  for(int i = 0; i < 1000; ++i)
  {
    posTask.position(RotX(-0.01) * posTask.position());
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mbs[0], mbcs[0], 0.001);

    forwardKinematics(mbs[0], mbcs[0]);
    forwardVelocity(mbs[0], mbcs[0]);

    // check if the CoM is on the good side of each plane
    Eigen::Vector3d com = rbd::computeCoM(mbs[0], mbcs[0]);
    double dist1 = n1.dot(com) + offset1;
    double dist2 = n2.dot(com) + offset2;
    BOOST_REQUIRE_GT(dist1, 0.005);
    BOOST_REQUIRE_GT(dist2, 0.005);
  }

  comPlaneConstr.rmPlane(planeId1);
  comPlaneConstr.rmPlane(planeId2);
  BOOST_CHECK_EQUAL(comPlaneConstr.nrPlanes(), 0);

  comPlaneConstr.removeFromSolver(solver);
  BOOST_CHECK_EQUAL(solver.nrInequalityConstraints(), 0);
  BOOST_CHECK_EQUAL(solver.nrConstraints(), 0);

  solver.removeTask(&posTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);
}

BOOST_AUTO_TEST_CASE(JointsSelector)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm(false);

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs = {mbcInit};

  qp::QPSolver solver;

  qp::PositionTask pt(mbs, 0, "b3", Vector3d::Zero());
  // construct two JointSelector that should have the same joints
  qp::JointsSelector js1(qp::JointsSelector::ActiveJoints(mbs, 0, &pt, {"j2", "j1"}));
  qp::JointsSelector js2(qp::JointsSelector::UnactiveJoints(mbs, 0, &pt, {"j0", "Root"}));
  qp::SetPointTask js1Sp(mbs, 0, &js1, 1., 1.);
  qp::SetPointTask js2Sp(mbs, 0, &js2, 1., 1.);

  BOOST_REQUIRE_EQUAL(js1.selectedJoints().size(), 2);
  BOOST_REQUIRE_EQUAL(js2.selectedJoints().size(), 2);
  // check they have the same joints selected
  for(std::size_t i = 0; i < 2; ++i)
  {
    BOOST_REQUIRE_EQUAL(js1.selectedJoints()[i].posInDof, js2.selectedJoints()[i].posInDof);
    BOOST_REQUIRE_EQUAL(js1.selectedJoints()[i].dof, js2.selectedJoints()[i].dof);
  }
  // check joints are sorted
  BOOST_REQUIRE_LT(js1.selectedJoints()[0].posInDof, js1.selectedJoints()[1].posInDof);

  solver.addTask(&js1Sp);
  solver.addTask(&js2Sp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 2);

  solver.nrVars(mbs, {}, {});
  solver.updateConstrSize();

  // Test JointsSelector
  BOOST_REQUIRE(solver.solve(mbs, mbcs));

  // jacobian first column should be zero
  BOOST_REQUIRE_EQUAL(js1.jac().block(0, 0, 3, 7), MatrixXd::Zero(3, 7));

  // matrix should be equals
  BOOST_REQUIRE_EQUAL(js1.jac(), js2.jac());
  BOOST_REQUIRE_EQUAL(js1.eval(), js2.eval());
  BOOST_REQUIRE_EQUAL(js1.speed(), js2.speed());
  BOOST_REQUIRE_EQUAL(js1.normalAcc(), js2.normalAcc());
}

BOOST_AUTO_TEST_CASE(QPTransformTaskTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace tasks;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbcInit;

  std::tie(mb, mbcInit) = makeZXZArm(false);

  std::vector<MultiBody> mbs = {mb};
  std::vector<MultiBodyConfig> mbcs(1);

  forwardKinematics(mb, mbcInit);
  forwardVelocity(mb, mbcInit);

  qp::QPSolver solver;

  solver.nrVars(mbs, {}, {});
  BOOST_CHECK_EQUAL(solver.nrVars(), 9);

  solver.updateConstrSize();

  // Test TransformTask
  sva::PTransformd X_b_s(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  sva::PTransformd X_0_t(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  Quaterniond E_0_c(Vector4d::Random().normalized());

  qp::TransformTask transTask(mbs, 0, "b3", X_0_t, X_b_s, E_0_c.matrix());
  VectorXd dimW(6);
  dimW << 1., 1., 1., 1., 1., 1.;
  qp::SetPointTask transTaskSp(mbs, 0, &transTask, 10., dimW, 100.);
  // must add postureTask to avoid singularity
  qp::PostureTask postureTask(mbs, 0, {{}, {0.}, {0.}, {0.}}, .5, 10.);

  // addTask all the tasks
  solver.addTask(&transTaskSp);
  solver.addTask(&postureTask);
  solver.updateTasksNrVars(mbs);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 2);

  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(transTask.eval().norm(), 1e-5);

  // removeTask transTaskSp
  solver.removeTask(&transTaskSp);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 1);

  // Test SurfaceTransformTask
  qp::SurfaceTransformTask surfTransTask(mbs, 0, "b3", X_0_t, X_b_s);
  qp::SetPointTask surfTransTaskSp(mbs, 0, &surfTransTask, 10., dimW, 100.);
  solver.addTask(&surfTransTaskSp);
  solver.updateTasksNrVars(mbs);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 2);

  mbcs[0] = mbcInit;
  for(int i = 0; i < 10000; ++i)
  {
    BOOST_REQUIRE(solver.solve(mbs, mbcs));
    eulerIntegration(mb, mbcs[0], 0.001);

    forwardKinematics(mb, mbcs[0]);
    forwardVelocity(mb, mbcs[0]);
  }

  BOOST_CHECK_SMALL(surfTransTask.eval().norm(), 1e-5);

  solver.removeTask(&surfTransTaskSp);
  solver.removeTask(&postureTask);
  BOOST_CHECK_EQUAL(solver.nrTasks(), 0);
}