/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
#include "CxxStd.hpp"

#include "NewtonEulerDS.hpp"
#include "BlockVector.hpp"
#include "BlockMatrix.hpp"
#include <boost/math/quaternion.hpp>

#include <iostream>
//#define DEBUG_NOCOLOR
//#define DEBUG_BEGIN_END_ONLY
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include <debug.h>

void computeRotationMatrix(double q0, double q1, double q2, double q3,
                           SP::SimpleMatrix rotationMatrix)
{

  /* Brute force version by multiplication of quaternion
   */
  // ::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
  // ::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
  // ::boost::math::quaternion<double>    quatx(0, 1, 0, 0);
  // ::boost::math::quaternion<double>    quaty(0, 0, 1, 0);
  // ::boost::math::quaternion<double>    quatz(0, 0, 0, 1);
  // ::boost::math::quaternion<double>    quatBuff;
  // quatBuff = quatQ * quatx * quatcQ;
  // rotationMatrix->setValue(0, 0, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 0, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 0, quatBuff.R_component_4());
  // quatBuff = quatQ * quaty * quatcQ;
  // rotationMatrix->setValue(0, 1, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 1, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 1, quatBuff.R_component_4());
  // quatBuff = quatQ * quatz * quatcQ;
  // rotationMatrix->setValue(0, 2, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 2, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 2, quatBuff.R_component_4());

  /* direct computation https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation */
  rotationMatrix->setValue(0, 0,     q0*q0 +q1*q1 -q2*q2 -q3*q3);
  rotationMatrix->setValue(0, 1, 2.0*(q1*q2        - q0*q3));
  rotationMatrix->setValue(0, 2, 2.0*(q1*q3        + q0*q2));

  rotationMatrix->setValue(1, 0, 2.0*(q1*q2        + q0*q3));
  rotationMatrix->setValue(1, 1,     q0*q0 -q1*q1 +q2*q2 -q3*q3);
  rotationMatrix->setValue(1, 2, 2.0*(q2*q3        - q0*q1));

  rotationMatrix->setValue(2, 0, 2.0*(q1*q3        - q0*q2));
  rotationMatrix->setValue(2, 1, 2.0*(q2*q3         + q0*q1));
  rotationMatrix->setValue(2, 2,     q0*q0 -q1*q1 -q2*q2 +q3*q3);
}

static
void computeJacobianConvectedVectorInBodyFrame(double q0, double q1, double q2, double q3,
                                               SP::SimpleMatrix jacobian, SP::SiconosVector v)
{

  /* This routine compute the jacobian with respect to p of R^T(p)v */
  jacobian->zero();

  double v0 = v->getValue(0);
  double v1 = v->getValue(1);
  double v2 = v->getValue(2);

  jacobian->setValue(0,3, q0*v0+q3*v1-q2*v2);
  jacobian->setValue(0,4, q1*v0+q2*v1+q3*v2);
  jacobian->setValue(0,5,-q2*v0+q1*v1-q0*v2);
  jacobian->setValue(0,6,-q3*v0+q0*v1+q1*v2);

  jacobian->setValue(1,3,-q3*v0+q0*v1+q1*v2);
  jacobian->setValue(1,4, q2*v0-q1*v1+q0*v2);
  jacobian->setValue(1,5, q1*v0+q2*v1+q3*v2);
  jacobian->setValue(1,6,-q0*v0-q3*v1+q2*v2);

  jacobian->setValue(2,3, q2*v0-q1*v1+q0*v2);
  jacobian->setValue(2,4, q3*v0-q0*v1-q1*v2);
  jacobian->setValue(2,5, q0*v0+q3*v1-q2*v2);
  jacobian->setValue(2,6, q1*v0+q2*v1+q3*v2);

  *jacobian *=2.0;
}

void rotateAbsToBody(double q0, double q1, double q2, double q3, SiconosVector& v)
{
  DEBUG_BEGIN("::rotateAbsToBody(double q0, double q1, double q2, double q3, SiconosVector& v )\n");
  DEBUG_EXPR(v.display(););
  DEBUG_PRINTF("( q0 = %16.12e,  q1 = %16.12e,  q2= %16.12e,  q3= %16.12e )\n", q0,q1,q2,q3);
  assert(v.size()==3);

  // First way. Using the rotation matrix
  // SP::SimpleMatrix rotationMatrix(new SimpleMatrix(3,3));
  // SiconosVector tmp(3);
  // ::computeRotationMatrix(q0,q1,q2,q3, rotationMatrix);
  // prod(*rotationMatrix, v, tmp);
  // v = tmp;
  // return;

  // Second way. Using the transpose of the rotation matrix
  // SP::SimpleMatrix rotationMatrix(new SimpleMatrix(3,3));
  // SiconosVector tmp(3);
  // ::computeRotationMatrix(q0,-q1,-q2,-q3, rotationMatrix);
  // prod(v, *rotationMatrix, tmp);
  // v = tmp;

  // Third way. cross product and axis angle
  // see http://www.geometrictools.com/Documentation/RotationIssues.pdf
  // SP::SiconosVector axis(new SiconosVector(3));
  // double angle = ::axisAngleFromQuaternion(q0,q1,q2,q3, axis);
  // SiconosVector t(3), tmp(3);
  // cross_product(*axis,v,t);
  // cross_product(*axis,t,tmp);
  // v += sin(angle)*t + (1.0-cos(angle))*tmp;

  // Direct computation with cross product
  // Works only with unit quaternion
  SiconosVector t(3), tmp(3);
  SiconosVector qvect(3);
  qvect(0)=q1;
  qvect(1)=q2;
  qvect(2)=q3;
  cross_product(qvect,v,t);
  t *= 2.0;
  cross_product(qvect,t,tmp);
  v += tmp;
  v += q0*t;
  DEBUG_EXPR(v.display(););
  DEBUG_END("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )\n");
}

void rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v)
{
  ::rotateAbsToBody(q0, q1, q2, q3, *v);
}

void rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SimpleMatrix m )\n");
  DEBUG_EXPR(m->display(););
  DEBUG_PRINTF("( q0 = %16.12e,  q1 = %16.12e,  q2= %16.12e,  q3= %16.12e )\n", q0,q1,q2,q3);

  // Direct computation with cross product for each column
  assert(m->size(0) == 3 && "::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SimpleMatrix m ) m must have 3 rows");
  SiconosVector v(3);
  SiconosVector t(3), tmp(3);
  SiconosVector qvect(3);
  qvect(0)=q1;
  qvect(1)=q2;
  qvect(2)=q3;
  for(unsigned int j = 0; j < m->size(1); j++)
  {
    v(0) = m->getValue(0,j);
    v(1) = m->getValue(1,j);
    v(2) = m->getValue(2,j);
    cross_product(qvect,v,t);
    t *= 2.0;
    cross_product(qvect,t,tmp);
    v += tmp;
    v += q0*t;
    m->setValue(0,j,v(0));
    m->setValue(1,j,v(1));
    m->setValue(2,j,v(2));
  }
  DEBUG_EXPR(m->display(););
  DEBUG_END("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SimpleMatrix m )\n");
}

void rotateAbsToBody(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::rotateAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::rotateAbsToBody(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), v);
  DEBUG_END("::rotateAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
}

void rotateAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::rotateAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::rotateAbsToBody(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),m);
  DEBUG_END("::rotateAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m)\n");
}

void changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v )\n");
  ::rotateAbsToBody(q.getValue(3),-q.getValue(4),-q.getValue(5),-q.getValue(6), v);
  DEBUG_END("::changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v )\n");
}
void changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::rotateAbsToBody(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6), v);
  DEBUG_END("::changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
}
void changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::rotateAbsToBody(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6), m);
  DEBUG_END("::changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m )\n");
}

void changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v )\n");
  ::rotateAbsToBody(q.getValue(3),q.getValue(4),q.getValue(5),q.getValue(6), v);
  DEBUG_END("::changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v )\n");
}
void changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::rotateAbsToBody(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), *v);
  DEBUG_END("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v )\n");
}
void changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::rotateAbsToBody(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), m);
  DEBUG_END("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m )\n");
}

void computeRotationMatrix(SP::SiconosVector q, SP::SimpleMatrix rotationMatrix)
{
  ::computeRotationMatrix(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),
                          rotationMatrix);
}
void computeRotationMatrixTransposed(SP::SiconosVector q, SP::SimpleMatrix rotationMatrix)
{
  ::computeRotationMatrix(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6),
                          rotationMatrix);
}

double axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis)
{
  DEBUG_BEGIN("axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis )\n");
  double angle = acos(q0) *2.0;
  //double f = sin( angle *0.5);
  double f = sqrt(1-q0*q0); // cheaper than sin ?
  if(f !=0.0)
  {
    axis->setValue(0, q1/f);
    axis->setValue(1, q2/f);
    axis->setValue(2, q3/f);
  }
  else
  {
    axis->zero();
  }
  DEBUG_PRINTF("angle= %12.8e\n", angle);
  DEBUG_EXPR(axis->display(););
  DEBUG_END("axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis )\n");
  return angle;
}

double axisAngleFromQuaternion(SP::SiconosVector q, SP::SiconosVector axis)
{
  double angle = ::axisAngleFromQuaternion(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),axis);
  return angle;
}

void rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector)
{
  DEBUG_BEGIN("rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector )\n");

  rotationVector->setValue(0, q1);
  rotationVector->setValue(1, q2);
  rotationVector->setValue(2, q3);

  double norm_v = sqrt(q1*q1+q2*q2+q3*q3);
  assert(norm_v <= M_PI);  /* it should be called for a unit quaternion */
  if(norm_v < 1e-12)
  {
    rotationVector->setValue(0, 0.0);
    rotationVector->setValue(1, 0.0);
    rotationVector->setValue(2, 0.0);
  }
  else
  {
    *rotationVector *=  2.0 * asin(norm_v)/norm_v;
  }
  DEBUG_EXPR(rotationVector->display(););
  DEBUG_END("rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector )\n");
}

void rotationVectorFromQuaternion(SP::SiconosVector q, SP::SiconosVector rotationVector)
{
  ::rotationVectorFromQuaternion(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), rotationVector);
}

void quaternionFromAxisAngle(SP::SiconosVector axis, double angle, SP::SiconosVector q)
{
  q->setValue(3,cos(angle/2.0));
  q->setValue(4,axis->getValue(0)* sin(angle *0.5));
  q->setValue(5,axis->getValue(1)* sin(angle *0.5));
  q->setValue(6,axis->getValue(2)* sin(angle *0.5));
}

static
double sin_x(double x)
{
  if(std::abs(x) <= 1e-3)
  {
    return 1.0 + x*x / 3.0 + pow(x,4) * 2.0 / 15.0 + pow(x,6) * 17.0 / 315.0 + pow(x,8) * 62.0 / 2835.0;
  }
  else
  {
    return sin(x)/x;
  }
}

void quaternionFromRotationVector(SP::SiconosVector rotationVector, SP::SiconosVector q)
{
  double angle = sqrt(rotationVector->getValue(0)*rotationVector->getValue(0)+
               rotationVector->getValue(1)*rotationVector->getValue(1)+
               rotationVector->getValue(2)*rotationVector->getValue(2));

  double f = 0.5 * sin_x(angle *0.5);

  q->setValue(3,cos(angle/2.0));
  q->setValue(4,rotationVector->getValue(0)* f);
  q->setValue(5,rotationVector->getValue(1)* f);
  q->setValue(6,rotationVector->getValue(2)* f);
}

void normalizeq(SP::SiconosVector q)
{
  double normq = sqrt(q->getValue(3) * q->getValue(3) +
                      q->getValue(4) * q->getValue(4) +
                      q->getValue(5) * q->getValue(5) +
                      q->getValue(6) * q->getValue(6));
  assert(normq > 0);
  normq = 1.0 / normq;
  q->setValue(3, q->getValue(3) * normq);
  q->setValue(4, q->getValue(4) * normq);
  q->setValue(5, q->getValue(5) * normq);
  q->setValue(6, q->getValue(6) * normq);
}

