#include "env_map.h"
#include "MovingObject.h"
#include "segment.h"
using namespace CGoGN ;


EnvMap::EnvMap()
{



#ifndef SPATIAL_HASHING

    //	RegisteredEdges = map.addAttribute<PFP::SegmentVECT, FACE>("Segments") ;
    //	neighborRegisteredEdges = map.addAttribute<PFP::SegmentVECT, FACE>("neighborSegments") ;
    //	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB, FACE>("subdivisableFace") ;

    //	refineCandidate.reserve(100) ;
    //	coarsenCandidate.reserve(100) ;
#endif
}

void EnvMap::init(int argc, char **argv)
{
    maxCellRay = 2.0f;
    std::cout << "Init EnvMap" << std::endl ;
    if (argc>2)
    {
        std::string filename(argv[2]);
        open_file(filename);
    }
    else
    {
        //Intialisation map
#ifdef IHMap
        map.initImplicitProperties();  // Si map MR
#endif


        ///// initialisation attributs
        position = map.addAttribute<VEC3, VERTEX, MAP>("position");
        facecenter = map.addAttribute<VEC3, FACE, MAP>("facecenter");
        color = map.addAttribute<VEC3, VOLUME, MAP>("color");
        RegisteredEdges=map.addAttribute<ARETES,VOLUME, MAP>("RegisteredEdges");
        RegisteredNeighborEdges=map.addAttribute<ARETES,VOLUME, MAP>("RegisteredNeighborEdges");



        int nb = 1;
        Algo::Volume::Tilings::Cubic::Grid<PFP> cubic(map, nb, nb, nb);
        cubic.embedIntoGrid(position, 1.0f, 1.0f, 1.0f);

        //        subdivideVolume(map.begin());


        //        Algo::Surface::Modelisation::createTetrahedron<PFP>(map,true);
        //        Algo::Surface::Modelisation::embedPyramid<PFP>(map,position,3,true,1.0f,1.0f);
        //        Algo::Volume::Modelisation::sqrt3Vol<PFP>(map,position);

        map.check();
        //        Dart d1=map.indexDart(0);
        //        Dart d2=map.indexDart(1);
        //        Dart d3=map.indexDart(8);
        //        Geom::Plane3D<PFP::REAL> pl (position[d1],position[d2],position[d3]);
        //        Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,d2,pl);
        //        map.check();


        //        VEC3 center = Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,map.indexDart(0),position);
        //        VEC3 norm = Algo::Surface::Geometry::faceNormal<PFP, VertexAttribute<VEC3, MAP>>(map,map.indexDart(0),position);
        //        VEC3 norm2 = Algo::Surface::Geometry::faceNormal<PFP, VertexAttribute<VEC3, MAP>>(map,map.indexDart(4),position);
        //        VEC3 norm3 = Algo::Surface::Geometry::faceNormal<PFP, VertexAttribute<VEC3, MAP>>(map,map.indexDart(16),position);
        //        Geom::Plane3D<PFP::REAL> pl (norm,center);
        //        Geom::Plane3D<PFP::REAL> pl2 (norm2,center);
        //        Geom::Plane3D<PFP::REAL> pl3 (norm3,center);
        //        Dart newd = Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,map.indexDart(0),pl);
        //        map.check();
        //        Dart newd2 =Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,map.indexDart(0),pl2);
        //        Dart newd3 =Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,newd,pl2);
        //        map.check();
        //        Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,map.indexDart(0),pl3);
        //        Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,newd,pl3);
        //        Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,map.indexDart(3),pl3);
        //        Algo::Volume::Modelisation::sliceConvexVolume<PFP>(map,position,map.indexDart(15),pl3);
        //        map.check();
        //        CGoGNout<<" darts : 0 , "<<newd<<" , "<<newd2<<" , "<<newd3<<CGoGNendl;



        TraversorV<MAP> tv(map);
        for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
        {
            position[d] *= 10;
        }
        TraversorF<MAP> tf(map);
        for(Dart d = tf.begin() ; d != tf.end() ; d = tf.next())
        {

            facecenter[d]=Algo::Surface::Geometry::faceCentroid<PFP, VertexAttribute<VEC3, MAP>>(map,d,position);

        }

        TraversorW<MAP> tra(map);
        for (Dart d = tra.begin(); d != tra.end(); d = tra.next())
        {
            color[d] = position[d]/10 + VEC3(0.5,0.5,0.5);
        }




        //        //// affichage positions
        //        CellMarkerStore<MAP, VOLUME> m(map) ;
        //        for (Dart d = map.begin(); d != map.end(); map.next(d))
        //        {
        //            if (!m.isMarked(d))
        //            {
        //                m.mark(d) ;
        //               CGoGNout<<"Dart "<<d.index<<" en : "<<position[d]<<CGoGNendl;
        //            }
        //        }



        /////////////////////////// Subdivision
        //		std::cout << "level :"<<map.getCurrentLevel() << std::endl ;
        //		Dart d =Algo::Volume::IHM::subdivideVolumeClassic<PFP>(map,map.phi2(map.begin()), position);
        //		std::cout << "centralDart : "<< d.index << std::endl ;
        //		map.setCurrentLevel(1);
        //		std::cout << "level :"<<map.getCurrentLevel() << std::endl ;


    }

}

