/// Create the context for the matrix tests.
    void SetUp()
    {        
        sofa::component::init();
//        if( sofa::simulation::getSimulation()==NULL )
        sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation());

        /// Create the scene
        root = simulation->createNewGraph("root");

        PointSetTopologyContainer::SPtr topology = New<PointSetTopologyContainer>();
        root->addObject(topology);

        dofs = New<MechanicalObject>();
        root->addObject(dofs);

        projection = New<ProjectToPlaneConstraint>();
        root->addObject(projection);

        /// Set the values
        numNodes = 3;
        dofs->resize(numNodes);


        origin = CPos(0,0,0);
        projection->f_origin.setValue(origin);
        normal = CPos(1,1,1);
        projection->f_normal.setValue(normal);

    }

    /**
     * @brief Create the physical model
     *
     * @param m Mass of the particle
     * @param s Stiffness of the spring
     * @param d Damping ratio of the spring
     * @param xx0 initial position
     * @param vv0 initial velocity
     */
    void setup(
            SReal m, SReal s, SReal d,
            SReal xx0, SReal vv0
            )
    {
        mass = m;
        stiffness = s;
        damping = d;
        x0=xx0; v0=vv0;

        node = clearScene();
        node->setGravity( Vec3(0,0,0) );

        // The oscillator
        simulation::Node::SPtr oscillator = node->createChild("oscillator");

        DOF = addNew<MechanicalObject1>(oscillator,"DOF");
        DOF->resize(1);
        DOF->writePositions()[0]  = Vec1(x0);
        DOF->writeVelocities()[0] = Vec1(v0);

        UniformMass1::SPtr Mass = addNew<UniformMass1>(oscillator,"mass");
        Mass->mass.setValue( mass );

        compliance = addNew<UniformCompliance1>(oscillator,"compliance");
        compliance->isCompliance.setValue(false);
        compliance->compliance.setValue(1.0/stiffness);
        compliance->damping.setValue(damping);
    }

    /// Create the context for the tests.
    void SetUp()
    {
//        if( sofa::simulation::getSimulation()==NULL )
        sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation());

        /// Create the scene
        root = simulation->createNewGraph("root");

        PointSetTopologyContainer::SPtr topology = core::objectmodel::New<PointSetTopologyContainer>();
        root->addObject(topology);

        dofs = core::objectmodel::New<MechanicalObject>();
        root->addObject(dofs);

        projection = core::objectmodel::New<ProjectToLineConstraint>();
        root->addObject(projection);

        /// Set the values
        numNodes = 3;
        dofs->resize(numNodes);


        origin = CPos(0,0,0);
        projection->f_origin.setValue(origin);
        direction = CPos(1,1,1);
        projection->f_direction.setValue(direction);

    }

        /// After simulation compare the positions of points to the theoretical positions.
        bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition)
        {
            // Init simulation
            sofa::simulation::getSimulation()->init(root.get());

            // Compute the theoretical final positions    
            VecCoord finalPos;
            typename PatchTestMovementConstraint::SPtr bilinearConstraint  = root->get<PatchTestMovementConstraint>(root->SearchDown);
            typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(root->SearchDown);
            typename MechanicalObject::ReadVecCoord x0 = dofs->readPositions();
            bilinearConstraint->getFinalPositions( finalPos,*dofs->write(core::VecCoordId::position()) );

            // Initialize
            size_t numNodes = finalPos.size();
            VecCoord xprev(numNodes);
            VecDeriv dx(numNodes); 
            bool hasConverged = true;
            
            for (size_t i=0; i<numNodes; i++)
            {
                xprev[i] = CPos(0,0,0);
            }

            // Animate
            do
            {
                hasConverged = true;
                sofa::simulation::getSimulation()->animate(root.get(),0.5);
                typename MechanicalObject::ReadVecCoord x = dofs->readPositions();

                // Compute dx
                for (size_t i=0; i<x.size(); i++)
                {
                    dx[i] = x[i]-xprev[i];
                    // Test convergence
                    if(dx[i].norm()>convergenceAccuracy) hasConverged = false;
                }
               
                // xprev = x
                for (size_t i=0; i<numNodes; i++)
                {
                    xprev[i]=x[i];
                }
            }
            while(!hasConverged); // not converged

            // Compare the theoretical positions and the simulated positions   
            bool succeed=true;
            for(size_t i=0; i<finalPos.size(); i++ )
            {
                if((finalPos[i]-x0[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition)
                {   
                    succeed = false;
                    ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x0[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl
                        << "difference = " <<(finalPos[i]-x0[i]).norm() << std::endl;
                }
            }
            return succeed;
        }

    /// After simulation compare the positions of points to the theoretical positions.
    bool compareSimulatedToTheoreticalPositions( double h, double tolerancePosition, double toleranceEnergy = 1e-13, double checkEnergyConservation=false)
    {
        int i = 0;
        // Init simulation
        sofa::simulation::getSimulation()->init(root.get());
        double time = root->getTime();

        // Get mechanical object
        simulation::Node::SPtr massNode = root->getChild("MassNode");
        typename MechanicalObject::SPtr dofs = massNode->get<MechanicalObject>(root->SearchDown);

        // Animate
        do
        {
            // Record the mass position
            Coord p0=dofs.get()->read(sofa::core::ConstVecCoordId::position())->getValue()[0];

            double absoluteError = fabs(p0[1]-positionsArray[i]);

            // Compare mass position to the theoretical position
            if( absoluteError > tolerancePosition )
            {
                ADD_FAILURE() << "Position of mass at time " << time << " is wrong: "  << std::endl
                    <<" expected Position is " << positionsArray[i] << std::endl
                    <<" actual Position is   " << p0[1] << std::endl
                    << "absolute error     = " << absoluteError << std::endl;
                return false;
            }

            //Animate
            sofa::simulation::getSimulation()->animate(root.get(),h);
            time = root->getTime();


            // Check if hamiltonian energy is constant when there is no damping
            if(checkEnergyConservation && fabs(variationalSolver->f_hamiltonianEnergy.getValue() -totalEnergy) > toleranceEnergy )
            {
                ADD_FAILURE() << "Hamiltonian energy at time " << time << " is wrong: "  << std::endl
                    <<" expected Energy is " << totalEnergy << std::endl
                    <<" actual Energy is   " << variationalSolver->f_hamiltonianEnergy.getValue() << std::endl
                    << "absolute error     = " << fabs(variationalSolver->f_hamiltonianEnergy.getValue() -totalEnergy) << std::endl;
                return false;
            }

            // Iterate
            i++;
        }
        while (time < 2);
        return true;
    }

void DefaultCollisionGroupManager::clearGroup(const Container &inNodes,
                                              simulation::Node::SPtr group)
{
    core::objectmodel::BaseNode::SPtr parent = *inNodes.begin();
    while(!group->child.empty()) parent->moveChild(core::objectmodel::BaseNode::SPtr(group->child.begin()->get()->toBaseNode()));

    simulation::CleanupVisitor cleanupvis(sofa::core::ExecParams::defaultInstance());
    cleanupvis.execute(group.get());
    simulation::DeleteVisitor vis(sofa::core::ExecParams::defaultInstance());
    vis.execute(group.get());
    group->detachFromGraph();
    //delete group;
    group.reset();
}

	 // Load the scene BezierTetrahedronTopology.sc from the Scenes directory
    void createScene()
    {
		// GenerateCylinder object
        typename sofa::component::engine::GenerateCylinder<DataTypes>::SPtr eng= sofa::modeling::addNew<sofa::component::engine::GenerateCylinder<DataTypes> >(root,"cylinder");
		eng->f_radius=0.2;
		eng->f_height=1.0;
		// TetrahedronSetTopologyContainer object
		sofa::component::topology::TetrahedronSetTopologyContainer::SPtr container1= sofa::modeling::addNew<sofa::component::topology::TetrahedronSetTopologyContainer>(root,"Container1");
		sofa::modeling::setDataLink(&eng->f_tetrahedra,&container1->d_tetrahedron);
		sofa::modeling::setDataLink(&eng->f_outputTetrahedraPositions,&container1->d_initPoints);
		// TetrahedronSetGeometryAlgorithms object
        typename sofa::component::topology::TetrahedronSetGeometryAlgorithms<DataTypes>::SPtr geo1= sofa::modeling::addNew<sofa::component::topology::TetrahedronSetGeometryAlgorithms<DataTypes> >(root);
		// mechanicalObject object
        typename MechanicalObject::SPtr meca1= sofa::modeling::addNew<MechanicalObject>(root);
		sofa::modeling::setDataLink(&eng->f_outputTetrahedraPositions,&meca1->x);
		// subnode
	    simulation::Node::SPtr bezierNode = root->createChild("BezierTetrahedronTopology");
		// BezierTetrahedronSetTopologyContainer
		sofa::component::topology::BezierTetrahedronSetTopologyContainer::SPtr container2= sofa::modeling::addNew<sofa::component::topology::BezierTetrahedronSetTopologyContainer>(bezierNode,"Container2");
		// Mesh2BezierTopologicalMapping
		sofa::component::topology::Mesh2BezierTopologicalMapping::SPtr mapping= sofa::modeling::addNew<sofa::component::topology::Mesh2BezierTopologicalMapping>(bezierNode,"Mapping");
		mapping->setTopologies(container1.get(),container2.get());
		mapping->bezierTetrahedronDegree=3;
		// mechanicalObject object
        typename MechanicalObject::SPtr meca2= sofa::modeling::addNew<MechanicalObject>(bezierNode,"BezierMechanicalObject");
		// BezierTetrahedronSetGeometryAlgorithms
        typename sofa::component::topology::BezierTetrahedronSetGeometryAlgorithms<DataTypes>::SPtr geo2= sofa::modeling::addNew<sofa::component::topology::BezierTetrahedronSetGeometryAlgorithms<DataTypes> >(bezierNode);
		// MeshMatrixMass
        typename MeshMatrixMass::SPtr mass= sofa::modeling::addNew<MeshMatrixMass >(bezierNode,"BezierMass");
		mass->m_massDensity=1.0;
		mass->d_integrationMethod.setValue(std::string("analytical"));
	}

    /// After simulation compare the positions of points to the theoretical positions.
    bool compareSimulatedToTheoreticalPositions(double tolerance)
    {
        // Init simulation
        sofa::simulation::getSimulation()->init(root.get());
        double time = root->getTime();
        double stiffnessSpring = 100;
        double mass = 10;
        double w = sqrt(stiffnessSpring/mass);

        // Get mechanical object
        simulation::Node::SPtr massNode = root->getChild("MassNode");
        typename MechanicalObject::SPtr dofs = massNode->get<MechanicalObject>(root->SearchDown);

        // Animate
        do
        {
            // Record the mass position
            Coord p0=dofs.get()->read(sofa::core::ConstVecCoordId::position())->getValue()[0];

            // Absolute error
            double absoluteError = fabs(p0[1]-(cos(w*time)));

            // Compare mass position to the theoretical position
            if( absoluteError > tolerance )
            {
                ADD_FAILURE() << "Position of mass at time " << time << " is wrong: "  << std::endl
                    <<" expected Position is " << cos(sqrt(stiffnessSpring/mass)*time) << std::endl
                    <<" actual Position is   " << p0[1] << std::endl
                    << "absolute error     = " << absoluteError << std::endl;
                return false;
            }

            //Animate
            sofa::simulation::getSimulation()->animate(root.get(),0.001);
            time = root->getTime();
        }
        while (time < 2);
        return true;
    }

    /// Create the context for the scene
    void createScene(double K, double m, double l0, double rm=0, double rk=0)
    {
        // Init simulation
        sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation());
        root = simulation::getSimulation()->createNewGraph("root");

        // Create the scene
        root->setGravity(Coord(0,-10,0));

        // Solver
        variationalSolver = addNew<VariationalSymplecticSolver> (getRoot());
        variationalSolver->f_rayleighStiffness.setValue(rk);
        variationalSolver->f_rayleighMass.setValue(rm);
        variationalSolver->f_computeHamiltonian.setValue(1);
        variationalSolver->f_newtonError.setValue(1e-12);//1e-18
        variationalSolver->f_newtonSteps.setValue(4);//7

        CGLinearSolver::SPtr cgLinearSolver = addNew<CGLinearSolver> (getRoot());
        cgLinearSolver->f_maxIter=3000;
        cgLinearSolver->f_tolerance =1e-12;
        cgLinearSolver->f_smallDenominatorThreshold=1e-12;

        // Set initial positions and velocities of fixed point and mass
        MechanicalObject3::VecCoord xFixed(1);
        MechanicalObject3::DataTypes::set( xFixed[0], 0., 2.,0.);
        MechanicalObject3::VecDeriv vFixed(1);
        MechanicalObject3::DataTypes::set( vFixed[0], 0.,0.,0.);
        MechanicalObject3::VecCoord xMass(1);
        MechanicalObject3::DataTypes::set( xMass[0], 0., 1.,0.);
        MechanicalObject3::VecDeriv vMass(1);
        MechanicalObject3::DataTypes::set( vMass[0], 0., 0., 0.);

        // Mass spring system
        root = this-> createMassSpringSystem(
                root,   // add mass spring system to the node containing solver
                K,      // stiffness
                m,      // mass
                l0,     // spring rest length
                xFixed, // Initial position of fixed point
                vFixed, // Initial velocity of fixed point
                xMass,  // Initial position of mass
                vMass); // Initial velocity of mass
    }

    /** Constrain one particle, and not the last one.
    Detects bugs like not setting the projection matrix entries beyond the last constrained particle
    */
    void init_2bones()
    {
        joints.clear();
        
        typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
        x.resize(2);
        VecCoord rigids(2);
        DataTypes::setCPos(x[1], CPos(0,1,0));
        DataTypes::setCRot(x[1], CRot(0.707107,0, 0, 0.707107)); // rotation x: 90 degree
        Coord target(CPos(1,1,1), CRot(0,0.382683,0,0.92388)); //rotation y : 45 degrees

        
        joints.resize(2);

        joints[0].addChannel(x[0], 0);
        joints[0].addChannel(target, 1);
        joints[0].setRestPosition(x[0]);

        joints[1].addChannel(x[1], 0);
        joints[1].setRestPosition(x[1]);
        joints[1].mParentIndex = 0;

        helper::vector<int> bones(2,0); bones[1] = 1;
        projection->setSkeletalMotion(joints, bones);

        /// Init
        sofa::simulation::getSimulation()->init(root.get());
        simulation->animate(root.get(),0.25);
        simulation->animate(root.get(),0.25);

    }

    /**
     * @brief Perform the time integration over several time steps and compare with the theoretical solution
     * @param endTime Simulation stops when time is higher than this
     * @param dt Time step
     * @param tolerance  Admissible absolute error
     * @param debug Print debug info
     */
    void testTimeIntegration( SReal endTime, SReal dt, SReal tolerance, bool debug )
    {

        node->setDt(dt);

        //**************************************************
        sofa::simulation::getSimulation()->init(node.get());
        //**************************************************

        if(debug) simulation::getSimulation()->exportXML ( node.get(), "/tmp/oscilator.scn" );


        //**************************************************
        // Simulation loop
        SReal t=0;
        printInfo(debug,t);
        while( t<endTime )
        {
            simulation::getSimulation()->animate(node.get(),dt);
            t+=dt ;
            printInfo(debug,t);

            SReal x = DOF->readPositions()[0][0];
            ASSERT_TRUE( fabs(x-theoreticalPosition(t)) < tolerance );
        }
        //**************************************************


    }

    /// Create the context for the tests.
    void SetUp()
    {        
//        if( sofa::simulation::getSimulation()==NULL )
        sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation());

        /// Create the scene
        root = simulation->createNewGraph("root");

        dofs = core::objectmodel::New<MechanicalObject>();
        root->addObject(dofs);

        projection = core::objectmodel::New<SkeletalMotionConstraint>();
        root->addObject(projection);

       


    }

    /** Constrain all the particles.
    */
    void init_allParticlesConstrained()
    {
        indices.clear();
        for(unsigned i = 0; i<numNodes; i++)
            indices.push_back(i);
         projection->f_indices.setValue(indices);

         /// Init
         sofa::simulation::getSimulation()->init(root.get());
    }

    /** Constrain one particle, and not the last one.
    Detects bugs like not setting the projection matrix entries beyond the last constrained particle
    */
    void init_oneConstrainedParticle()
    {
        indices.clear();
        indices.push_back(1);
        std::sort(indices.begin(),indices.end()); // checking vectors in linear time requires sorted indices
        projection->f_indices.setValue(indices);

        /// Init
        sofa::simulation::getSimulation()->init(root.get());
    }

   bool initScene (std::string sceneName)
   {
       LoadScene(sceneName);
      
       // Init the scene
       sofa::simulation::getSimulation()->init(root.get());

       // Test if root is not null
       if(!root)
       {  
           ADD_FAILURE() << "Error in init for the scene: " << sceneName << std::endl;
           return false;   
       }

       return true;

   }

    /// After simulation compare the positions of points to the theoretical positions.
    bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition)
    {
        // Init simulation
        sofa::simulation::getSimulation()->init(root.get());

        // Compute the theoretical final positions
        VecCoord finalPos;
        patchStruct.affineConstraint->getFinalPositions( finalPos,*patchStruct.dofs->write(core::VecCoordId::position()) );


        // Initialize
        size_t numNodes = finalPos.size();
        VecCoord xprev(numNodes);
        VecDeriv dx(numNodes);
        bool hasConverged = true;

        for (size_t i=0; i<numNodes; i++)
        {
            xprev[i] = CPos(0,0,0);
        }

        // Animate
        do
        {
            hasConverged = true;
            sofa::simulation::getSimulation()->animate(root.get(),0.5);
            typename MechanicalObject::ReadVecCoord x = patchStruct.dofs->readPositions();

            // Compute dx
            for (size_t i=0; i<x.size(); i++)
            {
                dx[i] = x[i]-xprev[i];
                // Test convergence
                if(dx[i].norm()>convergenceAccuracy) hasConverged = false;
            }

            // xprev = x
            for (size_t i=0; i<numNodes; i++)
            {
                xprev[i]=x[i];
            }
        }
        while(!hasConverged); // not converged

        // Get simulated positions
        typename MechanicalObject::WriteVecCoord x = patchStruct.dofs->writePositions();

        // Compare the theoretical positions and the simulated positions
        bool succeed=true;
        for(size_t i=0; i<finalPos.size(); i++ )
        {
            if((finalPos[i]-x[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition)
            {
                succeed = false;
                ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl
                    << "difference = " <<(finalPos[i]-x[i]).norm() << std::endl <<"rotation = " << testedRotation << std::endl << " translation = " << testedTranslation << std::endl
                    << "Failed seed number =" << BaseSofa_test::seed;

            }
        }
        return succeed;
    }

	bool testBezierTetrahedronTopology()
	{
		// Init simulation
		sofa::simulation::getSimulation()->init(root.get());
		BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown);
		size_t nTetras,elem;
		BezierDegreeType degree=container->getDegree();
		// check the total number of vertices.
		size_t nbPoints=container->getNumberOfTetrahedralPoints()+container->getNumberOfEdges()*(degree-1)+container->getNumberOfTriangles()*(degree-1)*(degree-2)/2+container->getNumberOfTetrahedra()*((degree-1)*(degree-2)*(degree-3)/6);
        if((size_t)container->getNbPoints()!=nbPoints) {
			ADD_FAILURE() << "wrong number of points " <<container->getNbPoints() << " is wrong. It should be  " <<nbPoints  << std::endl;
			return false;
		}

		sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray();
		
		BezierTetrahedronPointLocation location; 
		size_t elementIndex, elementOffset/*,localIndex*/;
		for (nTetras=0;nTetras<container->getNumberOfTetrahedra();++nTetras) {
			
			const BezierTetrahedronSetTopologyContainer::VecPointID &indexArray=container->getGlobalIndexArrayOfBezierPoints(nTetras);
			// check the number of control points per tetrahedron is correct
			nbPoints=(4+6*(degree-1)+2*(degree-1)*(degree-2)+(degree-1)*(degree-2)*(degree-3)/6);
			if(indexArray.size()!=nbPoints) {
				ADD_FAILURE() << "wrong number of control points in tetrahedron " <<nTetras<< ". It is "<<indexArray.size() <<" and should be "<<nbPoints  << std::endl;
				return false;
			}
			for(elem=0;elem<indexArray.size();++elem) {
				size_t globalIndex=container->getGlobalIndexOfBezierPoint(nTetras,tbiArray[elem]);
				// check that getGlobalIndexOfBezierPoint and getGlobalIndexArrayOfBezierPointsInTetrahedron give the same answer
				if(globalIndex!=indexArray[elem]) {
					ADD_FAILURE() << "wrong global index given by  getGlobalIndexOfBezierPoint(). It is : "<<globalIndex <<" and should be "<<indexArray[elem]  << std::endl;
					return false;
				}
				TetrahedronBezierIndex tbi=container->getTetrahedronBezierIndex(elem);
				if(elem!=container->getLocalIndexFromTetrahedronBezierIndex(tbi)) {
					ADD_FAILURE() << "wrong local index given by  getLocalIndexFromTetrahedronBezierIndex(). It is : "<<container->getLocalIndexFromTetrahedronBezierIndex(tbi) <<" and should be "<<elem  << std::endl;
					return false;
				}
				// check that getTetrahedronBezierIndex is consistant with getTetrahedronBezierIndexArray
				if ((tbiArray[elem][0]!=tbi[0]) || (tbiArray[elem][1]!=tbi[1]) || (tbiArray[elem][2]!=tbi[2]) || (tbiArray[elem][3]!=tbi[3])) {
					ADD_FAILURE() << "non consistent indices between getTetrahedronBezierIndexArray() and getTetrahedronBezierIndex(). Got  : "<<tbiArray[elem] <<" versus  "<<tbi  << std::endl;
					return false;
				}
				// check that getLocationFromGlobalIndex is consistent with 
				container->getLocationFromGlobalIndex(globalIndex,location,elementIndex,elementOffset);
				if (elem<4) {
					if ((location!=BezierTetrahedronSetTopologyContainer::POINT) || (elementIndex!=container->getTetrahedron(nTetras)[elem]) || (elementOffset!=0)) {
						ADD_FAILURE() << "non consistent indices given by  getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl;
						return false;
					}
				}
				else if (elem<(size_t)(4+6*(degree-1))){
					if ((location!=BezierTetrahedronSetTopologyContainer::EDGE) || (elementIndex!=container->getEdgesInTetrahedron(nTetras)[(elem-4)/(degree-1)])) {
						ADD_FAILURE() << "non consistent indices given by  getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl;
						return false;
					}

				}
				else if (elem<(size_t)(4+6*(degree-1)+2*(degree-1)*(degree-2))){
					size_t nbPointPerEdge=(degree-1)*(degree-2)/2;
					size_t val=(elem-4-6*(degree-1))/(nbPointPerEdge);
					if ((location!=BezierTetrahedronSetTopologyContainer::TRIANGLE) || (elementIndex!=container->getTrianglesInTetrahedron(nTetras)[val])) {
						ADD_FAILURE() << "non consistent indices given by  getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl;
						return false;
					}
				}
			}

		}
		return( true);
	}