void computeT(SP::SiconosVector q, SP::SimpleMatrix T)
{
  DEBUG_BEGIN("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")
  //  std::cout <<"\n NewtonEulerDS::computeT(SP::SiconosVector q)\n  " <<std::endl;
  double q0 = q->getValue(3) / 2.0;
  double q1 = q->getValue(4) / 2.0;
  double q2 = q->getValue(5) / 2.0;
  double q3 = q->getValue(6) / 2.0;
  T->setValue(3, 3, -q1);
  T->setValue(3, 4, -q2);
  T->setValue(3, 5, -q3);
  T->setValue(4, 3, q0);
  T->setValue(4, 4, -q3);
  T->setValue(4, 5, q2);
  T->setValue(5, 3, q3);
  T->setValue(5, 4, q0);
  T->setValue(5, 5, -q1);
  T->setValue(6, 3, -q2);
  T->setValue(6, 4, q1);
  T->setValue(6, 5, q0);
  DEBUG_END("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")

}

// From a set of data; Mass filled-in directly from a siconosMatrix -
// This constructor leads to the minimum NewtonEuler System form: \f$ M\ddot q = p \f$
/*
Q0 : contains the center of mass coordinate, and the quaternion initial. (dim(Q0)=7)
Twist0 : contains the initial velocity of center of mass and the omega initial. (dim(VTwist0)=6)
*/
NewtonEulerDS::NewtonEulerDS():
  DynamicalSystem(13),
  _hasConstantFExt(false),
  _hasConstantMExt(false),
  _isMextExpressedInInertialFrame(false),
  _nullifyMGyr(false),
  _computeJacobianFIntqByFD(false),
  _computeJacobianFInttwistByFD(false),
  _computeJacobianMIntqByFD(false),
  _computeJacobianMInttwistByFD(false),
  _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
{
  /* common code for constructors
   * would be better to use delagation of constructors in c++11
   */
  _init();
}

NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,
                             double  mass, SP::SiconosMatrix inertialMatrix):
  DynamicalSystem(13),
  _hasConstantFExt(false),
  _hasConstantMExt(false),
  _isMextExpressedInInertialFrame(false),
  _nullifyMGyr(false),
  _computeJacobianFIntqByFD(false),
  _computeJacobianFInttwistByFD(false),
  _computeJacobianMIntqByFD(false),
  _computeJacobianMInttwistByFD(false),
  _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))

{
  DEBUG_BEGIN("NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,double  mass, SP::SiconosMatrix inertialMatrix)\n");

  /* common code for constructors
   * would be better to use delegation of constructors in c++11
   */
  _init();

  // Initial conditions
  _q0 = Q0;
  _twist0 = Twist0;
  resetToInitialState();

  _scalarMass = mass;
  if(inertialMatrix)
    _I = inertialMatrix;
  updateMassMatrix();

  _T->zero();
  _T->setValue(0, 0, 1.0);
  _T->setValue(1, 1, 1.0);
  _T->setValue(2, 2, 1.0);
  computeT();

  DEBUG_END("NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,double  mass, SP::SiconosMatrix inertialMatrix)\n");
}

void NewtonEulerDS::_init()
{
  // --- NEWTONEULER INHERITED CLASS MEMBERS ---
  // -- Memory allocation for vector and matrix members --

  _qDim = 7;
  _ndof = 6;
  _n = _qDim + _ndof;

  _zeroPlugin();

  // Current state
  _q.reset(new SiconosVector(_qDim));
  _twist.reset(new SiconosVector(_ndof));
  _dotq.reset(new SiconosVector(_qDim));

  /** \todo lazy Memory allocation */
  _p.resize(3);
  _p[1].reset(new SiconosVector(_ndof)); // Needed in NewtonEulerR

  _massMatrix.reset(new SimpleMatrix(_ndof, _ndof));
  _massMatrix->zero();
  _T.reset(new SimpleMatrix(_qDim, _ndof));

  _scalarMass = 1.;
  _I.reset(new SimpleMatrix(3, 3));
  _I->eye();
  updateMassMatrix();

  _wrench.reset(new SiconosVector(_ndof));
  _mGyr.reset(new SiconosVector(3,0.0));

  /** The follwing jacobian are always allocated since we have always
   * Gyroscopical forces that has non linear forces
   * This should be remove if the integration is explicit or _nullifyMGyr(false) is set to true ?
   */

  _jacobianMGyrtwist.reset(new SimpleMatrix(3, _ndof));
  _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));

  //We initialize _z with a null vector of size 1, since z is required in plug-in functions call.
  _z.reset(new SiconosVector(1));

}

void NewtonEulerDS::updateMassMatrix()
{
  // _massMatrix->zero();
  // _massMatrix->setValue(0, 0, _scalarMass);
  // _massMatrix->setValue(1, 1, _scalarMass);
  // _massMatrix->setValue(2, 2, _scalarMass);

  _massMatrix->eye();
  * _massMatrix *=  _scalarMass;

  Index dimIndex(2);
  dimIndex[0] = 3;
  dimIndex[1] = 3;
  Index startIndex(4);
  startIndex[0] = 0;
  startIndex[1] = 0;
  startIndex[2] = 3;
  startIndex[3] = 3;
  setBlock(_I, _massMatrix, dimIndex, startIndex);

}

void NewtonEulerDS::_zeroPlugin()
{
  _pluginFExt.reset(new PluggedObject());
  _pluginMExt.reset(new PluggedObject());
  _pluginFInt.reset(new PluggedObject());
  _pluginMInt.reset(new PluggedObject());
  _pluginJacqFInt.reset(new PluggedObject());
  _pluginJactwistFInt.reset(new PluggedObject());
  _pluginJacqMInt.reset(new PluggedObject());
  _pluginJactwistMInt.reset(new PluggedObject());
}

// Destructor
NewtonEulerDS::~NewtonEulerDS()
{
}

void NewtonEulerDS::setInertia(double ix, double iy, double iz)
{
  _I->zero();

  (*_I)(0, 0) = ix;
  (*_I)(1, 1) = iy;
  (*_I)(2, 2) = iz;

  updateMassMatrix();
}

void NewtonEulerDS::initializeNonSmoothInput(unsigned int level)
{
  DEBUG_PRINTF("NewtonEulerDS::initializeNonSmoothInput(unsigned int level) for level = %i\n",level);

  if(!_p[level])
  {
    if(level == 0)
    {
      _p[level].reset(new SiconosVector(_qDim));
    }
    else
      _p[level].reset(new SiconosVector(_ndof));
  }

#ifdef DEBUG_MESSAGES
  DEBUG_PRINT("display() after initialization");
  display();
#endif
}

void NewtonEulerDS::initRhs(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::initRhs(double time)\n");
  // dim
  _n = _qDim + 6;

  _x0.reset(new SiconosVector(*_q0, *_twist0));

  _x[0].reset(new SiconosVector(*_q, *_twist));

  if (!_acceleration)
    _acceleration.reset(new SiconosVector(6));

  // Compute _dotq
  computeT();
  prod(*_T, *_twist, *_dotq, true);
  _x[1].reset(new SiconosVector(*_dotq, *_acceleration));

  // Nothing to do for the initialization of the wrench

  // Everything concerning rhs and its jacobian is handled in initRhs and computeXXX related functions.
  _rhsMatrices.resize(numberOfRhsMatrices);

  if(!_p[2])
    _p[2].reset(new SiconosVector(6));

  init_inverse_mass();

  computeRhs(time);

  /** \warning the derivative of T w.r.t to q is neglected */
  _rhsMatrices[jacobianXBloc00].reset(new SimpleMatrix(_qDim, _qDim, Siconos::ZERO));

  _rhsMatrices[jacobianXBloc01].reset(new SimpleMatrix(*_T));
  bool flag1 = false, flag2 = false;
  if(_jacobianWrenchq)
  {
    // Solve MjacobianX(1,0) = jacobianFL[0]
    computeJacobianqForces(time);

    _rhsMatrices[jacobianXBloc10].reset(new SimpleMatrix(*_jacobianWrenchq));
    _inverseMass->PLUForwardBackwardInPlace(*_rhsMatrices[jacobianXBloc10]);
    flag1 = true;
  }

  if(_jacobianWrenchTwist)
  {
    // Solve MjacobianX(1,1) = jacobianFL[1]
    computeJacobianvForces(time);
    _rhsMatrices[jacobianXBloc11].reset(new SimpleMatrix(*_jacobianWrenchTwist));
    _inverseMass->PLUForwardBackwardInPlace(*_rhsMatrices[jacobianXBloc11]);
    flag2 = true;
  }

  if(!_rhsMatrices[zeroMatrix])
    _rhsMatrices[zeroMatrix].reset(new SimpleMatrix(6, 6, Siconos::ZERO));

  if(!_rhsMatrices[zeroMatrixqDim])
    _rhsMatrices[zeroMatrixqDim].reset(new SimpleMatrix(6, _qDim, Siconos::ZERO));

  if(flag1 && flag2)
    _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
                                   _rhsMatrices[jacobianXBloc10], _rhsMatrices[jacobianXBloc11]));
  else if(flag1)  // flag2 = false
    _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
                                   _rhsMatrices[jacobianXBloc10], _rhsMatrices[zeroMatrix]));
  else if(flag2)  // flag1 = false
    _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
                                   _rhsMatrices[zeroMatrixqDim], _rhsMatrices[jacobianXBloc11]));
  else
    _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
                                   _rhsMatrices[zeroMatrixqDim], _rhsMatrices[zeroMatrix]));
  DEBUG_EXPR(display(););
  DEBUG_END("NewtonEulerDS::initRhs(double time)\n");
}

void NewtonEulerDS::resetToInitialState()
{
  // set q and q[1] to q0 and Twist0
  if(_q0)
  {
    *_q = *_q0;
  }
  else
    RuntimeException::selfThrow("NewtonEulerDS::resetToInitialState - initial position _q0 is null");

  if(_twist0)
  {
    *_twist = *_twist0;
  }
  else
    RuntimeException::selfThrow("NewtonEulerDS::resetToInitialState - initial twist _twist0 is null");
}

void NewtonEulerDS::init_inverse_mass()
{
  if(_massMatrix && !_inverseMass)
    {
      updateMassMatrix();
      _inverseMass.reset(new SimpleMatrix(*_massMatrix));
    }
}

void NewtonEulerDS::update_inverse_mass()
{
  if(_massMatrix && _inverseMass)
    {
      updateMassMatrix();
      *_inverseMass = *_massMatrix;
    }
}

void NewtonEulerDS::computeFExt(double time)
{
  // computeFExt(time, _fExt);

  if(_pluginFExt->fPtr)
  {
    ((FExt_NE)_pluginFExt->fPtr)(time, &(*_fExt)(0), _qDim, &(*_q0)(0));  // parameter z are assumed to be equal to q0
  }

}

void NewtonEulerDS::computeFExt(double time, SP::SiconosVector fExt)
{
  /* if the pointer has been set to an external vector
   * after setting the plugin, we do not call the plugin */
  if(_hasConstantFExt)
  {
    if(fExt != _fExt)
      *fExt = *_fExt;
  }
  else
  {
    if(_pluginFExt->fPtr)
    {
      ((FExt_NE)_pluginFExt->fPtr)(time, &(*fExt)(0), _qDim, &(*_q0)(0));  // parameter z are assumed to be equal to q0
    }
  }
}

/** This function has been added to avoid Swig director to wrap _MExt into numpy.array
 * when we call  NewtonEulerDS::computeMExt(double time, SP::SiconosVector q, SP::SiconosVector mExt)
 *  that calls in turn computeMExt(time, q, _mExt);
 */