REAL EnvMap::volumeMaxdistance(Vol volume)
{
    Dart d = volume;

    VEC3 center = Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,volume,position);

    REAL res = (position[d]-center).norm2();
    for ( Vertex vert : verticesIncidentToVolume3(map,volume))
    {
        REAL size=(position[vert]-center).norm2();
        if(size>res)
        {
            res=size;
        }
    }
    return res;
}

VEC3 EnvMap::normaleFromVolume(Dart volume,Dart face) // renvoie la normale a la face pointant vers l'extérieur du volume
{
    VEC3 center = Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,volume,position);
    VEC3 norm = Algo::Surface::Geometry::faceNormal<PFP, VertexAttribute<VEC3, MAP>>(map,face,position);
    VEC3 temp =center-facecenter[face];
    if(norm*temp>0) norm*=-1;
    return norm;
}

bool EnvMap::checkPointInMap(VEC3 pos,Dart neighborhood) // definit si le point pos est dans le voisinage interne a la carte du volume "neighborhood"
{
    if(insideVolume(pos, neighborhood))
        return true;

    for (Vol d : volumesAdjacentByVertex3(map,neighborhood))
    {
        if (!map.isBoundaryMarked<3>(d) )
        {
            if(insideVolume(pos, d))
                return true;
        }
    }


    return false;
}

bool EnvMap::insideVolume(VEC3 pos, Dart volume) // définit si le point pos est dans le volume convexe
{


    for(Face d : facesIncidentToVolume3(map,volume))
    {
        VEC3 normal = normaleFromVolume(volume,d);
        VEC3 center = facecenter[d];
        VEC3 dirPoint = pos-center;

        if(normal*dirPoint>0)
            return false;
    }


    return true;


}








///// a coder

Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
#ifdef IHMap
    assert(map.getCurrentLevel() == map.getMaxLevel());
#endif

    CellMarkerStore<MAP, VOLUME> m(map) ;
    for (Dart d = map.begin(); d != map.end(); map.next(d))
    {
        if (!m.isMarked(d))
        {
            m.mark(d) ;
            if (insideVolume(pos, d))
                return d ;
        }
    }

    std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
    return map.begin() ;
}









