#include "env_map.h"
#include "MovingObject.h"
#include "surface.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 = 1.0f;
    mapMinX=0;
    mapMaxX=0;
    mapMinY=0;
    mapMaxY=0;
    mapMinZ=0;
    mapMaxZ=0;

    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");
        RegisteredTriangles=map.addAttribute<TRIANGLES,VOLUME, MAP>("RegisteredTriangles");
        RegisteredNeighborTriangles=map.addAttribute<TRIANGLES,VOLUME, MAP>("RegisteredNeighborTriangles");


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

//        map.check();


        TraversorV<MAP> tv(map);
        for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
        {
            position[d] *= 10;
            if(position[d][0]<mapMinX) mapMinX=position[d][0];
            if(position[d][0]>mapMaxX) mapMaxX=position[d][0];
            if(position[d][1]<mapMinY) mapMinY=position[d][1];
            if(position[d][1]>mapMaxY) mapMaxY=position[d][1];
            if(position[d][2]<mapMinZ) mapMinZ=position[d][2];
            if(position[d][2]>mapMaxZ) mapMaxZ=position[d][2];
        }
        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);
        }


    }

}

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];
        }
    }
}
//////////////////////////////////enregistrement des segments

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(memo.size() == 1u)
        {
            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
    d=map.volumeOldestDart(d);
    addElementToVector(RegisteredEdges[d],o);

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

#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;


    for (Dart d :memo_cross)
    {
        d=map.volumeOldestDart(d);
        mo->belonging_cells[n].push_back(d);
        addElementToVector(RegisteredEdges[d],o);
        mo->addGeneralCell (d);
#ifdef DEBUG_affichage
        CGoGNout<<"cellule du belong :"<<(d)<<CGoGNendl;
#endif

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

    for (Dart d : mo->neighbor_cells[n])
    {
            addElementToVector<Segment*>(RegisteredNeighborEdges[d],o);
             mo->addGeneralNeighbor ( (d));

    }
#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 (Dart d : mo->belonging_cells[n])
        {
            removeElementFromVector(RegisteredEdges[d], o) ;
            mo->removeGeneralCell (d);
        }
        for (Dart d : mo->neighbor_cells[n])
        {
                removeElementFromVector(RegisteredNeighborEdges[d], o) ;
                mo->removeGeneralNeighbor (d);

        }
        mo->belonging_cells[n].clear();
        mo->neighbor_cells[n].clear();
    }
}


/////////////////////////////enregistrement des triangles



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

    Surface * mo = o->surf;

    if (mo != NULL)
    {
#ifdef DEBUG_affichage
        CGoGNout<<"First Registration Triangle "<<o->d<<CGoGNendl;
#endif
        CellMarkerMemo<MAP, VOLUME> inside(map);
        CellMarkerMemo<MAP, VOLUME> neighbor(map);
        Dart cell =o->cell;
        std::vector<Dart> memo;

        for(Edge e : edgesIncidentToFace2<SURFACE>(mo->surfaceMap,cell)) // first mark inside
        {
            Dart cell2=mo->surfaceMap.phi1(e);
            // on récupère les aprticules correspondant a chaque edge du triangle a enregistrer

            Particule * p1 = mo->parts_[mo->indexParticule[cell]];


            Dart d1=p1->d; // darts de la carte 3D contenant les particules
            memo = getMemoCross(p1->getPosition(),mo->positionSurface[cell2],d1);

            for (Dart d : memo)
                   inside.mark(d);

        }



        for(Dart d : inside.get_markedCells()) // and register them
        {
            d=map.volumeOldestDart(d);
            pushTriangleInCell(o,d);
            (mo->belonging_cells[o->indexTri]).push_back(d);
            mo->addGeneralCell ( d);
        }
        markNeighbors(&inside,&neighbor,(mo->belonging_cells[o->indexTri])); // then mark neighbors
        for(Dart d : neighbor.get_markedCells())    // and register them
        {
            d=map.volumeOldestDart(d);
            pushTriangleInCellAsNeighbor(o,d);
            (mo->neighbor_cells[o->indexTri]).push_back(d);
            mo->addGeneralNeighbor ( d);
        }


    }
}


