/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_CORE_COMPONENTMODEL_COLLISION_LINEARSOLVERCONTACTCORRECTION_INL
#define SOFA_CORE_COMPONENTMODEL_COLLISION_LINEARSOLVERCONTACTCORRECTION_INL

#include "LinearSolverConstraintCorrection.h"
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/simulation/common/Node.h>
#include <sofa/component/mass/UniformMass.h>
#include <sofa/simulation/common/MechanicalVisitor.h>

//compliance computation include
#include <sofa/component/odesolver/CGImplicitSolver.h>
//#include <glib.h>
#include <sstream>
#include <list>

namespace sofa
{

namespace component
{

namespace constraint
{
#define MAX_NUM_CONSTRAINT_PER_NODE 100
#define EPS_UNITARY_FORCE 0.01

using namespace sofa::component::odesolver;

template<class DataTypes>
LinearSolverConstraintCorrection<DataTypes>::LinearSolverConstraintCorrection(behavior::MechanicalState<DataTypes> *mm)
: wire_optimization(initData(&wire_optimization, false, "wire_optimization", "constraints are reordered along a wire-like topology (from tip to base)"))
,mstate(mm), odesolver(NULL), linearsolver(NULL)
{
}

template<class DataTypes>
LinearSolverConstraintCorrection<DataTypes>::~LinearSolverConstraintCorrection()
{
}