//
void EnvMap::open_file(std::string filename)
{
    map.clear(true);
    std::vector<std::string> attrNames ;

    size_t pos = filename.rfind(".");    // position of "." in filename
    std::string extension = filename.substr(pos);

    if(extension == std::string(".off"))
    {
        if(!Algo::Volume::Import::importMeshToExtrude<PFP>(map, filename, attrNames))
        {
            std::cerr << "could not import " << filename << std::endl ;

        }
        else
        {
            position = map.getAttribute<VEC3, VERTEX, MAP>(attrNames[0]) ;
            map.closeMap();
        }
    }
    else
    {


        if(extension == std::string(".map"))
        {
            map.loadMapBin(filename);
            position = map.getAttribute<VEC3, VERTEX, MAP>("position") ;
        }
        else
        {
            //			if(!Algo::Volume::Import::importMesh<PFP>(map, filename, attrNames))
            //			{
            //				std::cerr << "could not import " << filename << std::endl ;
            //				return ;
            //			}
            //			else
            //                position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
        }

        color = map.addAttribute<VEC3, VOLUME, MAP>("colorVol");

        TraversorCell<MAP, VOLUME> tra(map);
        float maxV = 0.0f;
        for (Dart d = tra.begin(); d != tra.end(); d = tra.next())
        {
            float v = Algo::Geometry::tetrahedronVolume<PFP>(map, d, position);
            color[d] = VEC3(v,0,0);
            if (v>maxV)
                maxV=v;

            //			if(envMap.map.isVolumeIncidentToBoundary(d))
            //				color[d] = VEC3(0,0,0);
            //			else
            color[d] = VEC3(v,0,0);
        }

        for (unsigned int i = color.begin(); i != color.end(); color.next(i))
        {
            color[i][0] /= maxV;
            color[i][2] = 1.0f - color[i][0];
        }
    }
}


//void EnvMap::subdivideAllToMaxLevel()
//{
//	bool subdiv ;
//	do
//	{
//		subdiv = false ;
//		{
//			CellMarker<FACE> subd(map) ;
//			for (Dart d = map.begin(); d != map.end(); map.next(d))
//			{
//				if (!subd.isMarked(d))
//				{
//					subd.mark(d) ;
//					if (!buildingMark.isMarked(d))
//					{
//						//check if subdivision is authorized
//						float minDistSq = Agent::neighborDistSq_ ;
//						bool subdivisable = true ;
//						Dart old = map.faceOldestDart(d) ;
//						unsigned int fLevel = map.faceLevel(old) ;
//						map.setCurrentLevel(fLevel) ;
//						PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
//						Dart fd = old ;
//						do
//						{
//							PFP::VEC3& p = position[fd] ;
//							PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd,
//							                                                      position) ;
//							PFP::VEC3 proj = fCenter
//							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
//							if (proj.norm2() < minDistSq)
//							{
//								subdivisable = false ;
//								break ;
//							}
//							fd = map.phi1(fd) ;
//						} while (fd != old) ;
//
//						if (subdivisable)
//						{
//							map.setCurrentLevel(fLevel) ;
//							Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
//							subdiv = true ;
//						}
//						map.setCurrentLevel(map.getMaxLevel()) ;
//					}
//				}
//			}
//		}
//	} while (subdiv) ;
//}
//
//void EnvMap::subdivideToProperLevel()
//{
//	bool subdiv ;
//	do
//	{
//		subdiv = false ;
//		{
//			CellMarker<FACE> subd(map) ;
//			for (Dart d = map.begin(); d != map.end(); map.next(d))
//			{
//				if (!subd.isMarked(d))
//				{
//					subd.mark(d) ;
//					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
//					{
//						std::pair<bool, bool>& sf = subdivisableFace[d] ;
//						if (sf.first == false || (sf.first == true && sf.second))
//						{
//							subdiv = true ;
//							refineMark.mark(d) ;
//							refineCandidate.push_back(d) ;
//						}
//					}
//				}
//			}
//			subd.unmarkAll() ;
//		}
//		updateMap() ;
//		refineCandidate.clear() ;
//		map.setCurrentLevel(map.getMaxLevel()) ;
//	} while (subdiv) ;
//}