static
void computeMExt_internal(double time, bool hasConstantMExt,
                          unsigned int qDim, SP::SiconosVector q0,
                          SP::PluggedObject pluginMExt, SP::SiconosVector mExt_attributes,
                          SP::SiconosVector mExt)
{
  /* if the pointer has been set to an external vector
   * after setting the plugin, we do not call the plugin */
  if(hasConstantMExt)
  {
    if(mExt != mExt_attributes)
      *mExt = *mExt_attributes;
  }
  else if(pluginMExt->fPtr)
    ((FExt_NE)pluginMExt->fPtr)(time, &(*mExt)(0), qDim, &(*q0)(0));  // parameter z are assumed to be equal to q0

}

void NewtonEulerDS::computeMExt(double time)
{
  DEBUG_BEGIN("N3ewtonEulerDS::computeMExt(double time)\n");
  computeMExt_internal(time,_hasConstantMExt,
                       _qDim, _q0,
                       _pluginMExt, _mExt, _mExt);
  DEBUG_END("NewtonEulerDS::computeMExt(double time)\n");
}

void NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)
{
  DEBUG_BEGIN("NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)\n");
  computeMExt_internal(time, _hasConstantMExt,
                       _qDim, _q0, _pluginMExt, _mExt, mExt);
  DEBUG_END("NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)\n");
}

void NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(double time, SP::SiconosVector q)
{

  DEBUG_BEGIN("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(...)\n");

  /* The computation of Jacobian of R^T mExt is somehow very rough since the pertubation
   * that we apply to q  that gives qeps does not provide a unit quaternion. The rotation
   * is computed assuming that the quaternion is unit (see rotateAbsToBody(double q0, double
   * q1, double q2, double q3, SP::SiconosVector v)).
   */

  SP::SiconosVector mExt(new SiconosVector(3));
  computeMExt(time, mExt);
  if(_isMextExpressedInInertialFrame)
    ::changeFrameAbsToBody(q,mExt);
  DEBUG_EXPR(q->display());
  DEBUG_EXPR(mExt->display(););

  double mExt0 = mExt->getValue(0);
  double mExt1 = mExt->getValue(1);
  double mExt2 = mExt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));
  _jacobianMExtq->zero();
  (*qeps)(3) += _epsilonFD;
  for(int j =3; j < 7; j++)
  {
    computeMExt(time, mExt);
    if(_isMextExpressedInInertialFrame)
      ::changeFrameAbsToBody(qeps,mExt);
    DEBUG_EXPR(mExt->display(););
    _jacobianMExtq->setValue(0,j, (mExt->getValue(0) - mExt0)/_epsilonFD);
    _jacobianMExtq->setValue(1,j, (mExt->getValue(1) - mExt1)/_epsilonFD);
    _jacobianMExtq->setValue(2,j, (mExt->getValue(2) - mExt2)/_epsilonFD);
    (*qeps)(j) -= _epsilonFD;
    if(j<6)(*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_EXPR(_jacobianMExtq->display(););
  DEBUG_END("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(...)\n");

}

void NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(double time, SP::SiconosVector q)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(...)\n");
  bool isMextExpressedInInertialFrame_save = _isMextExpressedInInertialFrame;
  _isMextExpressedInInertialFrame=false;
  SP::SiconosVector mExt(new SiconosVector(3));
  computeMExt(time, mExt);
  if(_isMextExpressedInInertialFrame)
    ::changeFrameAbsToBody(q,mExt);
  DEBUG_EXPR(q->display());
  DEBUG_EXPR(mExt->display());

  _isMextExpressedInInertialFrame=isMextExpressedInInertialFrame_save;

  double q0 = q->getValue(3);
  double q1 = q->getValue(4);
  double q2 = q->getValue(5);
  double q3 = q->getValue(6);

  computeJacobianConvectedVectorInBodyFrame(q0, q1,  q2,  q3, _jacobianMExtq, mExt);

  DEBUG_EXPR(_jacobianMExtq->display());

  // SP::SimpleMatrix jacobianMExtqtmp (new SimpleMatrix(*_jacobianMExtq));
  // computeJacobianMExtqExpressedInInertialFrameByFD(time, q);

  // std::cout << "#################  " << (*jacobianMExtqtmp- *_jacobianMExtq).normInf() << std::endl;
  // assert((*jacobianMExtqtmp- *_jacobianMExtq).normInf()< 1e-10);

  // DEBUG_EXPR(_jacobianMExtq->display(););
  DEBUG_END("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(...)\n");

}
void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  computeFInt(time,  q,  v, _fInt);
}

void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector fInt)
{
  if(_pluginFInt->fPtr)
    ((FInt_NE)_pluginFInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*fInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
   DEBUG_BEGIN("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)\n");
   computeMInt(time, q, v, _mInt);
   DEBUG_END("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)\n");
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)
{
  DEBUG_BEGIN("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)\n");
  if(_pluginMInt->fPtr)
    ((FInt_NE)_pluginMInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*mInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
  DEBUG_END("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)\n");
}

void NewtonEulerDS::computeJacobianFIntq(double time)
{
  computeJacobianFIntq(time, _q, _twist);
}
void NewtonEulerDS::computeJacobianFIntv(double time)
{
  computeJacobianFIntv(time, _q, _twist);
}

void NewtonEulerDS::computeJacobianFIntq(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianFIntq(...) starts");
  if(_pluginJacqFInt->fPtr)
    ((FInt_NE)_pluginJacqFInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianFIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if(_computeJacobianFIntqByFD)
    computeJacobianFIntqByFD(time, q, twist);
  DEBUG_EXPR(_jacobianFIntq->display(););
  DEBUG_END("NewtonEulerDS::computeJacobianFIntq(...)");
}

void NewtonEulerDS::computeJacobianFIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, twist, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));
  _jacobianFIntq->zero();
  (*qeps)(0) += _epsilonFD;
  for(int j =0; j < 7; j++)
  {
    computeFInt(time, qeps, twist, fInt);
    _jacobianFIntq->setValue(0,j, (fInt->getValue(0) - fInt0)/_epsilonFD);
    _jacobianFIntq->setValue(1,j, (fInt->getValue(1) - fInt1)/_epsilonFD);
    _jacobianFIntq->setValue(2,j, (fInt->getValue(2) - fInt2)/_epsilonFD);
    (*qeps)(j) -= _epsilonFD;
    if(j<6)(*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_END("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");

}

void NewtonEulerDS::computeJacobianFIntv(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  if(_pluginJactwistFInt->fPtr)
    ((FInt_NE)_pluginJactwistFInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianFInttwist)(0, 0), _qDim,  &(*_q0)(0));
  else if(_computeJacobianFInttwistByFD)
    computeJacobianFIntvByFD(time, q, twist);
}

void NewtonEulerDS::computeJacobianFIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, twist, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*twist));
  _jacobianFInttwist->zero();

  (*veps)(0) += _epsilonFD;
  for(int j =0; j < 6; j++)
  {
    computeFInt(time, q, veps, fInt);
    _jacobianFInttwist->setValue(0,j, (fInt->getValue(0) - fInt0)/_epsilonFD);
    _jacobianFInttwist->setValue(1,j, (fInt->getValue(1) - fInt1)/_epsilonFD);
    _jacobianFInttwist->setValue(2,j, (fInt->getValue(2) - fInt2)/_epsilonFD);
    (*veps)(j) -= _epsilonFD;
    if(j<5)(*veps)(j+1) += _epsilonFD;
  }

  DEBUG_END("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianMGyrtwistByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianMGyrvByFD(...)\n");
  SP::SiconosVector mGyr(new SiconosVector(3));
  computeMGyr(twist, mGyr);

  double mGyr0 = mGyr->getValue(0);
  double mGyr1 = mGyr->getValue(1);
  double mGyr2 = mGyr->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*twist));
  _jacobianMGyrtwist->zero();

  (*veps)(0) += _epsilonFD;
  for(int j =0; j < 6; j++)
  {
    computeMGyr(veps, mGyr);
    _jacobianMGyrtwist->setValue(3,j, (mGyr->getValue(0) - mGyr0)/_epsilonFD);
    _jacobianMGyrtwist->setValue(4,j, (mGyr->getValue(1) - mGyr1)/_epsilonFD);
    _jacobianMGyrtwist->setValue(5,j, (mGyr->getValue(2) - mGyr2)/_epsilonFD);
    (*veps)(j) -= _epsilonFD;
    if(j<5)(*veps)(j+1) += _epsilonFD;
  }
  DEBUG_EXPR(_jacobianMGyrtwist->display());
  DEBUG_END("NewtonEulerDS::computeJacobianMGyrvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianMIntq(double time)
{
  computeJacobianMIntq(time, _q, _twist);
}
void NewtonEulerDS::computeJacobianMIntv(double time)
{
  computeJacobianMIntv(time, _q, _twist);
}

void NewtonEulerDS::computeJacobianMIntq(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) starts");
  if(_pluginJacqMInt->fPtr)
    ((FInt_NE)_pluginJacqMInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianMIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if(_computeJacobianMIntqByFD)
    computeJacobianMIntqByFD(time, q, twist);
  DEBUG_EXPR(_jacobianMIntq->display());
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) ends");

}

void NewtonEulerDS::computeJacobianMIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, twist, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));

  (*qeps)(0) += _epsilonFD;
  for(int j =0; j < 7; j++)
  {
    computeMInt(time, qeps, twist, mInt);
    _jacobianMIntq->setValue(0,j, (mInt->getValue(0) - mInt0)/_epsilonFD);
    _jacobianMIntq->setValue(1,j, (mInt->getValue(1) - mInt1)/_epsilonFD);
    _jacobianMIntq->setValue(2,j, (mInt->getValue(2) - mInt2)/_epsilonFD);
    (*qeps)(j) -= _epsilonFD;
    if(j<6)(*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) ends\n");
}

void NewtonEulerDS::computeJacobianMIntv(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  if(_pluginJactwistMInt->fPtr)
    ((FInt_NE)_pluginJactwistMInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianMInttwist)(0, 0), _qDim,  &(*_q0)(0));
  else if(_computeJacobianMInttwistByFD)
    computeJacobianMIntvByFD(time,  q, twist);
}

void NewtonEulerDS::computeJacobianMIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, twist, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*twist));

  (*veps)(0) += _epsilonFD;
  for(int j =0; j < 6; j++)
  {
    computeMInt(time, q, veps, mInt);
    _jacobianMInttwist->setValue(0,j, (mInt->getValue(0) - mInt0)/_epsilonFD);
    _jacobianMInttwist->setValue(1,j, (mInt->getValue(1) - mInt1)/_epsilonFD);
    _jacobianMInttwist->setValue(2,j, (mInt->getValue(2) - mInt2)/_epsilonFD);
    (*veps)(j) -= _epsilonFD;
    if(j<5)(*veps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) ends\n");
}

void NewtonEulerDS::computeRhs(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeRhs(double time)");
  *_acceleration = *(_p[2]); // Warning: r/p update is done in Interactions/Relations

  computeForces(time, _q, _twist);
  *_acceleration += *_wrench;
  DEBUG_EXPR(_wrench->display(););

  if(_inverseMass)
    _inverseMass->PLUForwardBackwardInPlace(*_acceleration);

   // Compute _dotq
  computeT();
  prod(*_T, *_twist, *_dotq, true);

  _x[1]->setBlock(0, *_dotq);
  _x[1]->setBlock(_qDim, *_acceleration);

}

void NewtonEulerDS::computeJacobianRhsx(double time)
{
  if(_jacobianWrenchq)
  {
    SP::SiconosMatrix bloc10 = _jacxRhs->block(1, 0);
    computeJacobianqForces(time);
    *bloc10 = *_jacobianWrenchq;
    _inverseMass->PLUForwardBackwardInPlace(*bloc10);
  }
  if(_jacobianWrenchTwist)
  {
    SP::SiconosMatrix bloc11 = _jacxRhs->block(1, 1);
    computeJacobianvForces(time);
    *bloc11 = *_jacobianWrenchTwist;
    _inverseMass->PLUForwardBackwardInPlace(*bloc11);
  }

}

void NewtonEulerDS::computeForces(double time)
{
  computeForces(time, _q, _twist);
}

/** This function has been added to avoid Swig director to wrap _mGyr into numpy.array
 * when we call  NewtonEulerDS::computeMGyr(SP::SiconosVector twist) that calls in turn
 * computeMGyr(twist, _mGyr)
 */
