SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor)
{
  // return input(level) (ie with lambda[level]) for all Interactions.
  // assert(level>=0);

  DEBUG_BEGIN("Simulation::input(unsigned int level, unsigned int coor)\n");
  DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor);

  InteractionsGraph::VIterator ui, uiend;
  SP::Interaction inter;
  SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();

  SP::SiconosVector lambda (new SiconosVector (_nsds->topology()->indexSet0()->size() ));
  int i=0;
  for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
  {
    inter = indexSet0->bundle(*ui);
    assert(inter->lowerLevelForOutput() <= level);
    assert(inter->upperLevelForOutput() >= level);
    lambda->setValue(i,inter->lambda(level)->getValue(coor));
    i++;
  }
  DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n");
  return lambda;
}

void LagrangianDSTest::testcomputeDS()
{
  std::cout << "-->Test: computeDS." <<std::endl;
  DynamicalSystem * ds(new LagrangianDS(tmpxml2));
  SP::LagrangianDS copy =  std11::static_pointer_cast<LagrangianDS>(ds);
  double time = 1.5;
  ds->initialize("EventDriven", time);
  ds->computeRhs(time);
  std::cout << "-->Test: computeDS." <<std::endl;
  ds->computeJacobianRhsx(time);
  std::cout << "-->Test: computeDS." <<std::endl;
  SimpleMatrix M(3, 3);
  M(0, 0) = 1;
  M(1, 1) = 2;
  M(2, 2) = 3;
  SP::SiconosMatrix jx = ds->jacobianRhsx();
  SP::SiconosVector vf = ds->rhs();

  CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSI : ", *(vf->vector(0)) == *velocity0, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSJ : ", prod(M, *(vf->vector(1))) == (copy->getFExt() - copy->getFInt() - copy->getFGyr()) , true);

  CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 0))) == (copy->getJacobianFL(0)) , true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 1))) == (copy->getJacobianFL(1)) , true);
  std::cout << "--> computeDS test ended with success." <<std::endl;


}

void SphereLDS::computeJacobianFGyrqDot(SP::SiconosVector q, SP::SiconosVector v)
{
  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double sintheta   = sin(theta);

  _jacobianFGyrqDot->zero();


  (*_jacobianFGyrqDot)(3, 3) = 0;
  (*_jacobianFGyrqDot)(3, 4) = I * psidot * sintheta;
  (*_jacobianFGyrqDot)(3, 5) = I * phidot * sintheta;

  (*_jacobianFGyrqDot)(4, 3) = -I * psidot * sintheta;
  (*_jacobianFGyrqDot)(4, 4) = 0;
  (*_jacobianFGyrqDot)(4, 5) = -I * thetadot * sintheta;

  (*_jacobianFGyrqDot)(5, 3) =  -I * phidot * sintheta;
  (*_jacobianFGyrqDot)(5, 4) =  -I * thetadot * sintheta;
  (*_jacobianFGyrqDot)(5, 5) = 0;

}

SphereLDS::SphereLDS(double r, double m,
                     SP::SiconosVector qinit,
                     SP::SiconosVector vinit)
  : LagrangianDS(qinit, vinit), radius(r), massValue(m)
{

  normalize(q(), 3);
  normalize(q(), 4);
  normalize(q(), 5);
  _ndof = 6;

  assert(qinit->size() == _ndof);
  assert(vinit->size() == _ndof);

  _mass.reset(new SimpleMatrix(_ndof, _ndof));
  _mass->zero();
  I = massValue * radius * radius * 2. / 5.;
  (*_mass)(0, 0) = (*_mass)(1, 1) = (*_mass)(2, 2) = massValue;    ;
  (*_mass)(3, 3) = (*_mass)(4, 4) = (*_mass)(5, 5) = I;

  computeMass();

  _jacobianFGyrq.reset(new SimpleMatrix(_ndof, _ndof));
  _jacobianFGyrqDot.reset(new SimpleMatrix(_ndof, _ndof));

  _fGyr.reset(new SiconosVector(_ndof));
  _fGyr->zero();

  computeFGyr();


}

void normalize(SP::SiconosVector q, unsigned int i)
{

  q->setValue(i, fmod(q->getValue(i), _2PI));

  assert(fabs(q->getValue(i)) - std::numeric_limits<double>::epsilon() >= 0.);
  assert(fabs(q->getValue(i)) < _2PI);

}