 //////////////////////////////////////////////////////////////////////////
 //   Precomputation of the Constraint Correction for all type of data
 //////////////////////////////////////////////////////////////////////////

template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::init()
{
    mstate = dynamic_cast< behavior::MechanicalState<DataTypes>* >(getContext()->getMechanicalState());
    objectmodel::BaseContext* c = this->getContext();
//     odesolver = c->get< behavior::OdeSolver >();
//     linearsolver = c->get< behavior::LinearSolver >();
        odesolver=getOdeSolver(c);
        linearsolver=getLinearSolver(c);
    if (odesolver == NULL)
    {
        serr << "LinearSolverConstraintCorrection: ERROR no OdeSolver found."<<sendl;
        return;
    }
    if (linearsolver == NULL)
    {
        serr << "LinearSolverConstraintCorrection: ERROR no LinearSolver found."<<sendl;
        return;
    }

    int n = mstate->getSize()*Deriv::size();

    std::stringstream ss;

    ss << this->getContext()->getName() << ".comp";

    std::ifstream compFileIn(ss.str().c_str(), std::ifstream::binary);

    sout << "try to open : " << ss.str() << endl;

    if(compFileIn.good())
    {
        sout << "file open : " << ss.str() << " compliance being loaded" << endl;
        refMinv.resize(n,n);
        //complianceLoaded = true;
        compFileIn.read((char*)refMinv.ptr(), n*n*sizeof(double));
        compFileIn.close();
    }
}

template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::getCompliance(defaulttype::BaseMatrix* W)
{
    if (!mstate || !odesolver || !linearsolver) return;

    // use the OdeSolver to get the position integration factor
    //const double factor = 1.0;
    //const double factor = odesolver->getPositionIntegrationFactor(); // dt
    const double factor = odesolver->getPositionIntegrationFactor(); //*odesolver->getPositionIntegrationFactor(); // dt*dt

    const unsigned int numDOFs = mstate->getSize();
    const unsigned int N = Deriv::size();
    const unsigned int numDOFReals = numDOFs*N;
    if (refMinv.rowSize() > 0)			// What's for ?? 
    {
		std::cout<<"refMinv.rowSize() > 0"<<std::endl;
        J.resize(numDOFReals,numDOFReals);
        for (unsigned int i=0;i<numDOFReals;++i)
            J.set(i,i,1);
        linearsolver::FullMatrix<Real> Minv;
        Minv.resize(numDOFReals,numDOFReals);
        // use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
        linearsolver->addJMInvJt(&Minv, &J, factor);
        double err=0,fact=0;
        for (unsigned int i=0;i<numDOFReals;++i)
            for (unsigned int j=0;j<numDOFReals;++j)
            {
                //sout << "Minv("<<i<<","<<j<<") = "<<Minv.element(i,j)<<"\t refMinv("<<i<<","<<j<<") = "<<refMinv.element(i,j)<<sendl;
                if (fabs(refMinv.element(i,j)) > 1.0e-30)
                {
                    err += fabs(Minv.element(i,j)-refMinv.element(i,j))/refMinv.element(i,j);
                    fact += fabs(Minv.element(i,j)/refMinv.element(i,j));
                }
                else
                {
                    err += fabs(Minv.element(i,j)-refMinv.element(i,j));
                    fact += 1.0f;
                }
            }
        sout << "LinearSolverConstraintCorrection: mean relative error: "<<err/(numDOFReals*numDOFReals)<<sendl;
        sout << "LinearSolverConstraintCorrection: mean relative factor: "<<fact/(numDOFReals*numDOFReals)<<sendl;
        refMinv.resize(0,0);
    }
    // Compute J
    VecConst& constraints = *mstate->getC();
    const unsigned int numConstraints = constraints.size();
    const unsigned int totalNumConstraints = W->rowSize();

    J.resize(totalNumConstraints, numDOFReals);
    for(unsigned int c1 = 0; c1 < numConstraints; c1++)
    {
        int cid = mstate->getConstraintId()[c1];
        ConstraintIterator itConstraint;
        for (itConstraint=constraints[c1].getData().begin();itConstraint!=constraints[c1].getData().end();itConstraint++)
          {
            unsigned int dof = itConstraint->first;
            Deriv n = itConstraint->second;
            for (unsigned int r=0;r<N;++r)
                J.add(cid, dof*N+r, n[r]);
        }
    }

    // use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
    linearsolver->addJMInvJt(W, &J, factor);
}

template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::applyContactForce(const defaulttype::BaseVector *f)
{
    behavior::BaseMechanicalState::VecId forceID(behavior::BaseMechanicalState::VecId::V_DERIV, behavior::BaseMechanicalState::VecId::V_FIRST_DYNAMIC_INDEX);
    behavior::BaseMechanicalState::VecId dxID(behavior::BaseMechanicalState::VecId::dx()); //behavior::BaseMechanicalState::VecId::V_DERIV, behavior::BaseMechanicalState::VecId::V_FIRST_DYNAMIC_INDEX+1);
    mstate->vAlloc(forceID);
    mstate->vOp(forceID);
//    mstate->vAlloc(dxID);
    mstate->setDx(forceID);
    VecDeriv& force = *mstate->getDx();
    mstate->setDx(dxID);
    VecDeriv& dx = *mstate->getDx();
    mstate->setDx(behavior::BaseMechanicalState::VecId::dx());
        //VecConst& constraints = *mstate->getC();
        //unsigned int numConstraints = constraints.size();
        VecCoord& x = *mstate->getX();
        VecDeriv& v = *mstate->getV();
        VecDeriv v_free = *mstate->getVfree();
        VecCoord x_free = *mstate->getXfree();
        //double dt = this->getContext()->getDt();

    const unsigned int numDOFs = mstate->getSize();

        dx.clear();
        dx.resize(numDOFs);
    for (unsigned int i=0; i< numDOFs; i++)
        dx[i] = Deriv();

        force.clear();
        force.resize(numDOFs);
    for (unsigned int i=0; i< numDOFs; i++)
        force[i] = Deriv();
#if 0
    const unsigned int N = Deriv::size();
    const unsigned int numDOFReals = numDOFs*N;
    F.resize(numDOFReals);
    const linearsolver::FullVector<Real>* fcast = dynamic_cast< const linearsolver::FullVector<Real>* >(f);
    if (fcast)
        J.mulTranspose(F, *fcast); // fast
    else
        J.mulTranspose(F, fcast); // slow but generic
    for (unsigned int i=0; i< numDOFs; i++)
        for (unsigned int r=0;r<N;++r)
            force[i][r] = F[i*N+r];
#else
    const VecConst& constraints = *mstate->getC();
    unsigned int numConstraints = constraints.size();

    for(unsigned int c1 = 0; c1 < numConstraints; c1++)
    {
        int indexC1 = mstate->getConstraintId()[c1];
        double fC1 = f->element(indexC1);
        //sout << "fC("<<indexC1<<")="<<fC1<<sendl;
        if (fC1 != 0.0)
        {
          ConstraintIterator itConstraint;
          for (itConstraint=constraints[c1].getData().begin();itConstraint!=constraints[c1].getData().end();itConstraint++)
            {
              unsigned int dof = itConstraint->first;
              Deriv n = itConstraint->second;

                //sout << "f("<<constraints[c1][i].index<<") += "<< (constraints[c1][i].data * fC1) << sendl;
                force[dof] += n * fC1;
            }
        }
    }
#endif
    //for (unsigned int i=0; i< numDOFs; i++)
    //    sout << "f("<<i<<")="<<force[i]<<sendl;
    linearsolver->setSystemRHVector(forceID);
    linearsolver->setSystemLHVector(dxID);
    linearsolver->solveSystem(); 
	//TODO: tell the solver not to recompute the matrix

    // use the OdeSolver to get the position integration factor
    const double positionFactor = odesolver->getPositionIntegrationFactor();

    // use the OdeSolver to get the position integration factor
    const double velocityFactor = odesolver->getVelocityIntegrationFactor();

    for (unsigned int i=0; i< numDOFs; i++)
    {
        //sout << "dx("<<i<<")="<<dx[i]<<sendl;
        Deriv dxi = dx[i]*positionFactor;
        Deriv dvi = dx[i]*velocityFactor;
        x[i] = x_free[i] + dxi;
        v[i] = v_free[i] + dvi;
        dx[i] = dxi;
    }
    mstate->vFree(forceID);
}

template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::resetContactForce()
{
    VecDeriv& force = *mstate->getF();
    for( unsigned i=0; i<force.size(); ++i )
        force[i] = Deriv();
}

template<class DataTypes>
bool LinearSolverConstraintCorrection<DataTypes>::hasConstraintNumber(int index) 
{
  const VecConst& constraints = *mstate->getC();
  unsigned int numConstraints = constraints.size();
  
  for(unsigned int c = 0; c < numConstraints; c++)
  {
    int indexC = mstate->getConstraintId()[c];
	if (indexC == index)
		return true;
  }
  return false;
}


template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::verify_constraints()  
{
	VecConst& constraints = *mstate->getC();
	//unsigned int numConstraints = constraints.size();
	sofa::helper::vector<unsigned int>& Id_vec = mstate->getConstraintId();
	sofa::helper::vector<unsigned int>::iterator it_id(Id_vec.begin());	
	
	// debug
	/*
	bool change = false;
	
	for (unsigned int i=0; i<numConstraints; i++)
	 {
		int indexI =  mstate->getConstraintId()[i];
		for (unsigned int j=0; j<i; j++)
		{
			int indexJ =  mstate->getConstraintId()[j];
			
			if (indexI==indexJ)
			{
				change = true;
				//debug
				std::cout<<"!! duplicate detected !! i= "<<i<<"  j= "<<j<<"index commun"<<indexI<<std::endl;
			}
		}
	 }
	
	
	 if(change)
	 {
		std::cout<<"******* BEFORE******** \n constraints.size() ="<<constraints.size()<<"Id_vec.size()"<<Id_vec.size()<<std::endl;
		for (unsigned int j=0; j<Id_vec.size(); j++)
		{
			std::cout<<"Id_vec["<<j<<"] = "<<Id_vec[j]<<std::endl;
		}
	 }	
	*/

	
	VecConstIt it_constI, it_constJ;

	 for (it_constI=constraints.begin() ; it_constI!= constraints.end(); it_constI++ )
	 {
		unsigned int indexI= *it_id;
		
		int j=0;
		for (it_constJ=constraints.begin(); it_constJ!=it_constI; it_constJ++ )
		{
			unsigned int indexJ = Id_vec[j];
			
			if (indexI==indexJ)
			{
				
				//debug
				//std::cout<<"!! duplicate detected !!  j= "<<j<<"index commun"<<indexI<<std::endl;

				
				/// on copie les données de I dans J
				ConstraintIterator itConstraintI;
				//ConstraintIterator itConstraintJ;
				
				//std::cout<<" copie de I dans J"<<std::endl;
				for (itConstraintI=it_constI->getData().begin();itConstraintI!=it_constI->getData().end();itConstraintI++)
				{
					
					unsigned int dof = itConstraintI->first;
					Deriv n = itConstraintI->second;
					it_constJ->getData().insert(std::pair<unsigned int, Deriv> (dof,n ) );
					
				}
				//std::cout<<" suppression de I"<<std::endl;
				constraints.erase(it_constI);
				//std::cout<<" suppression de l'identifiant I"<<std::endl;
				Id_vec.erase(it_id); 				
				it_constI--;
				it_id--;
				break;
			}
			j++;

		}

		it_id++;
	 }
	 
	 /*
	 if(change)
	 {
		std::cout<<"******* AFTER ******** \n constraints.size() ="<<constraints.size()<<"Id_vec.size()"<<Id_vec.size()<<std::endl;
		for (unsigned int j=0; j<Id_vec.size(); j++)
		{
			std::cout<<"Id_vec["<<j<<"] = "<<Id_vec[j]<<std::endl;
		}
	 }
	 */
	 
}
	
template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::resetForUnbuiltResolution(double * f, std::list<int>& renumbering)  
{

	verify_constraints();
	
	VecConst& constraints = *mstate->getC();
	 unsigned int numConstraints = constraints.size();
	 

	constraint_disp.clear();
	constraint_disp.resize(mstate->getSize());
	
	constraint_force.clear();
	constraint_force.resize(mstate->getSize());
	
	constraint_dofs.clear();
	id_to_localIndex.clear();
	
	

////// TODO : supprimer le classement par indice max
	//std::vector<unsigned int> VecMaxDof;
	//VecMaxDof.resize(numConstraints);


	std::vector<unsigned int> VecMinDof;
	VecMinDof.resize(numConstraints);	
		
		
	int maxIndex = -1;
	for(unsigned int c = 0; c < numConstraints; c++)
	{
	

		I_last_Dforce.clear();
		
		int indexC = mstate->getConstraintId()[c];
		
		// resize table if necessary
		if (indexC > maxIndex){
			id_to_localIndex.resize(indexC+1, -1);   // debug : -1 value allows to know if the table is badly filled
			maxIndex = indexC;
		}
			
		if(id_to_localIndex[indexC]!=-1)
		{
			
			std::cerr<<" WARNING: id_to_localIndex["<<indexC<<"] has already a constraint : "<<id_to_localIndex[indexC]<<" concurrent constraint ="<<c<<std::endl;
			std::cerr<<" mstate->getConstraintId()["<<c<<"] = "<<mstate->getConstraintId()[c]<<std::endl;
			
		}	
		// buf the table of local indices
		id_to_localIndex[indexC] = c;
		
		
		// debug //
		//if (c==0)
		//	f[indexC]=1.0;
		
		// buf the value of force applied on concerned dof : constraint_force
		// buf a table of indice of involved dof : constraint_dofs
		double fC = f[indexC];
		// debug
		//std::cout<<"f["<<indexC<<"] = "<<fC<<std::endl;
		

			
		
		if (fC != 0.0)
		{
			ConstraintIterator itConstraint;
			for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)
			{
	
				unsigned int dof = itConstraint->first;
				Deriv n = itConstraint->second;
				constraint_force[dof] +=n * fC;
				I_last_Dforce.push_back(dof);

			}
		}
		
		
		//////////// for wire optimization ////////////
		ConstraintIterator itConstraint;
		//VecMaxDof[c] = 0;
		//for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)
		//{
		//	unsigned int dof = itConstraint->first;		
		//	constraint_dofs.push_back(dof);
		//	if(dof>VecMaxDof[c]) VecMaxDof[c]=dof;
		//}
		
		/// a vector of the minimal indice of dof involved with each constraint is built
		VecMinDof[c] = mstate->getSize()+1;
		for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)
		{
			unsigned int dof = itConstraint->first;	
			constraint_dofs.push_back(dof);	
			if(dof<VecMinDof[c]) VecMinDof[c]=dof;
				
		}
		