void EnvMap::markNeighbors(CellMarkerMemo<MAP,VOLUME> * memo_mark, CellMarkerMemo<MAP,VOLUME>* OneRingMark, const std::vector<Dart>& memo_cross) // enregistrer une dart dans un ensemble de cellules
{
#ifdef IHMap
    assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
    assert(!memo_cross.empty());


    for(Dart  e : memo_cross)
    {

        for (Vol d : volumesAdjacentByVertex3(map,e))
        {

            if (!map.isBoundaryMarked<3>(d) && !memo_mark->isMarked(d)  )
            {
                OneRingMark->mark(d);

            }
        }

    }
}


void EnvMap::popAndPushTriangle(Triangle* o)// maj de l'enregistrement
{
#ifdef DEBUG_affichage
    CGoGNout<<"pop&push arete "<<o->index<<CGoGNendl;
#endif
    popTriangle(o);
    FirstRegistrationTriangle(o);


}

void EnvMap::popTriangle(Triangle* o)
{
    Surface * mo = o->surf;
#ifdef IHMap
    assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
    if (mo != NULL)
    {
        Dart cell =o->cell;

        int n = mo->indexTriangle[cell];



        for (Dart d : mo->belonging_cells[n])
        {
            popTriangleInCell(o,d);
            mo->removeGeneralCell (d);
        }
        for (Dart d : mo->neighbor_cells[n])
        {

                popTriangleInCellAsNeighbor(o,d);
                mo->removeGeneralNeighbor (d);

        }
        mo->belonging_cells[n].clear();
        mo->neighbor_cells[n].clear();
    }
}

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


    map.setCurrentLevel(map.getMaxLevel()) ;
    unsigned int vLevel = map.volumeLevel(dglobal);
    Dart old = map.volumeOldestDart(dglobal);
    Dart centerDart=old;
#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())
//    {

    if(OneLevelDifference)
    {
        map.setCurrentLevel(vLevel) ;
        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);
                }
                else if(!map.volumeIsSubdivided(nv))  //check if neighboring volumes have to be subdivided first to avoid one cell surrounded by subdivided ones
                {
//                    CGoGNout<<"neighbor not subdivided :"<<nv<<CGoGNendl;
                    if(map.checkForSurrounded(nv))
                    {
//                        CGoGNout<<"subdiv to avoid"<<CGoGNendl;
                        subdivideVolume(nv,false);

                    }
                }
            }
        }
//        CGoGNout<<"\n";
    }


     const std::chrono::time_point<std::chrono::system_clock> startPop = std::chrono::system_clock::now();
    // on commence ensuite a subdiviser