static
void computeMGyr_internal(SP::SiconosMatrix I ,SP::SiconosVector twist, SP::SiconosVector mGyr)
{
  if(I)
  {
    DEBUG_EXPR(I->display());
    DEBUG_EXPR(twist->display());
    SiconosVector omega(3);
    SiconosVector iomega(3);
    omega.setValue(0, twist->getValue(3));
    omega.setValue(1, twist->getValue(4));
    omega.setValue(2, twist->getValue(5));
    prod(*I, omega, iomega, true);
    cross_product(omega, iomega, *mGyr);
  }
}
void NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)
{
  // computation of \Omega times I \Omega (MGyr is in the l.h.s of the equation of motion)
  DEBUG_BEGIN("NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)\n");

   ::computeMGyr_internal(_I, twist, mGyr);

  DEBUG_END("NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)\n");

}
void NewtonEulerDS::computeMGyr(SP::SiconosVector twist)
{
  /*computation of \Omega times I \Omega*/
  //DEBUG_BEGIN("NewtonEulerDS::computeMGyr(SP::SiconosVector twist)\n");
  ::computeMGyr_internal(_I , twist, _mGyr);
  //DEBUG_END("NewtonEulerDS::computeMGyr(SP::SiconosVector twist)\n");

}

void NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)
{
  DEBUG_BEGIN("NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)\n")

  if(_wrench)
  {
    _wrench->zero();

    // External wrench

    if(_fExt)
    {
      computeFExt(time);
      assert(!isnan(_fExt->vector_sum()));
      _wrench->setBlock(0, *_fExt);
    }
    if(_mExt)
    {
      computeMExt(time);
      assert(!isnan(_mExt->vector_sum()));
      if(_isMextExpressedInInertialFrame) {
        SP::SiconosVector mExt(std11::make_shared<SiconosVector>(*_mExt));
        ::changeFrameAbsToBody(q,mExt);
        _wrench->setBlock(3, *mExt);
      }
      else
        _wrench->setBlock(3, *_mExt);
    }

    // Internal wrench

    if(_fInt)
    {
      computeFInt(time, q, twist);
      assert(!isnan(_fInt->vector_sum()));
      _wrench->setValue(0, _wrench->getValue(0) - _fInt->getValue(0));
      _wrench->setValue(1, _wrench->getValue(1) - _fInt->getValue(1));
      _wrench->setValue(2, _wrench->getValue(2) - _fInt->getValue(2));

    }

    if(_mInt)
    {
      computeMInt(time, q , twist);
      assert(!isnan(_mInt->vector_sum()));
      _wrench->setValue(3, _wrench->getValue(3) - _mInt->getValue(0));
      _wrench->setValue(4, _wrench->getValue(4) - _mInt->getValue(1));
      _wrench->setValue(5, _wrench->getValue(5) - _mInt->getValue(2));
    }

    // Gyroscopical effect
    if(!_nullifyMGyr)
    {
      computeMGyr(twist);
      assert(!isnan(_mGyr->vector_sum()));
      _wrench->setValue(3, _wrench->getValue(3) - _mGyr->getValue(0));
      _wrench->setValue(4, _wrench->getValue(4) - _mGyr->getValue(1));
      _wrench->setValue(5, _wrench->getValue(5) - _mGyr->getValue(2));
    }
    DEBUG_EXPR(_wrench->display());
    DEBUG_END("NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)\n")

  }
  else
  {
    RuntimeException::selfThrow("NewtonEulerDS::computeForces _wrench is null");
  }
  // else nothing.
}

void NewtonEulerDS::computeJacobianqForces(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianqWrench(double time) \n");
  if(_jacobianWrenchq)
  {
    _jacobianWrenchq->zero();
    if(_jacobianFIntq)
    {
      computeJacobianFIntq(time);
      _jacobianWrenchq->setBlock(0,0,-1.0 * *_jacobianFIntq);
    }
    if(_jacobianMIntq)
    {
      computeJacobianMIntq(time);
    }
    if(_isMextExpressedInInertialFrame && _mExt)
    {
      computeJacobianMExtqExpressedInInertialFrame(time, _q);
      _jacobianWrenchq->setBlock(3,0,1.0* *_jacobianMExtq);
    }
    DEBUG_EXPR(_jacobianWrenchq->display(););
  }
  else
  {
    RuntimeException::selfThrow("NewtonEulerDS::computeJacobianqForces _jacobianWrenchq is null");
  }
  //else nothing.
  DEBUG_END("NewtonEulerDS::computeJacobianqForces(double time) \n");
}

void NewtonEulerDS::computeJacobianvForces(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobiantwistForces(double time) \n");
  if(_jacobianWrenchTwist)
  {
    _jacobianWrenchTwist->zero();
    if(_jacobianFInttwist)
    {
      computeJacobianFIntv(time);
      _jacobianWrenchTwist->setBlock(0,0,-1.0 * *_jacobianFInttwist);
    }
    if(_jacobianMInttwist)
    {
      computeJacobianMIntv(time);
      _jacobianWrenchTwist->setBlock(3,0,-1.0 * *_jacobianMInttwist);
    }
    if(!_nullifyMGyr)
    {
      if(_jacobianMGyrtwist)
      {
        //computeJacobianMGyrtwistByFD(time,_q,_twist);
        computeJacobianMGyrtwist(time);
        _jacobianWrenchTwist->setBlock(3,0,-1.0 * *_jacobianMGyrtwist);
      }
    }
  }
  //else nothing.
  DEBUG_END("NewtonEulerDS::computeJacobiantwistForces(double time) \n");
}

void NewtonEulerDS::computeJacobianMGyrtwist(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianMGyrtwist(double time) \n");
  if(_jacobianMGyrtwist)
  {
    //Omega /\ I \Omega:
    _jacobianMGyrtwist->zero();
    SiconosVector omega(3);
    omega.setValue(0, _twist->getValue(3));
    omega.setValue(1, _twist->getValue(4));
    omega.setValue(2, _twist->getValue(5));
    SiconosVector Iomega(3);
    prod(*_I, omega, Iomega, true);
    SiconosVector ei(3);
    SiconosVector Iei(3);
    SiconosVector ei_Iomega(3);
    SiconosVector omega_Iei(3);

    /*See equation of DevNotes.pdf, equation with label eq:NE_nablaFL1*/
    for(int i = 0; i < 3; i++)
    {
      ei.zero();
      ei.setValue(i, 1.0);
      prod(*_I, ei, Iei, true);
      cross_product(omega, Iei, omega_Iei);
      cross_product(ei, Iomega, ei_Iomega);
      for(int j = 0; j < 3; j++)
        _jacobianMGyrtwist->setValue(j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
    }
    // Check if Jacobian is valid. Warning to the transpose operation in
    // _jacobianMGyrtwist->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
  }
  //else nothing.
  DEBUG_EXPR(_jacobianMGyrtwist->display());
  // _jacobianMGyrtwist->display();
  // SP::SimpleMatrix jacobianMGyrtmp (new SimpleMatrix(*_jacobianMGyrtwist));
  // computeJacobianMGyrtwistByFD(time, _q, _twist);
  // jacobianMGyrtmp->display();
  // std::cout << "#################  " << (*jacobianMGyrtmp - *_jacobianMGyrtwist).normInf() << std::endl;
  // assert((*jacobianMGyrtmp - *_jacobianMGyrtwist).normInf()< 1e-10);
  DEBUG_END("NewtonEulerDS::computeJacobianMGyrtwist(double time) \n");
}

void NewtonEulerDS::display() const
{
  std::cout << "=====> NewtonEuler System display (number: " << _number << ")." <<std::endl;
  std::cout << "- _ndof : " << _ndof <<std::endl;
  std::cout << "- _qDim : " << _qDim <<std::endl;
  std::cout << "- _n : " << _n <<std::endl;
  std::cout << "- q " <<std::endl;
  if(_q) _q->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- q0 " <<std::endl;
  if(_q0) _q0->display();
  std::cout << "- twist " <<std::endl;
  if(_twist) _twist->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- twist0 " <<std::endl;
  if(_twist0) _twist0->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- dotq " <<std::endl;
  if(_dotq) _dotq->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[0] " <<std::endl;
  if(_p[0]) _p[0]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[1] " <<std::endl;
  if(_p[1]) _p[1]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[2] " <<std::endl;
  if(_p[2]) _p[2]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "mass :" <<  _scalarMass <<std::endl;
  std::cout << "Inertia :" <<std::endl;
  if(_I) _I->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "===================================== " <<std::endl;
}

// --- Functions for memory handling ---
void NewtonEulerDS::initMemory(unsigned int steps)
{
  DynamicalSystem::initMemory(steps);

  if(steps == 0)
    std::cout << "Warning : NewtonEulerDS::initMemory with size equal to zero" <<std::endl;
  else
  {
    _qMemory.setMemorySize(steps, _qDim);
    _twistMemory.setMemorySize(steps, _ndof);
    _forcesMemory.setMemorySize(steps, _ndof);
    _dotqMemory.setMemorySize(steps, _qDim);
    //    swapInMemory(); Useless, done in osi->initializeWorkVectorsForDS
  }
}

void NewtonEulerDS::swapInMemory()
{
  //  _xMemory->swap(_x[0]);
  _qMemory.swap(*_q);
  _twistMemory.swap(*_twist);
  _dotqMemory.swap(*_dotq);
  _forcesMemory.swap(*_wrench);
}

void NewtonEulerDS::resetAllNonSmoothParts()
{
  if(_p[1])
    _p[1]->zero();
  else
    _p[1].reset(new SiconosVector(_ndof));
}
void NewtonEulerDS::resetNonSmoothPart(unsigned int level)
{
  if(_p[level])
    _p[level]->zero();
}

void NewtonEulerDS::computeT()
{
  ::computeT(_q,_T);
}

void NewtonEulerDS::computeTdot()
{
  if(!_Tdot)
  {
    _Tdot.reset(new SimpleMatrix(_qDim, _ndof));
    _Tdot->zero();
  }

  ::computeT(_dotq,_Tdot);
}

void NewtonEulerDS::normalizeq()
{
  ::normalizeq(_q);
}

void NewtonEulerDS::setComputeJacobianFIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
  _pluginJacqFInt->setComputeFunction(pluginPath, functionName);
  if(!_jacobianFIntq)
    _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
  if(!_jacobianWrenchq)
    _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
  _pluginJactwistFInt->setComputeFunction(pluginPath, functionName);
  if(!_jacobianFInttwist)
    _jacobianFInttwist.reset(new SimpleMatrix(3, _ndof));

}
void NewtonEulerDS::setComputeJacobianFIntqFunction(FInt_NE fct)
{
  _pluginJacqFInt->setComputeFunction((void *)fct);
  if(!_jacobianFIntq)
    _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
  if(!_jacobianWrenchq)
    _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(FInt_NE fct)
{
  _pluginJactwistFInt->setComputeFunction((void *)fct);
  if(!_jacobianFInttwist)
    _jacobianFInttwist.reset(new SimpleMatrix(3, _ndof));
  if(!_jacobianWrenchTwist)
    _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
}

void NewtonEulerDS::setComputeJacobianMIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  _pluginJacqMInt->setComputeFunction(pluginPath, functionName);
  if(!_jacobianMIntq)
    _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
  if(!_jacobianWrenchq)
    _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));

}
void NewtonEulerDS::setComputeJacobianMIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  _pluginJactwistMInt->setComputeFunction(pluginPath, functionName);
  if(!_jacobianMInttwist)
    _jacobianMInttwist.reset(new SimpleMatrix(3, _ndof));
  if(!_jacobianWrenchTwist)
    _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
}
void NewtonEulerDS::setComputeJacobianMIntqFunction(FInt_NE fct)
{
  _pluginJacqMInt->setComputeFunction((void *)fct);
  if(!_jacobianMIntq)
    _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
  if(!_jacobianWrenchq)
    _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
}
void NewtonEulerDS::setComputeJacobianMIntvFunction(FInt_NE fct)
{
  _pluginJactwistMInt->setComputeFunction((void *)fct);
  if(!_jacobianMInttwist)
    _jacobianMInttwist.reset(new SimpleMatrix(3, _ndof));
  if(!_jacobianWrenchTwist)
    _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
}