		// debug
		//std::cout<<VecMaxDof[c]<<" ";
	}


	
	
	if (wire_optimization.getValue())
	{
		
		std::vector< std::vector<int> > ordering_per_dof;
		ordering_per_dof.resize(mstate->getSize());
		for(unsigned int c = 0; c < numConstraints; c++)
		{
			int indexC = mstate->getConstraintId()[c];
			ordering_per_dof[VecMinDof[c]].push_back(indexC);
		}
		
		
		renumbering.clear();
		for(int dof= 0; dof<mstate->getSize(); dof++)
		{
			for (unsigned int c = 0; c< ordering_per_dof[dof].size(); c++)
			{
				renumbering.push_back(ordering_per_dof[dof][c]);
			}
			
		}
		
	}	
	
	// debug
	//std::cout<<"in resetConstraintForce : constraint_force ="<<constraint_force<<std::endl;
	
	// constraint_dofs buff the DOF that are involved with the constraints
	constraint_dofs.unique();
	

	I_last_Dforce.sort();
	I_last_Dforce.unique();
	
	// debug
	/*std::cout<<"in resetConstraintForce I_last_Dforce.size() = "<<I_last_Dforce.size()<<"value : "<<std::endl;
	std::list<int>::const_iterator lit(I_last_Dforce.begin()), lend(I_last_Dforce.end());
		for(;lit!=lend;++lit) 
		{
			int dof =*lit;
			std::cout<<dof<<" - ";
		}
		std::cout<<" "<<std::endl;
	*/
	