//
//
void EnvMap::FirstRegistrationSegment(Segment * o)// réenregistre l'Segment en question
{

    ArticulatedObject * mo = o->nid;
    if (mo != NULL)
    {
        int n1 = o->indexPart1;
        int n2 = o->indexPart2;
#ifdef DEBUG_affichage
        CGoGNout<<"First Registration arete "<<o->index<<CGoGNendl;
#endif
        VEC3 p1 = o->p1;
        VEC3 p2 =  o->p2;

        Dart d1=mo->parts_[n1]->d;
        Dart d2=mo->parts_[n2]->d;
        std::vector<Dart> memo;


        memo = getMemoCross(p1,p2,d1);

        if(map.sameVolume(d1,d2))
        {
            pushAOneCellSegment(o , d1);
        }
        else
        {

            pushSegmentInSetOfCells(o, memo);
        }
    }
}

Dart EnvMap::popAndPushSegment(Segment* o)// maj de l'enregistrement
{
#ifdef DEBUG_affichage
    CGoGNout<<"pop&push arete "<<o->index<<CGoGNendl;
#endif
    popSegment(o);
    FirstRegistrationSegment(o);

    return o->nid->parts_[o->indexPart1]->d;;
}

void EnvMap::pushAOneCellSegment(Segment * o, Dart d)
{
    int n = o->index;
    ArticulatedObject * mo = o->nid;
#ifdef DEBUG_affichage
    CGoGNout<<"Push in one cell segment arete "<<n<<CGoGNendl;
#endif
#ifdef IHMap
    assert(map.getCurrentLevel() == map.getMaxLevel());
#endif

    addElementToVector<Segment*>(RegisteredEdges[d],o);

    mo->belonging_cells[n].push_back(d);
    mo->addGeneralCell (d);
    for (Vol volume : volumesAdjacentByVertex3(map,d))
    {
        if (!map.isBoundaryMarked<3>(volume))
        {
            pushSegmentInCellAsNeighbor(o, volume);
            mo->neighbor_cells[n].push_back(volume);
        }
    }

#ifdef DEBUG_affichage
    CGoGNout<<"fin du push , etat du belong/neighbor : "<<mo->belonging_cells[n].size()<<"/"<<mo->neighbor_cells[n].size()<<CGoGNendl;
    //    if(mo->neighbor_cells[n].size()>26)
    //    {
    //        for(auto d : mo->neighbor_cells[n])
    //        {
    //            CGoGNout<<" || "<<d<<" || "<<CGoGNendl;
    //        }
    //    }

#endif
}

void EnvMap::pushSegmentInSetOfCells(Segment* o, const std::vector<Dart>& memo_cross)
{
    int n = o->index;
#ifdef DEBUG_affichage
    CGoGNout<<"Push in set of cells segment arete "<<n<<CGoGNendl;
#endif
    //    assert( memo_cross.size() > 1);
    ArticulatedObject * mo = o->nid;

    mo->belonging_cells[n] = memo_cross;
    for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
    {

        addElementToVector<Segment*>(RegisteredEdges[*it],o);
        mo->addGeneralCell (*it);
#ifdef DEBUG_affichage
        CGoGNout<<"cellule du belong :"<<(*it)<<CGoGNendl;
#endif

    }
    FindNeighborCellsOfSegment(mo->belonging_cells[n], &(mo->neighbor_cells[n]));

    for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
    {
        if (!map.isBoundaryMarked<3>(*it))
        {
            addElementToVector<Segment*>(RegisteredNeighborEdges[*it],o);
        }
    }
#ifdef DEBUG_affichage
    CGoGNout<<"fin du push , etat du belong/neighbor : "<<mo->belonging_cells[n].size()<<"/"<<mo->neighbor_cells[n].size()<<CGoGNendl;
    //    if(mo->neighbor_cells[n].size()>26)
    //    {
    //        for(auto d : mo->neighbor_cells[n])
    //        {
    //            CGoGNout<<" || "<<d<<" || "<<CGoGNendl;
    //        }
    //    }

#endif
}