//    map.check();
    map.setCurrentLevel(map.getMaxLevel()) ;

    const ARETES oldobst(RegisteredEdges[dglobal]) ;

    for (Segment * o : oldobst)
    {
        this->popSegment(o) ;
    }

    ARETES oldNeighborObst(RegisteredNeighborEdges[old]) ;

    for (Segment * o : oldNeighborObst)
    {
//        this->popSegment(o) ; // old version
//        removeElementFromVector(RegisteredNeighborEdges[old], o) ;
//        CGoGNout<<"tentative de pop voisin "<<o->index<<" : ";
        if(removeElementFromVector(o->nid->neighbor_cells[o->index],old))
        {
             o->nid->removeGeneralNeighbor (old);
        }
        else
        {
             CGoGNout<<"failed to remove... ";
             afficheVector(o->nid->neighbor_cells[o->index]);
             CGoGNout<<" et old :"<<old<<CGoGNendl;
        }

    }
    RegisteredNeighborEdges[old].clear();

    const TRIANGLES oldTrian(RegisteredTriangles[dglobal]) ;

    for (Triangle * o : oldTrian)
    {
        this->popTriangle(o) ;
    }

    TRIANGLES oldTrianNeighbor(RegisteredNeighborTriangles[old]) ;

    for (Triangle * o : oldTrianNeighbor)
    {
//        this->popTriangle(o) ; // old version
//        removeElementFromVector(RegisteredNeighborTriangles[old], o) ;
        removeElementFromVector(o->surf->neighbor_cells[o->indexTri],old);
        o->surf->removeGeneralNeighbor (old);
    }
    RegisteredNeighborTriangles[old].clear();

    const std::chrono::time_point<std::chrono::system_clock> endPop = std::chrono::system_clock::now();
    duration_pop = endPop - startPop;
    if(duration_pop.count()>0.035)std::cout << "pop duration : " << duration_pop.count()  << "s\n";

    const std::chrono::time_point<std::chrono::system_clock> startSubdiv = std::chrono::system_clock::now();

    if(!map.isBoundaryMarked(3,dglobal) && map.getDartLevel(dglobal) <= map.getMaxLevel() && !map.volumeIsSubdivided(dglobal) )
    {
        centerDart =Algo::Volume::IHM::subdivideVolumeClassic<PFP>(map, dglobal, position,false);

        res=true;
    }

    const std::chrono::time_point<std::chrono::system_clock> endSubdiv = std::chrono::system_clock::now();
    duration_subdiv = endSubdiv - startSubdiv;
    if(duration_subdiv.count()>0.035)std::cout << "subdiv duration : " << duration_subdiv.count()  << "s\n";

    map.setCurrentLevel(map.getMaxLevel()) ;
    CellMarkerMemo<MAP,VOLUME> newVolumes(map);
    if(res)
    {
        for(Vol vo : VolumesIncidentToVertex3(map,centerDart))
        {
            newVolumes.mark(map.volumeOldestDart(vo));
        }
    }
    else
    {
        newVolumes.mark(old);
    }




//    Dart start = old;
//    do                  // on marque les nouvelles cellules ! attention TODO cas des volumes qui n'ont aucune arete en commun avec le volume d'origine
//    {
//        map.setCurrentLevel(vLevel+1);
//        newVolumes.mark(map.volumeOldestDart(start));
//        map.setCurrentLevel(vLevel);
//        start=map.phi1(start);
//    }while(start !=old);


    //    map.check();



//    CGoGNout<<"nombre de sous cellules markées :"<<newVolumes.get_markedCells().size()<<CGoGNendl;
    for (Dart d : newVolumes.get_markedCells())
    {
//        CGoGNout<<"markée :"<<d<<CGoGNendl;

        for ( Face f : facesIncidentToVolume3(map,d)) // maj des attributs de face et volume de la carte
        {
            facecenter[f]=Algo::Surface::Geometry::faceCentroid<PFP, VertexAttribute<VEC3, MAP>>(map,f,position);

        }
        VEC3 center=Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d,position);
        color[d] = VEC3((center[0]-mapMinX)/(mapMaxX-mapMinX),(center[1]-mapMinY)/(mapMaxY-mapMinY),(center[2]-mapMinZ)/(mapMaxZ-mapMinZ));


        // reenregistrement des voisins
        for(Vol v : volumesAdjacentByVertex3(map,d))
        {
            if(!newVolumes.isMarked(v))
            {
//                CGoGNout<<"reenregistrement des voisins de : "<<v<<CGoGNendl;
                for(Segment * s : RegisteredEdges[v])
                {
//                    CGoGNout<<"tentative ajout de : "<<s->index<<CGoGNendl;
                    if(addElementToVector(RegisteredNeighborEdges[d],s))
                    {
//                        CGoGNout<<"reussie car non présent"<<CGoGNendl;
                           s->nid->neighbor_cells[s->index].push_back(d);
                    }
                }
                for(Triangle * t : RegisteredTriangles[v])
                {
                    if(addElementToVector(RegisteredNeighborTriangles[d],t))
                    {
                           t->surf->neighbor_cells[t->indexTri].push_back(d);
                    }
                }
            }
        }
    }

    const std::chrono::time_point<std::chrono::system_clock> startPush = std::chrono::system_clock::now();