void adjointInput::beta(double t, SiconosVector& xvalue, SP::SiconosVector beta)
{


  beta->setValue(0, -1.0 / 2.0 * xvalue(1) + 1.0 / 2.0) ;
  beta->setValue(1, 1.0 / 2.0 * xvalue(0)) ;
#ifdef SICONOS_DEBUG
  std::cout << "beta\n" << std::endl;;
  beta->display();
#endif

}

/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("point de contact :\n");
  _Pc1->display();
  printf("center of masse :\n");
  q1->display();
#endif
  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;


  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);


  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
  _jachqT->display();
#endif
}

void DynamicalSystem::setX0Ptr(SP::SiconosVector newPtr)
{
  // check dimensions ...
  if (newPtr->size() != _n)
    RuntimeException::selfThrow("DynamicalSystem::setX0Ptr - inconsistent sizes between x0 input and n - Maybe you forget to set n?");
  _x0 = newPtr;
  _normRef = _x0->norm2() + 1;
}

void SphereLDS::computeJacobianFGyrq(SP::SiconosVector q, SP::SiconosVector v)
{
  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double costheta = cos(theta);

  _jacobianFGyrq->zero();

  (*_jacobianFGyrq)(3, 3) = -I * psidot * phidot * costheta;
  (*_jacobianFGyrq)(4, 3) = I * psidot * thetadot * costheta;
  (*_jacobianFGyrq)(5, 3) = I * psidot * thetadot * costheta;


}

void KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )
{

  //x1->display();
  double X1 = x1->getValue(0);
  double Y1 = x1->getValue(1);
  double Z1 = x1->getValue(2);
  double q10 = x1->getValue(3);
  double q11 = x1->getValue(4);
  double q12 = x1->getValue(5);
  double q13 = x1->getValue(6);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(x2)
  {
    //printf("checkInitPos x2:\n");
    //x2->display();
    X2 = x2->getValue(0);
    Y2 = x2->getValue(1);
    Z2 = x2->getValue(2);
    q20 = x2->getValue(3);
    q21 = x2->getValue(4);
    q22 = x2->getValue(5);
    q23 = x2->getValue(6);
  }

  if (Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hx is large :" << Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hy is large :" << Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hz is large :" << Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
     
  
  // printf("checkInitPos Hx : %e\n", Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hy : %e\n", Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hz : %e\n", Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));


}

void DynamicalSystem::setRhsPtr(SP::SiconosVector newPtr)
{
  // Warning: this only sets the pointer (*x)[1]

  // check dimensions ...
  if (newPtr->size() != _n)
    RuntimeException::selfThrow("DynamicalSystem::setRhsPtr - inconsistent sizes between x input and n - Maybe you forget to set n?");

  _x[1] = newPtr;
}

void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::SiconosVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
  if (init) // y = subA * x , else y += subA * x
    y->zero();
  private_addprod(x, A, startCol, 0 , y);

}

/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}

BulletDS::BulletDS(SP::BulletWeightedShape weightedShape,
                   SP::SiconosVector position,
                   SP::SiconosVector velocity,
                   SP::SiconosVector relative_position,
                   SP::SiconosVector relative_orientation,
                   int group) :
  NewtonEulerDS(position, velocity, weightedShape->mass(),
                weightedShape->inertia()),
  _weightedShape(weightedShape),
  _collisionObjects(new CollisionObjects())
{
  SiconosVector& q = *_q;


  /* with 32bits input ... 1e-7 */
  if (fabs(sqrt(pow(q(3), 2) + pow(q(4), 2) +
                pow(q(5), 2) + pow(q(6), 2)) - 1.) >= 1e-7)
  {
    RuntimeException::selfThrow(
      "BulletDS: quaternion in position parameter is not a unit quaternion "
    );
  }

  /* initialisation is done with the weighted shape as the only one
   * collision object */
  if (! relative_position)
  {
    relative_position.reset(new SiconosVector(3));
    relative_position->zero();
  }
  if (! relative_orientation)
  {
    relative_orientation.reset(new SiconosVector(4));
    relative_orientation->zero();
    (*relative_orientation)(1) = 1;
  }

  addCollisionShape(weightedShape->collisionShape(), relative_position,
                      relative_orientation, group);

}