double NewtonEulerDS::computeKineticEnergy()
{
  DEBUG_BEGIN("NewtonEulerDS::computeKineticEnergy()\n");
  assert(_twist);
  assert(_massMatrix);
  DEBUG_EXPR(_twist->display());
  DEBUG_EXPR(_massMatrix->display());

  SiconosVector tmp(6);
  prod(*_massMatrix, *_twist, tmp, true);
  double K =0.5*inner_prod(tmp,*_twist);

  DEBUG_PRINTF("Kinetic Energy = %e\n", K);
  DEBUG_END("NewtonEulerDS::computeKineticEnergy()\n");
  return K;
}
void NewtonEulerDS::setBoundaryConditions(SP::BoundaryCondition newbd)
{
  if(!_boundaryConditions)
  {
    std::cout << "Warning : NewtonEulerDS::setBoundaryConditions. old boundary conditions were pre-existing" <<std::endl;
  }
  _boundaryConditions = newbd;
  _reactionToBoundaryConditions.reset(new SiconosVector(_boundaryConditions->velocityIndices()->size()));

};

SP::SiconosVector NewtonEulerDS::linearVelocity(bool absoluteRef) const
{
  // Short-cut: return the _twist 6-vector without modification, first
  // 3 components are the expected linear velocity.
  if (absoluteRef)
    return _twist;

  SP::SiconosVector v(std11::make_shared<SiconosVector>(3));
  linearVelocity(absoluteRef, *v);
  return v;
}

void NewtonEulerDS::linearVelocity(bool absoluteRef, SiconosVector &v) const
{
  v(0) = (*_twist)(0);
  v(1) = (*_twist)(1);
  v(2) = (*_twist)(2);

  /* See _twist: linear velocity is in absolute frame */
  if (!absoluteRef)
    changeFrameAbsToBody(*_q, v);
}

SP::SiconosVector NewtonEulerDS::angularVelocity(bool absoluteRef) const
{
  SP::SiconosVector w(std11::make_shared<SiconosVector>(3));
  angularVelocity(absoluteRef, *w);
  return w;
}

void NewtonEulerDS::angularVelocity(bool absoluteRef, SiconosVector &w) const
{
  w(0) = (*_twist)(3);
  w(1) = (*_twist)(4);
  w(2) = (*_twist)(5);

  /* See _twist: angular velocity is in relative frame */
  if (absoluteRef)
    changeFrameBodyToAbs(*_q, w);
}

void computeExtForceAtPos(SP::SiconosVector q, bool isMextExpressedInInertialFrame,
                          SP::SiconosVector force, bool forceAbsRef,
                          SP::SiconosVector pos, bool posAbsRef,
                          SP::SiconosVector fExt, SP::SiconosVector mExt,
                          bool accumulate)
{
  assert(!!fExt && fExt->size() == 3);
  assert(!!force && force->size() == 3);
  if (pos)
    assert(!!mExt && mExt->size() == 3);

  SiconosVector abs_frc(*force), local_frc(*force);

  if (forceAbsRef) {
    if (pos)
      changeFrameAbsToBody(*q, local_frc);
  } else
    changeFrameBodyToAbs(*q, abs_frc);

  if (pos) {
    assert(!!mExt && mExt->size() >= 3);
    SiconosVector moment(3);
    if (posAbsRef) {
      SiconosVector local_pos(*pos);
      local_pos(0) -= (*q)(0);
      local_pos(1) -= (*q)(1);
      local_pos(2) -= (*q)(2);
      changeFrameAbsToBody(*q, local_pos);
      cross_product(local_pos, local_frc, moment);
    }
    else {
      cross_product(*pos, local_frc, moment);
    }

    if (isMextExpressedInInertialFrame)
      changeFrameBodyToAbs(*q, moment);

    if (accumulate)
      *mExt = *mExt + moment;
    else
      *mExt = moment;
  }

  if (accumulate)
    *fExt += *fExt + abs_frc;
  else
    *fExt = abs_frc;
}

void NewtonEulerDS::addExtForceAtPos(SP::SiconosVector force, bool forceAbsRef,
                                     SP::SiconosVector pos, bool posAbsRef)
{
  assert(!!_fExt && _fExt->size() == 3);
  assert(!!force && force->size() == 3);
  if (pos)
    assert(!!_mExt && _mExt->size() == 3);

  computeExtForceAtPos(_q, _isMextExpressedInInertialFrame,
                       force, forceAbsRef,
                       pos, posAbsRef,
                       _fExt, _mExt, true);
}


#include "utils.h"

#include <fstream>
#include <iostream>
#include <ctime>
#include <thread>

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

#include <Eigen/Dense>

using namespace Eigen;

struct Pair {
    size_t i;
    size_t j;
};

void reset_columns(MatrixXd& weights,
                   std::uniform_real_distribution<double_t>& udouble_dist,
                   std::mt19937& random_engine) {
#ifdef NO_RESET
    return;
#endif
    if (weights.cols() < 2) return;

    double max_sim = -1;
    Pair max_indexes;
    for (size_t i = 0; i < weights.cols() - 1; ++i) {
        for(size_t j = i + 1; j < weights.cols(); ++j) {
            VectorXd normalized_column_i = weights.col(i).normalized();
            VectorXd normalized_column_j = weights.col(j).normalized();
            double sim = normalized_column_i.dot(normalized_column_j);
            if (sim > max_sim) {
                max_indexes.i = i;
                max_indexes.j = j;
                max_sim = sim;
            }
        }
    }

    if (max_sim > 0.9 && weights.col(max_indexes.i).norm() > 1) {
        weights.col(max_indexes.j) += weights.col(max_indexes.i);
        for(int i = 0; i < weights.rows(); ++i) {
            weights(i, max_indexes.i) = udouble_dist(random_engine);
        }
    }
}

void train_with_features(std::string data_file_path,
                         std::string features_file_path,
                         std::string noise_file_path,
                         int n_steps, double eta_0, double iter_power,
                         size_t l_dimensions, size_t k_dimensions,
                         std::string output_file_path,
                         std::string objective_output_file_path,
                         double *elapsed_time) {
    auto random_engine = std::mt19937(std::time(0));
    time_t start = std::time(0);
    // Data loading
    std::fstream data_file_input;
    data_file_input.open(data_file_path, std::ios::in);
    std::string line;
    std::vector<std::string> tokens;
    getline(data_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t data_size = stoul(tokens[0]);
    size_t n_samples = stoul(tokens[1]);

    std::vector<size_t> start_indexes(n_samples);
    std::vector<size_t> permutation(n_samples);
    std::vector<int> data(data_size + n_samples);
    long n_data = 0;
    long n_noise = 0;
    for (size_t i = 0, data_index = 0; i < n_samples; ++i) {
        getline(data_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        start_indexes[i] = data_index;
        permutation[i] = i;
        for (size_t j = 0; j < tokens.size(); ++j) {
            data[data_index + j] = stoi(tokens[j]);
        }
        if (data[data_index] == 0) {
            ++n_noise;
        } else {
            ++n_data;
        }
        data[data_index + tokens.size()] = -1;
        data_index += tokens.size() + 1;
    }

    size_t n_items;
    size_t m_features;
    Matrix<double, Dynamic, Dynamic, RowMajor> features;
    masterthesis::readFeatureFileBoost(
            features_file_path, n_items, m_features, features);

    std::fstream noise_file_input;
    noise_file_input.open(noise_file_path, std::ios::in);
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    VectorXd a_weights(m_features);
    for (size_t i = 0; i < m_features; ++i) {
        a_weights(i) = stod(tokens[i]);
    }
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    RowVectorXd noise_utilities(n_items);
    for (size_t i = 0; i < n_items; ++i) {
        noise_utilities(i) = stod(tokens[i]);
    }

    // Calculate constant quantities.
    double nu = ((float)n_noise) / ((float)n_data);
    double log_nu = log(nu);
    double logz_noise = 0;
    for (size_t i = 0; i < n_items; ++i) {
        logz_noise += masterthesis::log1exp(noise_utilities[i]);
    }

    // Initialize parameters.
    std::uniform_real_distribution<double_t> udouble_dist(0, 1e-3);
    MatrixXd b_weights(m_features, l_dimensions);
    MatrixXd c_weights(m_features, k_dimensions);
    double n_logz = 0;

    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions; ++j) {
            b_weights(i, j) = udouble_dist(random_engine);
        }
        for (size_t j = 0; j < k_dimensions; ++j) {
            c_weights(i, j) = udouble_dist(random_engine);
        }
    }
    VectorXd item_utilities = features*a_weights;
    for (size_t i = 0; i < n_items; ++i) {
        n_logz -= masterthesis::log1exp(item_utilities[i]);
    }

    std::vector<double> objectives(n_steps);
    VectorXd a_gradient(m_features);
    MatrixXd b_weights_gradient(m_features, l_dimensions);
    MatrixXd c_weights_gradient(m_features, k_dimensions);

#ifdef ADAGRAD
    // for adagrad
    VectorXd g_a_weights = VectorXd::Constant(m_features, 1e-2);
    MatrixXd g_b_weights = MatrixXd::Constant(m_features, l_dimensions, 1e-2);
    MatrixXd g_c_weights = MatrixXd::Constant(m_features, k_dimensions, 1e-2);
    double g_n_logz = 1e-2;
#endif
    // initialize all entries of g_a_weights, g_b_weights, g_c_weights to 1e-2
    for (size_t iter = 0; iter < n_steps; ++iter) {
#ifdef COMPUTE_OBJ
        double objective = 0;
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            size_t start_idx = start_indexes[sub_iter];
            size_t end_idx;
            if (sub_iter == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[sub_iter + 1] - 1;
            }
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;

            size_t set_size = end_idx - start_idx - 1;
            if (set_size > 0) {
                Matrix<double, Dynamic, Dynamic, RowMajor> sub_feature_matrix(set_size, m_features);
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    RowVectorXd feature_row = features.row(data[i]);
                    sub_feature_matrix.row(i - start_idx - 1) = feature_row;
                    p_model += feature_row.dot(a_weights);
                    p_noise += noise_utilities[data[i]];
                }

                MatrixXd div_weights_slice(set_size, l_dimensions);
                MatrixXd coh_weights_slice(set_size, k_dimensions);
                div_weights_slice.noalias() = sub_feature_matrix * b_weights;
                coh_weights_slice.noalias() = sub_feature_matrix * c_weights;
                p_model -= div_weights_slice.sum();
                p_model += div_weights_slice.colwise().maxCoeff().sum();
                p_model += coh_weights_slice.sum();
                p_model -= coh_weights_slice.colwise().maxCoeff().sum();
            }
            if (label == 1) {
                objective -= masterthesis::log1exp(log_nu + p_noise - p_model);
            } else {
                objective -= masterthesis::log1exp(p_model - log_nu - p_noise);
            }
        }
#ifdef PRINT_DEBUG
        std::cout << objective << std::endl;
#endif
        objectives[iter] = objective;