void EnvMap::popSegment(Segment* o)
{
    ArticulatedObject * mo = o->nid;
    int n = o->index;
#ifdef IHMap
    assert(map.getCurrentLevel() == map.getMaxLevel());
#endif

    if (mo != NULL)
    {
        for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
        {
            removeElementFromVector<Segment*>(RegisteredEdges[*it], o) ;
            mo->removeGeneralCell (*it);
        }
        for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
        {
            if (!map.isBoundaryMarked<3>(*it))
            {
                removeElementFromVector<Segment*>(RegisteredNeighborEdges[*it], o) ;
            }
        }
        mo->belonging_cells[n].clear();
        mo->neighbor_cells[n].clear();
    }
}


bool EnvMap::subdivideVolume(Dart dglobal)
{
    bool res =false;
#ifdef IHMap


    map.setCurrentLevel(map.getMaxLevel()) ;
    unsigned int vLevel = map.volumeLevel(dglobal);
    Dart old = map.volumeOldestDart(dglobal);
#ifdef DEBUG_affichage
    int count = 0;
#endif
    // la subdivision ne doit pas mettre plus d'un level de différence entre des cellules adjacentes
    // on commence donc par subdiviser les voisins qui ont trop de différence
//    Traversor3WF<typename PFP::MAP> traF(map, old, true);
//    for(Dart dit = traF.begin(); dit != traF.end(); dit = traF.next())
//    {
    for ( Face dit : facesIncidentToVolume3(map,old))
    {
#ifdef DEBUG_affichage
        count++;
#endif
        Dart nv = map.phi3(dit);
        if(!map.isBoundaryMarked(3, nv))
        {
            if(map.volumeLevel(nv) == vLevel - 1)
            {
#ifdef DEBUG_affichage
                std::cout << "OK pour cascade en : "<<nv<< std::endl;
#endif
                int currentLevel = map.getCurrentLevel();
                subdivideVolume(nv);
                map.setCurrentLevel(currentLevel);
            }
        }
    }



    // on commence ensuite a subdiviser
//    map.check();
    map.setCurrentLevel(map.getMaxLevel()) ;

#ifdef DEBUG_affichage
     CGoGNout<<"debut refine"<<CGoGNendl;
    std::cout << "dglobal = " << dglobal << "level = "<< map.getDartLevel(dglobal) << " nombre de faces verifiées : "<<count<< std::endl;
#endif

    ARETES oldobst(RegisteredEdges[dglobal]) ;

    for (Segment * o : oldobst)
    {
#ifdef DEBUG_affichage
        ArticulatedObject * mo = o->nid;
        CGoGNout<<"pop obst inside  (index articul , segment ) : "<<mo->index_articulated<<" || "<<o->index<<CGoGNendl;
#endif
        this->popSegment(o) ;
    }

    ARETES oldNeighborObst(RegisteredNeighborEdges[dglobal]) ;

    for (Segment * o : oldNeighborObst)
    {
#ifdef DEBUG_affichage
        ArticulatedObject * mo = o->nid;
        CGoGNout<<"pop obst voisin  (index articul , segment ) : "<<mo->index_articulated<<" || "<<o->index<<CGoGNendl;
#endif
        this->popSegment(o) ;
    }
#ifdef DEBUG_affichage
    CGoGNout<<"tests pour subdivision"<< CGoGNendl;
#endif
    if(!map.isBoundaryMarked(3,dglobal) && map.getDartLevel(dglobal) <= map.getMaxLevel() && !map.volumeIsSubdivided(dglobal) )
    {
#ifdef DEBUG_affichage
        CGoGNout<<"debut subdivision"<< CGoGNendl;
#endif
        Algo::Volume::IHM::subdivideVolumeClassic<PFP>(map, dglobal, position,false);
        res=true;
    }

    map.setCurrentLevel(map.getMaxLevel()) ;
    //    map.check();

    for (Vol d : volumesAdjacentByVertex3(map,(dglobal))) // parcours des sous cellules crées par la subdivision pour mettr a jour l'attribut facecenter a remplacer par un traverseur de sous cellules IHM
    {
        for ( Face f : facesIncidentToVolume3(map,d))
        {
            facecenter[f]=Algo::Surface::Geometry::faceCentroid<PFP, VertexAttribute<VEC3, MAP>>(map,f,position);

        }

    }




#ifdef DEBUG_affichage
    std::cout << "after level = "<< map.getMaxLevel() << std::endl;
#endif
    //same for adjacent obstacles // optimiser
    for (Segment * o : oldNeighborObst)
    {
#ifdef DEBUG_affichage
        ArticulatedObject * mo = o->nid;
        CGoGNout<<"reenregistrement obst voisin  (index articul , segment ) : "<<mo->index_articulated<<" || "<<o->index<<CGoGNendl;

#endif
        FirstRegistrationSegment(o) ;
    }
    //same for obstacles contained
    for (Segment * o : oldobst)
    {
#ifdef DEBUG_affichage
        ArticulatedObject * mo = o->nid;
        CGoGNout<<"reenregistrement obst inside  (index articul , segment ) : "<<mo->index_articulated<<" || "<<o->index<<CGoGNendl;
#endif

        resetPartSubdiv(o);
        FirstRegistrationSegment(o);

    }



#ifdef DEBUG_affichage
    CGoGNout<<"fin refine principal"<<CGoGNendl;
#endif

#endif
    return res;

}