	/////////////// SET INFO FOR LINEAR SOLVER /////////////
	
	behavior::BaseMechanicalState::VecId forceID(behavior::BaseMechanicalState::VecId::V_DERIV, behavior::BaseMechanicalState::VecId::V_FIRST_DYNAMIC_INDEX);
    behavior::BaseMechanicalState::VecId dxID(behavior::BaseMechanicalState::VecId::dx()); //behavior::BaseMechanicalState::VecId::V_DERIV, behavior::BaseMechanicalState::VecId::V_FIRST_DYNAMIC_INDEX+1);

    linearsolver->setSystemRHVector(forceID);
    linearsolver->setSystemLHVector(dxID);
	
	systemMatrix_buf   = linearsolver->getSystemBaseMatrix();
    systemRHVector_buf = linearsolver->getSystemRHBaseVector();
	systemLHVector_buf = linearsolver->getSystemLHBaseVector();
	
	// systemRHVector_buf is set to constraint_force;
	//std::cerr<<"WARNING: resize is called"<<std::endl;	
    const unsigned int derivDim = Deriv::size();
	const unsigned int systemSize = mstate->getSize() * derivDim;
	systemRHVector_buf->resize(systemSize) ;
	systemLHVector_buf->resize(systemSize) ;
	//std::cerr<<"resize ok"<<std::endl;
	
 
	