#endif
        shuffle(std::begin(permutation), std::end(permutation), random_engine);
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            if (sub_iter % 1000 == 0) {
                reset_columns(b_weights, udouble_dist, random_engine);
                reset_columns(c_weights, udouble_dist, random_engine);
            }
            size_t start_idx = start_indexes[permutation[sub_iter]];
            size_t end_idx;
            if (permutation[sub_iter] == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[permutation[sub_iter] + 1] - 1;
            }
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;

            size_t set_size = end_idx - start_idx - 1;
            a_gradient.setZero();
            if (set_size > 0) {
                Matrix<double, Dynamic, Dynamic, RowMajor> sub_feature_matrix(set_size, m_features);
                MatrixXd div_weights_slice(set_size, l_dimensions);
                MatrixXd coh_weights_slice(set_size, k_dimensions);
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    RowVectorXd feature_row = features.row(data[i]);
                    sub_feature_matrix.row(i - start_idx - 1) = feature_row;
                    a_gradient += feature_row.transpose();
                    p_model += feature_row.dot(a_weights);
                    p_noise += noise_utilities[data[i]];
                }
                div_weights_slice.noalias() = sub_feature_matrix * b_weights;
                coh_weights_slice.noalias() = sub_feature_matrix * c_weights;
                p_model -= div_weights_slice.sum();
                p_model += coh_weights_slice.sum();

                for (size_t i = 0; i < l_dimensions; ++i) {
                    int index;
                    p_model += div_weights_slice.col(i).maxCoeff(&index);
                    b_weights_gradient.col(i) = features.row(data[start_idx + 1 + index]).transpose();
                }
                for (size_t i = 0; i < k_dimensions; ++i) {
                    int index;
                    p_model -= coh_weights_slice.col(i).maxCoeff(&index);
                    c_weights_gradient.col(i) = features.row(data[start_idx + 1 + index]).transpose();
                }
            }
#ifdef ADAGRAD
            double learning_rate = eta_0;
            double tfactor = (label - masterthesis::expit(p_model - p_noise - log_nu));
            double tfactor_sq = tfactor * tfactor;
            double step = learning_rate * tfactor;
#else
            double learning_rate = eta_0 * pow((iter * n_samples) + sub_iter + 1, -iter_power);
            double step = learning_rate * (label - masterthesis::expit(p_model - p_noise - log_nu));
#endif
            if (set_size > 0) {
                b_weights_gradient.colwise() -= a_gradient;
                c_weights_gradient.colwise() -= a_gradient;
                c_weights_gradient *= -1;
#ifdef ADAGRAD
                g_a_weights += tfactor_sq*(a_gradient.cwiseProduct(a_gradient));
                g_b_weights += tfactor_sq*(b_weights_gradient.cwiseProduct(b_weights_gradient));
                g_c_weights += tfactor_sq*(c_weights_gradient.cwiseProduct(c_weights_gradient));
                // element-wise division
                a_weights += step*(a_gradient.array() / g_a_weights.cwiseSqrt().array()).matrix();
                b_weights += step*(b_weights_gradient.array() / g_b_weights.cwiseSqrt().array()).matrix();
                c_weights += step*(c_weights_gradient.array() / g_c_weights.cwiseSqrt().array()).matrix();
#else
                a_weights += step*a_gradient;
                b_weights += step*b_weights_gradient;
                c_weights += step*c_weights_gradient;
#endif
                for (size_t i = 0; i < m_features; ++i) {
                    for (size_t j = 0; j < l_dimensions; ++j) {
                        if (b_weights(i, j) < 0) {
                            b_weights(i, j) = udouble_dist(random_engine);
                        }
                    }
                    for (size_t j = 0; j < k_dimensions; ++j) {
                        if (c_weights(i, j) < 0) {
                            c_weights(i, j) = udouble_dist(random_engine);
                        }
                    }
                }
            }
#ifdef ADAGRAD
            g_n_logz += tfactor_sq;
            n_logz += step / std::sqrt(g_n_logz);
#else
            n_logz += step;
#endif
        }
    }

    std::fstream output_file(output_file_path, std::ios::out);

    output_file << n_logz << std::endl;
    for (size_t i = 0; i < m_features - 1; ++i) {
        output_file << a_weights[i] << ",";
    }
    output_file << a_weights[m_features - 1] << std::endl;

    for (size_t i = 0; i < m_features; ++i) {
        if (l_dimensions > 0) {
            for (size_t j = 0; j < l_dimensions - 1; ++j) {
                output_file << b_weights(i, j) << ",";
            }
            output_file << b_weights(i, l_dimensions - 1) << std::endl;
        }
    }

    for (size_t i = 0; i < m_features; ++i) {
        if (k_dimensions > 0) {
            for (size_t j = 0; j < k_dimensions - 1; ++j) {
                output_file << c_weights(i, j) << ",";
            }
            output_file << c_weights(i, k_dimensions - 1) << std::endl;
        }
    }

    output_file.close();
#ifdef COMPUTE_OBJ
    std::fstream objective_output_file(
            objective_output_file_path, std::ios::out);

    for (size_t i = 0; i < n_steps; ++i) {
        objective_output_file << objectives[i] << std::endl;
    }
    std::cout << "Fold final objective: " << objectives[n_steps - 1] << std::endl;
    objective_output_file.close();
#endif
    time_t end = std::time(0);
    double elapsed = std::difftime(end, start);
    *elapsed_time = elapsed;
#ifdef PRINT_DEBUG
    std::cout << "Fold finished, took: " << elapsed << "s." << std::endl;
#endif
}

static const int total_threads = 4;

int main(int argc, char* argv[]) {
    int fold_number = std::stoi(argv[1]);
    int l_dimensions = std::stoi(argv[2]);
    int k_dimensions = std::stoi(argv[3]);
    char* feature_set = argv[4];
    char* dataset_name = argv[5];
    double noise_factor = std::stod(argv[6]);
    int iterations = std::stoi(argv[7]);
    double eta_0 = std::stod(argv[8]);
    std::vector<double> times(fold_number);
    std::thread thread_pool[total_threads];
    int used_threads = 0;
#ifdef ADAGRAD
    short uses_adagrad = 1;
#else
    short uses_adagrad = 0;
#endif
    time_t start = std::time(0);
    for (int i = 1; i <= fold_number; ++i) {
        if (used_threads == total_threads) {
            for(size_t j = 0; j < used_threads; ++j) {
                thread_pool[j].join();
            }
            used_threads = 0;
        }
        thread_pool[used_threads] = std::thread(train_with_features,
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_data_features_%2%_fold_%3%_noise_%4%.csv") %
             dataset_name % feature_set % i % noise_factor).str(),
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_features_%2%.csv") %
             dataset_name % feature_set).str(),
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_noise_features_%2%_fold_%3%.csv") %
             dataset_name % feature_set % i).str(),
            iterations, eta_0, 0.1,
            l_dimensions,
            k_dimensions,
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_out_features_%2%_l_dim_%3%_k_dim_%4%_fold_%5%_iter_%6%_eta_%7%_adagrad_%8%_noise_%9%.csv") %
             dataset_name % feature_set % l_dimensions % k_dimensions % i % iterations % eta_0 % uses_adagrad % noise_factor).str(),
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_objective_features_%2%_l_dim_%3%_k_dim_%4%_fold_%5%_iter_%6%_eta_%7%_adagrad_%8%_noise_%9%.csv") %
             dataset_name % feature_set % l_dimensions % k_dimensions % i % iterations % eta_0 % uses_adagrad % noise_factor).str(),
            &times[i-1]
        );
        ++used_threads;
    }
    for(size_t j = 0; j < used_threads; ++j) {
        thread_pool[j].join();
    }
    time_t end = std::time(0);
    double total_time = std::difftime(end, start);
    std::fstream times_output_file(
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_timing_features_%2%_l_dim_%3%_k_dim_%4%_iter_%5%_eta_%6%_adagrad_%7%_noise_%8%.csv") %
             dataset_name % feature_set % l_dimensions % k_dimensions % iterations % eta_0 % uses_adagrad % noise_factor).str(),
            std::ios::out);
    times_output_file << total_time << std::endl;
    for (size_t i = 0; i < fold_number; ++i) {
        times_output_file << times[i] << std::endl;
    }
    times_output_file.close();
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/*!\file
  C++ input file, D1MinusLinearOSI-Time-Stepping version
  T. Schindler, V. Acary

  Slider-crank simulation with a D1MinusLinearOSI-Time-Stepping scheme

  see Flores/Leine/Glocker : Modeling and analysis of planar rigid multibody systems with
  translational clearance joints based on the non-smooth dynamics approach
  */
#include "SiconosKernel.hpp"
#include <boost/numeric/ublas/matrix.hpp>
using namespace std;

int main(int argc, char* argv[])
{
  try
  {
    // ================= Creation of the model =======================

    // parameters according to Table 1
    unsigned int nDof = 3; // degrees of freedom for robot arm
    double t0 = 0.0;         // initial computation time
    double T = 0.2;       // final computation time
    //T=0.00375;
    double h = 1e-5;       // time step : do not decrease, because of strong penetrations

    // geometrical characteristics
    double l1 = 0.1530;
    double l2 = 0.3060;
    double a = 0.05;
    double b = 0.025;
    double c = 0.001;

    // contact parameters
    double e1 = 0.4;
    double e2 = 0.4;
    double e3 = 0.4;
    double e4 = 0.4;
    // e1 = 0.1;
    // e2 = 0.1;
    // e3 = 0.1;
    // e4 = 0.1;
    //double mu1 = 0.01;
    //double mu2 = 0.01;
    //double mu3 = 0.01;
    //double mu4 = 0.01;

    // initial conditions
    SP::SiconosVector q0(new SiconosVector(nDof));
    SP::SiconosVector v0(new SiconosVector(nDof));
    q0->zero();
    v0->zero();
    (*v0)(0) = 150.;
    (*v0)(1) = -75.;

    // t0 = 7e-5;
    // (*q0)(0)=  1.129178e-02;
    // (*q0)(1)= -5.777764e-03;
    // (*q0)(2)=  0.000000e+00;

    // (*v0)(0) = 1.971606e+02 ;
    // (*v0)(1) = -1.064301e+02;

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------
    cout << "====> Model loading ..." << endl << endl;

    SP::LagrangianDS slider(new LagrangianDS(q0, v0, "SliderCrankPlugin:mass"));
    slider->setComputeFGyrFunction("SliderCrankPlugin", "FGyr");
    slider->setComputeJacobianFGyrqFunction("SliderCrankPlugin", "jacobianFGyrq");
    slider->setComputeJacobianFGyrqDotFunction("SliderCrankPlugin", "jacobianFGyrqDot");
    slider->setComputeFIntFunction("SliderCrankPlugin", "FInt");
    slider->setComputeJacobianFIntqFunction("SliderCrankPlugin", "jacobianFIntq");
    slider->setComputeJacobianFIntqDotFunction("SliderCrankPlugin", "jacobianFIntqDot");

    // -------------------
    // --- Interactions---
    // -------------------
    // -- corner 1 --
    SP::NonSmoothLaw nslaw1(new NewtonImpactNSL(e1));
    SP::Relation relation1(new LagrangianScleronomousR("SliderCrankPlugin:g1", "SliderCrankPlugin:W1", "SliderCrankPlugin:W1dot"));
    SP::Interaction inter1(new Interaction(nslaw1, relation1));

    // -- corner 2 --
    SP::NonSmoothLaw nslaw2(new NewtonImpactNSL(e2));
    SP::Relation relation2(new LagrangianScleronomousR("SliderCrankPlugin:g2", "SliderCrankPlugin:W2", "SliderCrankPlugin:W2dot"));
    SP::Interaction inter2(new Interaction(nslaw2, relation2));

    // -- corner 3 --
    SP::NonSmoothLaw nslaw3(new NewtonImpactNSL(e3));
    SP::Relation relation3(new LagrangianScleronomousR("SliderCrankPlugin:g3", "SliderCrankPlugin:W3", "SliderCrankPlugin:W3dot"));
    SP::Interaction inter3(new Interaction(nslaw3, relation3));

    // -- corner 4 --
    SP::NonSmoothLaw nslaw4(new NewtonImpactNSL(e4));
    SP::Relation relation4(new LagrangianScleronomousR("SliderCrankPlugin:g4", "SliderCrankPlugin:W4", "SliderCrankPlugin:W4dot"));
    SP::Interaction inter4(new Interaction(nslaw4, relation4));

    // -------------
    // --- Model ---
    // -------------
    SP::NonSmoothDynamicalSystem sliderWithClearance(new NonSmoothDynamicalSystem(t0, T));
    sliderWithClearance->insertDynamicalSystem(slider);
    sliderWithClearance->link(inter1, slider);
    sliderWithClearance->link(inter2, slider);
    sliderWithClearance->link(inter3, slider);
    sliderWithClearance->link(inter4, slider);

    // ----------------
    // --- Simulation ---
    // ----------------
    SP::D1MinusLinearOSI OSI(new D1MinusLinearOSI(D1MinusLinearOSI::halfexplicit_velocity_level));
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem force(new LCP());

    SP::TimeSteppingD1Minus s(new TimeSteppingD1Minus(sliderWithClearance, t, 2));
    s->insertIntegrator(OSI);
    s->insertNonSmoothProblem(impact, SICONOS_OSNSP_TS_VELOCITY);
    s->insertNonSmoothProblem(force, SICONOS_OSNSP_TS_VELOCITY + 1);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    int N = ceil((T - t0) / h) + 1; // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 35;
    SimpleMatrix dataPlot(N + 1, outputSize);

    SP::SiconosVector q = slider->q();
    SP::SiconosVector v = slider->velocity();
    int k =0;
    // computation for a first consistent output
    inter1->computeOutput(t0,0);
    inter2->computeOutput(t0,0);
    inter3->computeOutput(t0,0);
    inter4->computeOutput(t0,0);

    dataPlot(k, 0) = sliderWithClearance->t0();
    dataPlot(k, 1) = (*q)(0) / (2.*M_PI); // crank revolution
    dataPlot(k, 2) = (*q)(1);
    dataPlot(k, 3) = (*q)(2);
    dataPlot(k, 4) = (*v)(0);
    dataPlot(k, 5) = (*v)(1);
    dataPlot(k, 6) = (*v)(2);
    // std::cout << "(*q)(0)= " << (*q)(0)<< std::endl;
    // std::cout << "(*q)(1)= " << (*q)(1)<< std::endl;

    dataPlot(k, 7) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 1 (normalized)
    dataPlot(k, 8) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 2 (normalized)
    dataPlot(k, 9) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 3 (normalized)
    dataPlot(k, 10) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 4 (normalized)

    dataPlot(k, 11) = (l1 * cos((*q)(0)) + l2 * cos((*q)(1)) - l2) / l1; // x slider (normalized)
    dataPlot(k, 12) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1))) / c; // y slider (normalized)

    dataPlot(k, 13) = (*inter1->y(0))(0) ; // g1
    dataPlot(k, 14) = (*inter2->y(0))(0) ; // g2
    dataPlot(k, 15) = (*inter3->y(0))(0) ; // g3
    dataPlot(k, 16) = (*inter4->y(0))(0) ; // g4
    dataPlot(k, 17) = (*inter1->y(1))(0) ; // dot g1
    dataPlot(k, 18) = (*inter2->y(1))(0) ; // dot g2
    dataPlot(k, 19) = (*inter3->y(1))(0) ; // dot g3
    dataPlot(k, 20) = (*inter4->y(1))(0) ; // dot g4
    dataPlot(k, 21) = (*inter1->lambda(1))(0) ; // lambda1
    dataPlot(k, 22) = (*inter2->lambda(1))(0) ; // lambda2
    dataPlot(k, 23) = (*inter3->lambda(1))(0) ; // lambda3
    dataPlot(k, 24) = (*inter4->lambda(1))(0) ; // lambda4
    dataPlot(k, 25) = 0;
    dataPlot(k, 26) = 0;
    dataPlot(k, 27) = (*inter1->lambda(2))(0) ; // lambda1_{k+1}^-
    dataPlot(k, 28) = (*inter2->lambda(2))(0) ; // lambda1_{k+1}^-
    dataPlot(k, 29) = (*inter3->lambda(2))(0) ; // lambda1_{k+1}^-
    dataPlot(k, 30) = (*inter4->lambda(2))(0) ; // lambda1_{k+1}^-

    // not yet allocated
    // dataPlot(k, 31) = ( inter1->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda1old
    // dataPlot(k, 32) = ( inter2->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda1old
    // dataPlot(k, 33) = ( inter3->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda1old
    // dataPlot(k, 34) = ( inter4->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda1old

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;

    // ==== Simulation loop - Writing without explicit event handling =====
    k++;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();