void EnvMap::resetPartSubdiv(Segment* o)
{

    ArticulatedObject * mo = o->nid;

    if (mo != NULL)
    {

        VEC3 pos = mo->parts_[o->indexPart1]->getPosition();
        VEC3 pos2 = mo->parts_[o->indexPart2]->getPosition();
        Dart d1 =mo->parts_[o->indexPart1]->d;
        Dart d2 =mo->parts_[o->indexPart2]->d;

#ifdef DEBUG_affichage
        CGoGNout<<"Reset part subdiv (index articul , segment ,ind1, pos1, d1, ind2, pos2, d2) : "<<mo->index_articulated<<" || "<<o->index<<" || "<<o->indexPart1<<" || "<<pos<<" || "<<d1<<" || "<<o->indexPart2<<" || "<<pos2<<" || "<<d2<<CGoGNendl;
        CGoGNout<<"Faces centroids : "<<Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d1,position)<<" || "<<Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d2,position)<<CGoGNendl;

#endif

        mo->parts_[o->indexPart1]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d1,position)) ;
        mo->parts_[o->indexPart1]->reset_positionFace();
#ifdef DEBUG_affichage
        CGoGNout<<"part1 moved to centroid ,d : "<< mo->parts_[o->indexPart1]->getPosition()<<" || "<<mo->parts_[o->indexPart1]->d<<CGoGNendl;

#endif
        mo->parts_[o->indexPart1]->setState(VOLUME) ;
        mo->parts_[o->indexPart1]->move(pos) ;
#ifdef DEBUG_affichage
        CGoGNout<<"part1 moved in the end (destination, arrivée,d) : "<<pos<<" || "<< mo->parts_[o->indexPart1]->getPosition()<<" || "<<mo->parts_[o->indexPart1]->d<<CGoGNendl;

#endif
        mo->parts_[o->indexPart2]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d2,position)) ;
        mo->parts_[o->indexPart2]->reset_positionFace();
#ifdef DEBUG_affichage
        CGoGNout<<"part2 moved to centroid ,d : "<< mo->parts_[o->indexPart2]->getPosition()<<" || "<<mo->parts_[o->indexPart2]->d<<CGoGNendl;