    for ( int i=0; i<mstate->getSize(); i++)
	{
		for  (unsigned int j=0; j<derivDim; j++)
			systemRHVector_buf->set(i*derivDim+j, constraint_force[i][j]);
	} 
	
	
	// debug !!
	//double toto[12];
	//toto[0]=0.0;
	//addConstraintDisplacement(toto, 0,0) ;
	//std::cout<<"toto[0] ="<<toto[0]<<std::endl;

	
	///////// new : précalcul des liste d'indice ///////
	Vec_I_list_dof.clear(); // clear = the list is fill during the block compliance computation
	Vec_I_list_dof.resize(numConstraints);
	last_disp = 0;
	last_force= numConstraints-1;
	_new_force = true;
			

	


}
	
template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::addConstraintDisplacement(double *d, int begin,int end) 
{
	
	
	const VecConst& constraints = *mstate->getC();
	const unsigned int derivDim = Deriv::size();
	


	/*
	I_last_Disp.clear();
	for ( int id_=begin; id_<=end; id_++)
	{
		int c = id_to_localIndex[id_];
		ConstraintIterator itConstraint;
		for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)		
		{	
			int dof = (int) itConstraint->first;
			I_last_Disp.push_back(dof);		
		}
	}
	I_last_Disp.sort();
	I_last_Disp.unique();
	*/
	
	last_disp = begin;

	
	/*
	std::cout<<"in addConstraintDisplacement I_last_Disp.size() = "<<I_last_Disp.size()<<" value : "<<std::endl;
	std::list<int>::const_iterator lit(I_last_Disp.begin()), lend(I_last_Disp.end());
		for(;lit!=lend;++lit) 
		{
			int dof =*lit;
			std::cout<<dof<<" - ";
		}
		std::cout<<" "<<std::endl;
	*/
	//std::cout<<"constraint_force : "<<constraint_force<<std::endl;
	
	
	linearsolver->partial_solve(Vec_I_list_dof[last_disp], Vec_I_list_dof[last_force], _new_force);
	
	_new_force = false;
	

	
	for ( int id_=begin; id_<=end; id_++)
	{
		//std::cout<<"dfree["<<id_<<"] ="<<d[id_];
		int c = id_to_localIndex[id_];
		ConstraintIterator itConstraint;
		for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)		
		{	
			int dof = (int) itConstraint->first;
			Deriv n = itConstraint->second;
			Deriv Disp;
			
			for(unsigned int j=0; j<derivDim; j++){
			    Disp[j] = (Real)(systemLHVector_buf->element(dof*derivDim + j) * odesolver->getPositionIntegrationFactor());
			}
			d[id_] += n*Disp; 
			
			
		}
		//std::cout<<" - d["<<id_<<"] = "<<d[id_]<<std::endl;
	}

	
	
}
	
