#include "env_map.h"
#include "utils.h"
#include "agent.h"
#include "obstacle.h"

#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"
#include "Algo/Import/importSvg.h"
#include "Algo/BooleanOperator/mergeVertices.h"

#include "env_generator.h"

using namespace CGoGN ;

EnvMap::EnvMap() :
	obstacleDistance(Agent::range_),
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
#ifndef SPATIAL_HASHING
	refineMark(map),
	coarsenMark(map)
#else
ht_agents(1024),
ht_obstacles(1024)
#endif
{
	position = map.addAttribute<PFP::VEC3,VERTEX>("position") ;
	normal = map.addAttribute<PFP::VEC3,VERTEX>("normal") ;

#ifndef SPATIAL_HASHING
	agentvect = map.addAttribute<PFP::AGENTVECT,FACE>("agents") ;
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT,FACE>("neighborAgents") ;
	obstvect = map.addAttribute<PFP::OBSTACLEVECT,FACE>("obstacles") ;
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB,FACE>("subdivisableFace") ;

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

void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize)
{
	std::cout << "Init EnvMap" << std::endl ;
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

	geometry.reset() ;
	geometry.addPoint(bottomLeft) ;
	geometry.addPoint(topRight) ;
	minCellSize = minSize ;
	maxCellSize = maxSize ;
	std::cout << " - Geometry : " << geometry ;
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
#endif

	unsigned int nbSquares = 6 ;

	switch (config)
	{
	case 0 :
		CityGenerator::generateGrid<PFP>(*this) ;
		break ;
	case 1 :
		CityGenerator::generateGrid<PFP>(*this) ;
		break ;
	case 2 :
		CityGenerator::generateCity<PFP>(*this) ;
		// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
		break ;
	case 3 :
	{
//			std::string filename = "/home/jund/Desktop/drawingQuads.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingTest.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingLines.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrutSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrut.svg" ;
//			Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
//			scale(3.2808399f) ;
//
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
	}
		CityGenerator::generateCity<PFP>(*this) ;
		break ;
	case 4 :
		CityGenerator::generatePlanet<PFP>(map, position, obstacleMark, buildingMark, 200.0f,
		                                   nbSquares) ;
		break ;
	}

//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);

#ifndef SPATIAL_HASHING
//	for (Dart d = map.begin() ; d != map.end(); map.next(d)) {
//		Dart d1 = map.phi1(d);
//		Dart d11= map.phi1(d1);
//		if (map.phi1(d11) != d) map.splitFace(d,d11);
//	}
	map.init() ;
//	registerObstaclesInFaces();

//	subdivideAllToMaxLevel();

	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end();
	    subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
#endif
}

void EnvMap::markPedWay()
{
	CellMarker<FACE> treat(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
			treat.mark(d) ;

			Dart dd = d ;
			do
			{
				Dart ddd = dd ;
				do
				{
					if (obstacleMark.isMarked(dd))
					{
						pedWayMark.mark(d) ;
						break ;
					}
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;

				dd = map.phi1(dd) ;

			} while (dd != d) ;
		}
	}

	treat.unmarkAll() ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
			treat.mark(d) ;

			Dart dd = d ;
			do
			{
				if (pedWayMark.isMarked(map.phi2(dd)) && pedWayMark.isMarked(map.phi2(map.phi1(dd)))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(dd))))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(map.phi1(dd))))))
				{
					pedWayMark.mark(d) ;
					break ;
				}

				dd = map.phi1(dd) ;

			} while (dd != d) ;
		}
	}
}

unsigned int EnvMap::mapMemoryCost()
{
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
}

#ifndef SPATIAL_HASHING
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::Geometry::faceCentroid<PFP>(map, old, position) ;
						Dart fd = old ;
						do
						{
							PFP::VEC3& p = position[fd] ;
							PFP::VEC3 edge = Algo::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::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) ;
}

Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
	CellMarkerStore<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
		}
	}

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

#ifndef SPATIAL_HASHING
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
	Dart dd = d ;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd)
		{
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
		}
		dd = map.phi1(dd) ;
	} while (dd != d) ;
}

void EnvMap::registerObstaclesInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;
			if (!buildingMark.isMarked(d))
			{
				Dart dd = d ;

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
				do
				{
					if (obstacleMark.isMarked(dd))
					{
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
						                           position[previous], position[map.phi1(next)]) ;
						obstvect[d].push_back(o) ;
					}
					dd = map.phi1(dd) ;
				} while (dd != d) ;

				//second : test all edges of neighboring faces
				do
				{
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
					while (ddd != dd)
					{
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
						ddd = map.alpha1(ddd) ;
					}
					dd = map.phi1(dd) ;
				} while (dd != d) ;
			}
		}
	}
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
	Dart stop ;
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;

	Dart dd = d ;
	do
	{
		if (obstacleMark.isMarked(dd))
		{
//			if(buildingMark.isMarked(dd))
//			{
//				std::cout << "caca prout" << std::endl;
//				Dart next = map.phi1(dd);
//				Dart previous = map.phi_1(dd);
//				Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]);
//				obst.push_back(o);
//			}
//			else
//			{
			Dart dd2 = map.phi2(dd) ;
			Dart next = map.phi1(dd2) ;
			Dart previous = map.phi_1(dd2) ;
			Obstacle* o = new Obstacle(position[dd2], position[next], position[previous],
			                           position[map.phi1(next)]) ;
			obst.push_back(o) ;
//			}
		}
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
}