//    //same for adjacent triangles // optimiser
//    for (Triangle * o : oldTrianNeighbor)
//    {
////        this->FirstRegistrationTriangle(o) ; // old version

//    }


    //same for triangles // optimiser
    for (Triangle * o : oldTrian)
    {
        resetPartSubdiv(o);
        this->FirstRegistrationTriangle(o) ;
    }

    const std::chrono::time_point<std::chrono::system_clock> endPushTriangle = std::chrono::system_clock::now();


//    //same for adjacent obstacles // optimiser
//    for (Segment * o : oldNeighborObst)
//    {
////        FirstRegistrationSegment(o) ; // old version
//    }

    const std::chrono::time_point<std::chrono::system_clock> endPushSegmentNeighbors = std::chrono::system_clock::now();
    //same for obstacles contained
    for (Segment * o : oldobst)
    {
        resetPartSubdiv(o);
        FirstRegistrationSegment(o);

    }

    const std::chrono::time_point<std::chrono::system_clock> endPush = std::chrono::system_clock::now();
    duration_push = endPush - startPush;
    std::chrono::duration<double> duration_triangle= (endPushTriangle - startPush);
    std::chrono::duration<double> duration_segment= (endPush - endPushSegmentNeighbors);
    std::chrono::duration<double> duration_segmentNeighbor= (endPushSegmentNeighbors - endPushTriangle);
     if(duration_triangle.count()>0.035)std::cout << "push triangle duration : " << duration_triangle.count()  << "s\n";
    if(duration_push.count()>0.035)std::cout << "push duration : " << duration_push.count() << "\n dont pour les segments présents : "<<duration_segment.count() <<"s\n et pour les voisins :"<<duration_segmentNeighbor.count() <<"s\n";


#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::resetPartSubdiv(Triangle * o)
{

    Surface * mo = o->surf;
    Dart cell =o->cell;
    Dart cell2 = mo->surfaceMap.phi1(cell);
    Dart cell3 = mo->surfaceMap.phi1(cell2);
    Particule * p = mo->parts_[mo->indexParticule[cell]];
    Particule * p2 = mo->parts_[mo->indexParticule[cell2]];
    Particule * p3 = mo->parts_[mo->indexParticule[cell3]];

    if (mo != NULL)
    {

        VEC3 pos = p->getPosition();
        VEC3 pos2 = p2->getPosition();
        VEC3 pos3 = p3->getPosition();
        Dart d1 =p->d;
        Dart d2 =p2->d;
        Dart d3 = p3->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

        p->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d1,position)) ;
        p->reset_positionFace(); // attention avec les particules 3D on doit mettre a jour la position_face !!!
#ifdef DEBUG_affichage
        CGoGNout<<"part1 moved to centroid ,d : "<< p->getPosition()<<" || "<<p->d<<CGoGNendl;

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

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

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

#endif

        p3->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::volumeCentroid<PFP,VertexAttribute<VEC3,MAP>>(map,d3,position)) ;
        p3->reset_positionFace();
#ifdef DEBUG_affichage
        CGoGNout<<"part3 moved to centroid ,d : "<< p3->getPosition()<<" || "<<p3->d<<CGoGNendl;

#endif
        p3->setState(VOLUME) ;
        p3->move(pos3) ;
#ifdef DEBUG_affichage
        CGoGNout<<"part3 moved in the end (destination, arrivée,d) : "<<pos3<<" || "<< p3->getPosition()<<" || "<<p3->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();
//}