/*get the quaternion from siconos and 1787update the CADs model*/
void MBTB_updateDSFromSiconos()
{
  //ACE_times[ACE_TIMER_UPDATE_POS].start();
  for(unsigned int numDS=0; numDS<sNbOfBodies; numDS++)
  {
    SP::SiconosVector q = sDS[numDS]->q();
    //printf("step %d siconos %s ->q:\n",mTimerCmp,sPieceName[numDS]);
    //q->display();
    double x=q->getValue(0);
    double y=q->getValue(1);
    double z=q->getValue(2);
    double q1=q->getValue(3);
    double q2=q->getValue(4);
    double q3=q->getValue(5);
    double q4=q->getValue(6);
    ACE_times[ACE_TIMER_UPDATE_POS].start();
    CADMBTB_moveObjectFromQ(numDS,x,y,z,q1,q2,q3,q4);
    ACE_times[ACE_TIMER_UPDATE_POS].stop();
    int res = sTimerCmp%FREQ_UPDATE_GRAPHIC;
    ACE_times[ACE_TIMER_GRAPHIC].start();
    if(!res)
    {
      /*THIS CODE REBUILD THE GRAPHICAL MODEL
      getContext()->Erase(spAISToposDS[numDS]);
      spAISToposDS[numDS] = new AIS_Shape( sTopoDSPiece[numDS] );
      getContext()->Display( spAISToposDS[numDS], false );*/

      //spAISToposDS[numDS]->SetTransformation(&(sGeomTrsf[numDS]),true,false);//new Geom_Transformation(sTrsfPiece[numDS]),true);

      CADMBTB_moveGraphicalModelFromModel(numDS,numDS);

      //spAISToposDS[numDS]->SetTransformation(&(sGeomTrsf[numDS])
      //				     ,false,true);
      //      getContext()->Display( spAISToposDS[numDS], false );
    }
    ACE_times[ACE_TIMER_GRAPHIC].stop();
  }

}

void private_addprod(double a, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SPC::SiconosVector x, SP::SiconosVector y)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A->isBlock())
    SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for block matrix.");

  // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
  // Then computation of y = subA*x + y.
  unsigned int numA = A->getNum();
  unsigned int numY = y->getNum();
  unsigned int numX = x->getNum();
  unsigned int sizeX = x->size();
  unsigned int sizeY = y->size();

  if (numX != numY)
    SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x and y of different types.");

  if (numY == 1 && numX == 1)
  {

    assert(y->dense() != x->dense());

    if (numA == 1)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 2)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 3)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 4)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else //if(numA==5)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
  }
  else // x and y sparse
  {
    if (numA == 4)
      *y->sparse() += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
    else
      SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x, y  sparse and A not sparse.");
  }

}

void SphereLDS::computeFGyr(SP::SiconosVector q, SP::SiconosVector v)
{

  assert(q->size() == 6);
  assert(v->size() == 6);

  //  normalize(q,3);
  //normalize(q,4);
  //normalize(q,5);

  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double sintheta   = sin(theta);

  (*_fGyr)(0) = (*_fGyr)(1) = (*_fGyr)(2) = 0;

  (*_fGyr)(3) = I * psidot * phidot * sintheta;
  (*_fGyr)(4) = -I * psidot * thetadot * sintheta;
  (*_fGyr)(5) = -I * phidot * thetadot * sintheta;
}

void SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
  if (_num == 1)
  {
    if (vIn->size() != size(1))
      RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: the given vector does not have the right length");
    DenseVect tmpV = DenseVect(size(0));
    for (unsigned int i = 0; i < size(1); i++)
    {
       ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
       (*vIn)(i) = norm_inf(tmpV);
    }
  }
  else
    RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: not implemented for data other than DenseMat");
}

void KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 )
{
  DEBUG_BEGIN("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2) \n");
  _dotjachq->zero();
  
  double Xdot1 = qdot1->getValue(0);
  double Ydot1 = qdot1->getValue(1);
  double Zdot1 = qdot1->getValue(2);
  double qdot10 = qdot1->getValue(3);
  double qdot11 = qdot1->getValue(4);
  double qdot12 = qdot1->getValue(5);
  double qdot13 = qdot1->getValue(6);

  double Xdot2 = 0;
  double Ydot2 = 0;
  double Zdot2 = 0;
  double qdot20 = 1;
  double qdot21 = 0;
  double qdot22 = 0;
  double qdot23 = 0;

  if(qdot2)
  {
    Xdot2 = qdot2->getValue(0);
    Ydot2 = qdot2->getValue(1);
    Zdot2 = qdot2->getValue(2);
    qdot20 = qdot2->getValue(3);
    qdot21 = qdot2->getValue(4);
    qdot22 = qdot2->getValue(5);
    qdot23 = qdot2->getValue(6);
    DotJd1d2(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13, Xdot2, Ydot2, Zdot2, qdot20, qdot21, qdot22, qdot23);
  }
  else
    DotJd1(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13);

  DEBUG_END("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 ) \n");
}

void rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )
{
  DEBUG_BEGIN("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )\n");
  DEBUG_EXPR(v->display(););