//void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
//{
//	Dart oldFace = agent->part_.lastCrossed;
//	Dart newFace = agent->part_.d;
//
//	agentvect[newFace].push_back(agent);
//
//	PFP::AGENTS::iterator end = agentvect[oldFace].end();
//	for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
//	{
//		if(*it == agent)
//		{
//			*it = agentvect[oldFace].back();
//			agentvect[oldFace].pop_back();
//			break;
//		}
//	}
//
//	// mark adjacent cells shared by newFace and oldFace
//	CellMarkerNoUnmark<FACE> f(map);
//	Dart d = oldFace;
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != oldFace);
//
//	d = map.phi1(oldFace);
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != map.phi1(oldFace));
//
//	// remove agent from cells adjacent to oldFace but not to newFace
//	Dart dd = oldFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//			{
//				bool found = false;
//				PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
//				for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
//				{
//					if(*it == agent)
//					{
//						*it = neighborAgentvect[ddd].back();
//						neighborAgentvect[ddd].pop_back();
//						found = true;
//					}
//				}
//			}
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != oldFace);
//
//	// add agent from cells adjacent to oldFace but not to newFace
//	// and unmark shared cells
//	dd = newFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//				neighborAgentvect[ddd].push_back(agent);
//			else
//				f.unmark(ddd);
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != newFace);
//}

void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
	Dart newFace = agent->part_.d ;

	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;

	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
		if (sf.first == false || (sf.first == true && sf.second))
		{
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
		}
	}

	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
	}
}

void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
}

void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
	    ++it)
	{
		Dart d = (*it) ;
		refineMark.unmark(d) ;

		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
			int fLevel = -1 ;
			Dart old = map.faceOldestDart(d) ;

			bool subdivisable = true ;

			std::pair<bool, bool>& sf = subdivisableFace[old] ;
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré

				fLevel = map.faceLevel(old) ;
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
				do
				{
					PFP::VEC3& p = position[fd] ;
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
						subdivisable = false ;
						break ;
					}
					fd = map.phi1(fd) ;
				} while (fd != old) ;
				map.setCurrentLevel(map.getMaxLevel()) ;

				sf.first = true ;
				sf.second = subdivisable ;
			}

			if (subdivisable)
			{
				if (fLevel == -1) fLevel = map.faceLevel(old) ;

				sf.first = false ;

				PFP::AGENTS oldAgents(agentvect[old]) ;
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;

				neighborAgentvect[old].clear() ;

				map.setCurrentLevel(fLevel) ;
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
				CellMarkerStore newF(map, FACE) ;
				unsigned int degree = 0 ;
				Dart dd = old ;
				do
				{
					++degree ;
					newF.mark(dd) ;
					dd = map.phi1(dd) ;
				} while (dd != old) ;
				if (degree == 3)
				{
					Dart centerFace = map.phi2(map.phi1(old)) ;
					newF.mark(centerFace) ;
				}
				map.setCurrentLevel(map.getMaxLevel()) ;

				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
				}

				dd = old ;
				do
				{
					Dart d3 = dd ;
					do
					{
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
						}
						d3 = map.phi1(d3) ;
					} while (d3 != dd) ;

					map.setCurrentLevel(fLevel) ;
					dd = map.phi1(dd) ;
					map.setCurrentLevel(map.getMaxLevel()) ;
				} while (dd != old) ;

				if (degree == 3)
				{
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
					do
					{
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
						}
						d3 = map.phi1(d3) ;
					} while (d3 != centerFace) ;
				}
			}
		}
	}
	refineCandidate.clear() ;

	// 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) ;

				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 ;
		fit = old ;
		do
		{
			PFP::AGENTS a(agentvect[fit]) ;
			agents.insert(agents.end(), a.begin(), a.end()) ;
			map.setCurrentLevel(map.getMaxLevel()) ;
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, fit) ;
			map.setCurrentLevel(fLevel - 1) ;
			Dart nf = map.phi2(fit) ;
			if (!map.faceIsSubdivided(nf))
			{
				map.setCurrentLevel(fLevel) ;
				PFP::AGENTS& an = agentvect[nf] ;
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
				{
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
				}
				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]) ;
			agents.insert(agents.end(), a.begin(), a.end()) ;
			map.setCurrentLevel(map.getMaxLevel()) ;
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, centerFace) ;
			map.setCurrentLevel(fLevel - 1) ;
		}
		neighborAgentvect[old].clear() ;

		Algo::IHM::coarsenFace<PFP>(map, old, position) ;

		std::pair<bool, bool>& sf = subdivisableFace[old] ;
		sf.first = true ;
		sf.second = true ;

		map.setCurrentLevel(map.getMaxLevel()) ;

		for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
		{
			(*itA)->part_.d = old ;
			pushAgentInCells(*itA, old) ;
		}

		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::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
	agent->part_.move(pos) ;
}
#endif

#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}

const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a) {
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors) {
	Geom::Vec2ui c = agentPositionCell(a) ;
	for(int ii = -1 ; ii <= 1 ; ++ii)
	{
		for(int jj = -1 ; jj <= 1 ; ++jj)
		{
			if (ii != 0 || jj != 0)
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				const std::vector<Agent*>& v = ht_agents[cc] ;
				neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
			}
		}
	}
}

void EnvMap::addAgentInGrid(Agent* a) {
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c) {
	ht_agents[c].push_back(a) ;
}


void EnvMap::removeAgentFromGrid(Agent* a) {
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c) {
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;
}

#endif