template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::setConstraintDForce(double *df, int begin, int end, bool update) 
{
	const VecConst& constraints = *mstate->getC();
	const unsigned int derivDim = Deriv::size();
	
	//std::cout<<" setConstraintDForce is called"<<std::endl;
	// debug
	//if (end<3)
	//	std::cout<<"addDf - df["<<begin<<" to "<<end<<"] ="<< df[begin] << " " << df[begin+1] << " "<< df[begin+2] << std::endl;
	
	
	if (update==false)
		return;
	
	_new_force = true;
	//debug
	Deriv DF_c;
	
	for ( int id_=begin; id_<=end; id_++)
	{
		int c = id_to_localIndex[id_];
		
		// debug
		//std::cout<<" - setConstraintDForce : constraint"<< c <<std::endl;
		
		
		ConstraintIterator itConstraint;
		for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)		
		{	
			Deriv n = itConstraint->second;
			int dof = (int) itConstraint->first;
			//debug
			//std::cout<<"constraint_force["<<dof<<"] = "<<constraint_force[dof] ;
			
			constraint_force[dof] += n * df[id_];
			//I_last_Dforce.push_back(dof);
			//std::cout<<"     after ... constraint_force["<<dof<<"] = "<<constraint_force[dof] ;
			
			DF_c +=  n * df[id_]; 
									
		}
		
		//std::cout<<std::endl;
		
	}
	
	/*
	if (df[begin]<0)
	{
		std::cout<<" DF_c : "<< DF_c<< std::endl;
	}
	*/
	
	last_force = begin;
	//debug
	
	

	std::list<int>::const_iterator it_dof(Vec_I_list_dof[last_force].begin()), it_end(Vec_I_list_dof[last_force].end());
	for(;it_dof!=it_end;++it_dof)
	{
	    int dof =(*it_dof) ;
		//std::cout<<"dof -  "<<dof <<std::endl;
		for  (unsigned int j=0; j<derivDim; j++)
			systemRHVector_buf->set(dof * derivDim + j, constraint_force[dof][j]);
	}
	

	
	
	

}

template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(defaulttype::BaseMatrix* W, int begin, int end) 
{
    if (!mstate || !odesolver || !linearsolver) return;

    // use the OdeSolver to get the position integration factor
    const double factor = odesolver->getPositionIntegrationFactor(); //*odesolver->getPositionIntegrationFactor(); // dt*dt

    const unsigned int numDOFs = mstate->getSize();
    const unsigned int N = Deriv::size();
    const unsigned int numDOFReals = numDOFs*N;
	
    // Compute J
    VecConst& constraints = *mstate->getC();
  //  const unsigned int numConstraints = constraints.size();
    const unsigned int totalNumConstraints = W->rowSize();

    J.resize(totalNumConstraints, numDOFReals);
	


	for (int id1=begin; id1<=end; id1++)
	{
		//std::cerr<<"constraint : "<<id1;
		int c1 = id_to_localIndex[id1];
		//std::cerr<<" local index : "<<c1<<std::endl;
		
		ConstraintIterator itConstraint1;
		unsigned int dof_buf=0;
		int toto=0;
		for (itConstraint1=constraints[c1].getData().begin();itConstraint1!=constraints[c1].getData().end();itConstraint1++)		
		{	
			
			unsigned int dof = itConstraint1->first;
            Deriv n = itConstraint1->second;
            for (unsigned int r=0;r<N;++r)
                J.add(id1, dof*N+r, n[r]);	
			
			if (toto!=0)
			{
				int test=dof_buf - dof;
				if (test>2 || test< -2)
					std::cout<<"YES !!!! for constraint id1 dof1 = "<<dof_buf<<" dof2 = "<<dof<<std::endl;
			}
			dof_buf =dof;
				
		}
	}
	
	// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
    linearsolver->addJMInvJt(W, &J, factor);
	
	
	
	// construction of  Vec_I_list_dof : vector containing, for each constraint block, the list of dof concerned
	
	ListIndex list_dof;	
	for ( int id_=begin; id_<=end; id_++)
	{
		int c = id_to_localIndex[id_];		
		ConstraintIterator itConstraint;
		for (itConstraint=constraints[c].getData().begin();itConstraint!=constraints[c].getData().end();itConstraint++)		
		{	
			int dof = (int) itConstraint->first;
			list_dof.push_back(dof);		
		}
	}
	list_dof.sort();
	list_dof.unique();	
	for ( int id_=begin; id_<=end; id_++)
	{
		Vec_I_list_dof[id_] = list_dof;	
	}
	

	

	
}


} // namespace collision

} // namespace component

} // namespace sofa

#endif