#endif
        mo->parts_[o->indexPart2]->setState(VOLUME) ;
        mo->parts_[o->indexPart2]->move(pos2) ;
#ifdef DEBUG_affichage
        CGoGNout<<"part2 moved in the end (destination, arrivée,d) : "<<pos2<<" || "<< mo->parts_[o->indexPart2]->getPosition()<<" || "<<mo->parts_[o->indexPart2]->d<<CGoGNendl;

#endif
    }


}

//void EnvMap::coarse()
//{
////	CGoGNout<<"coarse"<<CGoGNendl;
//	// On recrée une liste des faces à simplifier en empêchant les doublons
//	// On en profite pour vérifier les conditions de simplifications
//	std::vector<Dart> checkCoarsenCandidate ;
//	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
//
//	for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
//	{
//		Dart old = coarsenCandidate[it] ;
//		bool oldIsMarked = coarsenMark.isMarked(old) ;
//		coarsenMark.unmark(old) ;
//
//		unsigned int fLevel = map.faceLevel(old) ;
//
//		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
//		{
//			unsigned int cur = map.getCurrentLevel() ;
//			map.setCurrentLevel(fLevel - 1) ;
//
//			if (map.faceIsSubdividedOnce(old))
//			{
//				// on compte le nombre d'agents dans les sous-faces
//				// on en profite pour compter le degré de la face grossière
//				unsigned int degree = 0 ;
//				unsigned int nbAgents = 0 ;
//				Dart fit = old ;
//				do
//				{
//					nbAgents += agentvect[fit].size() ;
//					++degree ;
//					coarsenMark.unmark(fit) ;
//					fit = map.phi1(fit) ;
//				} while (fit != old) ;
//
//
//				//Loop subdivision
//				if (degree == 3)
//				{
//					map.setCurrentLevel(fLevel) ;
//					Dart centerFace = map.phi2(map.phi1(old)) ;
//					nbAgents += agentvect[centerFace].size() ;
//					coarsenMark.unmark(centerFace) ;
//					map.setCurrentLevel(fLevel - 1) ;
//				}
//				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
//			}
//			map.setCurrentLevel(cur) ;
//		}
//	}
//	coarsenCandidate.clear() ;
//
////		 On réalise la simplification (les conditions ont déjà été vérifiées)
//	for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
//	{
//		Dart old = checkCoarsenCandidate[it] ;
//
//		unsigned int fLevel = map.faceLevel(old) ;
//		unsigned int cur = map.getCurrentLevel() ;
//		map.setCurrentLevel(fLevel - 1) ;
//
//		// on compte le degré de la face grossière
//		unsigned int degree = 0 ;
//		Dart fit = old ;
//		do
//		{
//			++degree ;
//			fit = map.phi1(fit) ;
//		} while (fit != old) ;
//
//		PFP::AGENTS agents ;
//		PFP::SegmentVECT obst ;
//		PFP::SegmentVECT neighborObst ;
//		//premier tour pour les darts qui vont disparaitre
//		fit = old ;
//		do
//		{
//			Dart nf = map.phi2(fit) ;
//			if (!map.faceIsSubdivided(nf))
//			{
//				map.setCurrentLevel(fLevel) ;
//				PFP::AGENTS& an = agentvect[nf] ;
//				PFP::SegmentVECT resetob = RegisteredEdges[nf] ;
//				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
//				{
//					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
//				}
//				for (PFP::SegmentVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
//				{
//
//						resetPart(*ait,nf) ;
//
//				}
//				map.setCurrentLevel(fLevel - 1) ;
//			}
//
//			fit = map.phi1(fit) ;
//		} while (fit != old) ;
//		//deuxieme tour pour les présents
//		fit = old ;
//		do
//		{
//			PFP::AGENTS a(agentvect[fit]) ;
//			PFP::SegmentVECT ob(RegisteredEdges[fit]) ;
//
//			agents.insert(agents.end(), a.begin(), a.end()) ;
//
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			for(PFP::SegmentVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
//			{
////				resetObstPartInFace(*ait, fit,fLevel);
//				this->popSegment(*ait) ;
//				obst.push_back(*ait);
//			}
//
//
//
//			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
//				popAgentInCells(*ait, fit) ;
//			map.setCurrentLevel(fLevel - 1) ;
//
//			fit = map.phi1(fit) ;
//		} while (fit != old) ;
//
//		if (degree == 3)
//		{
//			map.setCurrentLevel(fLevel) ;
//			Dart centerFace = map.phi2(map.phi1(old)) ;
//			PFP::AGENTS a(agentvect[centerFace]) ;
//			PFP::SegmentVECT ob(RegisteredEdges[centerFace]) ;
//
//			agents.insert(agents.end(), a.begin(), a.end()) ;
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			for(PFP::SegmentVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
//			{
////				resetObstPartInFace(*ait, fit,Level);
//				this->popSegment(*ait) ;
//				obst.push_back(*ait);
//			}
//			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
//				popAgentInCells(*ait, centerFace) ;
//			map.setCurrentLevel(fLevel - 1) ;
//		}
//
//		//dernier tour concernant les voisins
//		fit = old ;
//		do
//		{
//			PFP::SegmentVECT nob(neighborRegisteredEdges[fit]) ;
//
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			for(PFP::SegmentVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
//			{
//				this->popSegment(*ait) ;
//				neighborObst.push_back(*ait);
//			}
//			map.setCurrentLevel(fLevel - 1) ;
//
//			fit = map.phi1(fit) ;
//		} while (fit != old) ;
//
//		if (degree == 3)
//		{
//			map.setCurrentLevel(fLevel) ;
//			Dart centerFace = map.phi2(map.phi1(old)) ;
//			PFP::SegmentVECT nob(neighborRegisteredEdges[centerFace]) ;
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			for(PFP::SegmentVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
//			{
//				this->popSegment(*ait) ;
//				neighborObst.push_back(*ait);
//			}
//			map.setCurrentLevel(fLevel - 1) ;
//		}
//
//
//
//
//
//		map.setCurrentLevel(fLevel - 1) ;
//
//		Algo::Surface::IHM::coarsenFace<PFP>(map, old, position) ;
//
//		std::pair<bool, bool>& sf = subdivisableFace[old] ;
//		sf.first = true ;
//		sf.second = true ;
//
//		map.setCurrentLevel(map.getMaxLevel()) ;
//
//		neighborAgentvect[old].clear() ;
//
//		for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
//		{
//			(*itA)->part_.d = old ;
//			pushAgentInCells(*itA, old) ;
//		}
//		for (PFP::SegmentVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
//		{
//			resetObstPartInFace(*ait, old);
//			pushSegmentInSetOfCells(*ait) ;
//		}
//		for (PFP::SegmentVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
//		{
//			pushSegmentInSetOfCells(*ait) ;
//		}
//
//		Dart dd = old ;
//		do
//		{
//			Dart ddd = map.alpha1(map.alpha1(dd)) ;
//			while (ddd != dd)
//			{
//				neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
//				ddd = map.alpha1(ddd) ;
//			}
//			dd = map.phi1(dd) ;
//		} while (dd != old) ;
//
//		if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
//		{
//			coarsenMark.mark(old) ;
//			coarsenCandidate.push_back(map.faceOldestDart(old)) ;
//		}
//
//		map.setCurrentLevel(cur) ;
//	}
//	map.setCurrentLevel(map.getMaxLevel()) ;
//
//	if (coarsenCandidate.size() > 0)
//		updateMap() ;
//}
//
//void EnvMap::updateMap()
//{
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
//
//	refine();
//	coarse();
//}