//    while ((s->hasNextEvent()) && (k <= 271))
    while ((s->hasNextEvent()))
    {

      // std::cout <<"=====================================================" <<std::endl;
      // std::cout <<"=====================================================" <<std::endl;
      // std::cout <<"=====================================================" <<std::endl;
      // std::cout <<"Iteration k = " << k <<std::endl;
      // std::cout <<"s->nextTime() = " <<s->nextTime()  <<std::endl;
      // std::cout <<"=====================================================" <<std::endl;

      s->advanceToEvent();

      // --- Get values to be plotted ---
      dataPlot(k, 0) = s->nextTime();
      dataPlot(k, 1) = (*q)(0) / (2.*M_PI); // crank revolution
      dataPlot(k, 2) = (*q)(1);
      dataPlot(k, 3) = (*q)(2);
      dataPlot(k, 4) = (*v)(0);
      dataPlot(k, 5) = (*v)(1);
      dataPlot(k, 6) = (*v)(2);
      dataPlot(k, 7) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 1 (normalized)
      dataPlot(k, 8) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 2 (normalized)
      dataPlot(k, 9) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 3 (normalized)
      dataPlot(k, 10) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 4 (normalized)
      dataPlot(k, 11) = (l1 * cos((*q)(0)) + l2 * cos((*q)(1)) - l2) / l1; // x slider (normalized)
      dataPlot(k, 12) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1))) / c; // y slider (normalized)
      dataPlot(k, 13) = (*inter1->y(0))(0) ; // g1
      dataPlot(k, 14) = (*inter2->y(0))(0) ; // g2
      dataPlot(k, 15) = (*inter3->y(0))(0) ; // g3
      dataPlot(k, 16) = (*inter4->y(0))(0) ; // g4
      dataPlot(k, 17) = (*inter1->y(1))(0) ; // dot g1
      dataPlot(k, 18) = (*inter2->y(1))(0) ; // dot g2
      dataPlot(k, 19) = (*inter3->y(1))(0) ; // dot g3
      dataPlot(k, 20) = (*inter4->y(1))(0) ; // dot g4
      dataPlot(k, 21) = (*inter1->lambda(1))(0) ; // Lambda1 (impulse)
      dataPlot(k, 22) = (*inter2->lambda(1))(0) ; // Lambda2
      dataPlot(k, 23) = (*inter3->lambda(1))(0) ; // Lambda3
      dataPlot(k, 24) = (*inter4->lambda(1))(0) ; // Lambda4
      dataPlot(k, 25) = 0;
      dataPlot(k, 26) = 0;
      dataPlot(k, 27) = (*inter1->lambda(2))(0) ; // lambda1_{k+1}^- (nonimpulsive force)
      dataPlot(k, 28) = (*inter2->lambda(2))(0) ; // lambda1_{k+1}^-
      dataPlot(k, 29) = (*inter3->lambda(2))(0) ; // lambda1_{k+1}^-
      dataPlot(k, 30) = (*inter4->lambda(2))(0) ; // lambda1_{k+1}^-

      dataPlot(k, 31) = ( inter1->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda1_k^+
      dataPlot(k, 32) = ( inter2->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda2_k^+
      dataPlot(k, 33) = ( inter3->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda3_k^+
      dataPlot(k, 34) = ( inter4->lambdaMemory(2).getSiconosVector(0) )(0) ; // lambda4_k^+

      // std::cout << "dataPlot(k, 27)" << dataPlot(k, 27)  << std::endl;
      // std::cout << "dataPlot(k, 31)" << dataPlot(k, 31)  << std::endl;

      // std::cout <<" q->display()" <<  std::endl;
      // q->display();
      // std::cout <<" v->display()" <<  std::endl;
      // v->display();

      s->processEvents();
      ++show_progress;
      k++;
    }

    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
    double error=0.0, eps=1e-11;
    if ((error=ioMatrix::compareRefFile(dataPlot,
                                        "SliderCrankD1MinusLinearOSIVelocityLevel.ref",
                                        eps)) >= 0.0
        && error > eps)
      return 1;

  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in SliderCrankD1MinusLinear.cpp" << endl;
  }
}


/*
 * Copyright (c) 2012, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * You can contact the author at <acmarkus at ethz dot ch>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <msf_core/similaritytransform.h>

#include <Eigen/Eigenvalues>

namespace msf_core {
namespace similarity_transform {
From6DoF::From6DoF() { }

void From6DoF::AddMeasurement(const PosePair & measurement) {
  measurements_.push_back(measurement);
}

void From6DoF::AddMeasurement(const Pose & pose1, const Pose & pose2) {
  AddMeasurement(PosePair(pose1, pose2));
}

bool From6DoF::Compute(Pose & pose, double *scale, double *cond, double eps) {
  const int n = 4;  // Number of parameters we need to optimize.
  const int m = measurements_.size();

  if (m < 2)
    return false;

  Matrix4 M(Matrix4::Zero());  // Quaternion outer sum matrix.
  // Matrix collecting the measurements for position and scale.
  Eigen::Matrix<double, Eigen::Dynamic, n> A;
  A.resize(m * 3, Eigen::NoChange);

  Eigen::Matrix<double, Eigen::Dynamic, 1> b;
  b.resize(m * 3, Eigen::NoChange);

  for (int i = 0; i < m; i++) {
    // Quaternion averaging.
    const PosePair & pp = measurements_[i];
    const Eigen::Quaterniond q1 = GeometryMsgsToEigen(
        pp.first.pose.orientation);
    const Eigen::Quaterniond q2 = GeometryMsgsToEigen(
        pp.second.pose.orientation);
    Eigen::Quaterniond q(q1.inverse() * q2);
    M += q.coeffs() * q.coeffs().transpose();  // Order is x y z w here !!!

        //
    const Vector3 t1 = GeometryMsgsToEigen(pp.first.pose.position);
    const Vector3 t2 = GeometryMsgsToEigen(pp.second.pose.position);

    A.block<3, 3>(i * 3, 0) = Eigen::Matrix<double, 3, 3>::Identity() * -1;
    A.block<3, 1>(i * 3, 3) = q1.inverse() * t2;
    b.block<3, 1>(i * 3, 0) = q1.inverse() * t1;
  }

  // Mean quaternion.
  Eigen::SelfAdjointEigenSolver < Eigen::Matrix<double, 4, 4> > q_solver(M);
  // Eigenvalues/vectors are sorted in increasing order here ...
  Eigen::Quaterniond q_mean = Eigen::Quaterniond(
      q_solver.eigenvectors().col(3));

  // Mean position and scale.
  Matrix4 A_hat = A.transpose() * A;
  Matrix4 S_hat(Matrix4::Zero());
  Vector4 b_hat = A.transpose() * b;

  Eigen::JacobiSVD<Matrix4> svd(A_hat,
                                Eigen::ComputeFullU | Eigen::ComputeFullV);
  for (int i = 0; i < n; i++) {
    if (svd.singularValues()[i] < eps)
      S_hat(i, i) = 0;
    else
      S_hat(i, i) = 1 / svd.singularValues()[i];
  }
  if (cond)
    *cond = svd.singularValues()[0] / svd.singularValues()[n - 1];

  Vector4 x = svd.matrixV() * S_hat * svd.matrixU().transpose() * b_hat;
  if (scale)
    *scale = x[3];

  pose.pose.position = EigenToGeometryMsgs(x.block<3, 1>(0, 0));
  pose.pose.orientation = EigenToGeometryMsgs(q_mean);

  return true;
}
}
}  // namespace msf_core


// This file is part of PoseEstimation.
// This file is a modified version of p3p.m <http://rpg.ifi.uzh.ch/software_datasets.html>,
// see 3-Clause BSD license below.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

// Copyright (c) 2011, Laurent Kneip, ETH Zurich
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of ETH Zurich nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL ETH ZURICH BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef P3P_HPP_
#define P3P_HPP_

#include <Eigen/Dense>

class P3PTest;

namespace P3P
{
template <typename FloatType>
class P3P {
	friend ::P3PTest;

private:
	typedef Eigen::Matrix<std::complex<FloatType>, 4, 1> CMatrix_4x1;
	typedef Eigen::Matrix<FloatType, 3, 1> Matrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> Matrix_3x3R;
	typedef Eigen::Matrix<FloatType, 4, 1> Matrix_4x1;
	typedef Eigen::Matrix<FloatType, 4, 4> Matrix_4x4;
	const static FloatType m_eps;

public:
	// x = R'*(X - t) =>  x = R*X + t
	static void convertPose(FloatType *R, FloatType *t)
	{
		Eigen::Map<Matrix_3x3R> mR1(R);
		Eigen::Map<Matrix_3x1> mt1(t);
		Matrix_3x3 mR2 = mR1.transpose();
		Matrix_3x1 mt2 = -mR2*mt1;
		mR1 = mR2;
		mt1 = mt2;
	}

	// x = R'*(X - t)
	bool getMatrix(const FloatType *p2dn, const FloatType *p3d, int &nSols, std::vector<FloatType> &Ps)
	{
		// check degenerate case
		if (checkColinearity(p3d))
		{
			nSols = 0;
			Ps.resize(0);
			return false;
		}

		// select P1, f1 and compute T matrix, N matrix
		selectP1(p2dn, p3d);
		transformCoordinate();

		// compute constraints
		getConstraints();

		// compute real roots for the constraints
		getRealRoot();
		if (m_realRoots.empty())
		{
			nSols = 0;
			Ps.resize(0);
			return true;
		}

		// compute poses corresponding to the real root
		getPoses(nSols, Ps);

		return true;
	}

private:
	inline void getConstraints()
	{
		FloatType pcotn[2] = {m_p[1], m_p[0] * m_phi[0] / m_phi[1] - m_d12 * m_b}; // numerator of Eq. 9
		FloatType pcotd[2] = {m_p[1] * m_phi[0] / m_phi[1], -m_p[0] +  m_d12}; // denominator of Eq. 9
		FloatType pcotn2[3] = {}, pcotd2[3] = {}, pcotnd[3] = {}; // nume^2, denom^2, nume*denom
		polynomialMultiplication1(pcotn, pcotn, pcotn2);
		polynomialMultiplication1(pcotd, pcotd, pcotd2);
		polynomialMultiplication1(pcotn, pcotd, pcotnd);
		FloatType pcotd2n2[3] = {}; // denom^2 + nume^2
		pcotd2n2[0] = pcotd2[0] + pcotn2[0];
		pcotd2n2[1] = pcotd2[1] + pcotn2[1];
		pcotd2n2[2] = pcotd2[2] + pcotn2[2];

		FloatType pcos1[] = {-m_phi[1] * m_phi[1] * m_p[1] * m_p[1], 0, 0}; // term 1 of l.h.s of Eq.10, I think f_2^2 in original equation is phi_2^2
		pcos1[2] = -pcos1[0];
		polynomialMultiplication2(pcos1, pcotd2n2, m_as);

		FloatType p12 = m_p[0] * m_p[0];
		FloatType p122 = 2 * m_p[0] * m_p[1];
		FloatType p22 = m_p[1] * m_p[1];
		m_as[2] -= (p12 * pcotd2[0]); // term 1 of r.h.s of Eq.10
		m_as[3] -= (p12 * pcotd2[1]);
		m_as[4] -= (p12 * pcotd2[2]);
		m_as[1] += (p122 * pcotnd[0]); // term 2 of r.h.s of Eq.10
		m_as[2] += (p122 * pcotnd[1]);
		m_as[3] += (p122 * pcotnd[2]);
		m_as[0] -= (p22 * pcotn2[0]); // term 3 of r.h.s of Eq.10
		m_as[1] -= (p22 * pcotn2[1]);
		m_as[2] -= (p22 * pcotn2[2]);
	}

	void getPoses(int &nSols, std::vector<FloatType> &Ps)
	{
		nSols = 0;
		Ps.resize((3 + 9) * m_realRoots.size());
		for (size_t k = 0; k < m_realRoots.size(); k++)
		{
			nSols++;
			const FloatType cos_theta = m_realRoots[k];
			const FloatType sin_theta = std::sqrt(1 - (cos_theta * cos_theta));
			const FloatType cot_alpha =
				(m_phi[0] * m_p[0] / m_phi[1] + cos_theta * m_p[1] - m_d12 * m_b) /
				(m_phi[0] * m_p[1] * cos_theta / m_phi[1] - m_p[0] + m_d12); // Eq. 9
			const FloatType sin_alpha = std::sqrt(1 / (cot_alpha * cot_alpha + 1));
			FloatType cos_alpha = std::sqrt(1 - (sin_alpha * sin_alpha));
			if (cot_alpha < 0)
			{
				cos_alpha = -cos_alpha;
			}

			FloatType *pC = &(Ps[(3 + 9) * k]);
			Eigen::Map<Matrix_3x1> mC(pC); // Eq. 5
			mC[0] = cos_alpha;
			mC[1] = sin_alpha * cos_theta;
			mC[2] = sin_alpha * sin_theta;

			Eigen::Map<Matrix_3x3R> mR(pC + 3); // Eq. 6
			mR(0,0) = -mC[0];
			mR(0,1) = -mC[1];
			mR(0,2) = -mC[2];
			mR(1, 0) = sin_alpha;
			mR(1, 1) = -cos_alpha * cos_theta;
			mR(1, 2) = -cos_alpha * sin_theta;
			mR(2, 0) = 0;
			mR(2, 1) = -sin_theta;
			mR(2, 2) = cos_theta;

			mC *= (m_d12 * (sin_alpha * m_b + cos_alpha));
			mC = m_mP1 + m_mN.transpose() * mC; // Eq. 12
			mR = m_mN.transpose() * mR.transpose() * m_mT; // Eq. 13
		}
	}

	inline void getRealRoot()
	{
		m_realRoots.resize(0);
		Matrix_4x4 mCom; // companion matrix
		mCom.setZero();
		mCom(1, 0) = mCom(2, 1) = mCom(3, 2) = 1;
		mCom(0, 3) = -m_as[4] / m_as[0];
		mCom(1, 3) = -m_as[3] / m_as[0];
		mCom(2, 3) = -m_as[2] / m_as[0];
		mCom(3, 3) = -m_as[1] / m_as[0];
		Eigen::EigenSolver<Matrix_4x4> eig(mCom, false);
		CMatrix_4x1 mSols = eig.eigenvalues();

		for (int k = 0; k < 4; k++)
		{
			if (std::abs(mSols[k].imag()) < m_eps)
			{
				m_realRoots.push_back(mSols[k].real());
			}
		}
	}

	inline void selectP1(const FloatType *f, const FloatType *P)
	{
		const FloatType *f1 = f;
		const FloatType *f2 = f1 + 3;
		const FloatType *f3 = f2 + 3;
		// compute 3rd row of T
		FloatType T3[3] = {f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0]};
		FloatType sign = T3[0] * f3[0] + T3[1] * f3[1] + T3[2] * f3[2];
		if (sign > 0)
		{
			m_f[0] = f + 3;
			m_f[1] = f;
			m_P[0] = P + 3;
			m_P[1] = P;
		}
		else
		{
			m_f[0] = f;
			m_f[1] = f + 3;
			m_P[0] = P;
			m_P[1] = P + 3;
		}
		m_f[2] = f + 6;
		m_P[2] = P + 6;
		m_mP1[0] = m_P[0][0];
		m_mP1[1] = m_P[0][1];
		m_mP1[2] = m_P[0][2];
	}

	void transformCoordinate()
	{
		// compute T matrix
		m_mT(0, 0) = m_f[0][0];
		m_mT(0, 1) = m_f[0][1];
		m_mT(0, 2) = m_f[0][2];
		m_mT(1, 0) = m_f[1][0];
		m_mT(1, 1) = m_f[1][1];
		m_mT(1, 2) = m_f[1][2];
		FloatType cos_beta = m_mT.row(0).dot(m_mT.row(1));
		m_mT.row(2) = m_mT.row(0).cross(m_mT.row(1));
		m_mT.row(1) = m_mT.row(2).cross(m_mT.row(0));
		m_mT.rowwise().normalize();

		// compute N matrix
		m_mN(0, 0) = m_P[1][0] - m_P[0][0];
		m_mN(0, 1) = m_P[1][1] - m_P[0][1];
		m_mN(0, 2) = m_P[1][2] - m_P[0][2];
		m_d12 = m_mN.row(0).norm();
		m_mN(1, 0) = m_P[2][0] - m_P[0][0];
		m_mN(1, 1) = m_P[2][1] - m_P[0][1];
		m_mN(1, 2) = m_P[2][2] - m_P[0][2];
		m_mN.row(2) = m_mN.row(0).cross(m_mN.row(1));
		m_mN.row(1) = m_mN.row(2).cross(m_mN.row(0));
		m_mN.rowwise().normalize();

		// compute b
		m_b = std::sqrt((1 / (1 - (cos_beta * cos_beta))) - 1); // Eq. 3
		if (cos_beta < 0)
		{
			m_b = -m_b;
		}

		// compute phis
		Matrix_3x1 tmp(m_f[2]);
		tmp = m_mT * tmp;
		m_phi[0] = tmp[0] / tmp[2]; // Eq. 8
		m_phi[1] = tmp[1] / tmp[2]; // Eq. 8

		// compute ps
		tmp[0] = m_P[2][0];
		tmp[1] = m_P[2][1];
		tmp[2] = m_P[2][2];
		tmp = m_mN * (tmp - m_mP1); // Eq. 2
		m_p[0] = tmp[0];
		m_p[1] = tmp[1];
	}

private:
	static inline bool checkColinearity(const FloatType *p)
	{
		FloatType p12[3] = {p[3] - p[0], p[4] - p[1], p[5] - p[2]};
		FloatType p13[3] = {p[6] - p[0], p[7] - p[1], p[8] - p[2]};
		FloatType pcross[] = {p12[2] * p13[1] - p12[1] * p13[2], p12[0] * p13[2] - p12[2] * p13[0], p12[1] * p13[0] - p12[0] * p13[1]};
		FloatType ncross = std::sqrt(pcross[0] * pcross[0] + pcross[1] * pcross[1] + pcross[2] * pcross[2]);
		return (ncross < m_eps);
	}

	// 1st order polynomial multiplication
	// (ax + b) * (cx + d) = (acx^2 + (ad + bc)x + bd)
	// p1 = [x 1]
	// p2 = [x 1]
	// p3 = [x^2 x 1]
	static inline void polynomialMultiplication1(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^2
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x
		p3[2] = (p1[1])*(p2[1]);					// 1
	}

	// 2nd order polynomial multiplication
	// (ax^2 + bx + c) * (dx^2 + ex + f) = (adx^4 + (ae + bd)x^3 + (af + be + cd)x^2 + (bf + ce)x + cf
	// p1 = [x^2 x 1]
	// p2 = [x^2 x 1]
	// p3 = [x^4 x^3 x^2 x 1]
	static inline void polynomialMultiplication2(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);										// x^4
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);						// x^3
		p3[2] = (p1[0])*(p2[2]) + (p1[1])*(p2[1]) + (p1[2])*(p2[0]);	// x^2
		p3[3] = (p1[1])*(p2[2]) + (p1[2])*(p2[1]);						// x
		p3[4] = (p1[2])*(p2[2]);										// 1
	}

private:
	const FloatType *m_f[3]; // pointer to 2D feature vector
	const FloatType *m_P[3]; // pointer to 3D feature vector
	Matrix_3x1 m_mP1;
	Matrix_3x3 m_mT;
	Matrix_3x3 m_mN;
	FloatType m_phi[2];
	FloatType m_p[2];
	FloatType m_b;
	FloatType m_d12;
	FloatType m_as[5]; // coefficient of 4th order polynomial
	std::vector<FloatType> m_realRoots; // real root for cos_theta polynomial
};

template <typename FloatType>
const FloatType P3P<FloatType>::m_eps = 1E-7;
}

#endif // P3P_HPP_