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

#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"

using namespace CGoGN;

EnvMap::EnvMap() :
	obstacleMark(map, EDGE),
	buildingMark(map, FACE),
	refineMark(map, FACE),
	coarsenMark(map, FACE)
{
	position = map.addAttribute<PFP::VEC3>(VERTEX, "position");
	normal = map.addAttribute<PFP::VEC3>(VERTEX, "normal");
	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");
}

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

Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
	CellMarkerStore m(map, FACE);
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(!m.isMarked(d))
		{
			m.mark(d);
			if(!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();
}

void EnvMap::init()
{
	float sideSize = 70.0f;
	unsigned int nbSquares = 14;
	CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
//	CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
	map.init();
	registerObstaclesInFaces();
	for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
}

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 m(map, FACE);
	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;
				do
				{
					if(obstacleMark.isMarked(dd))
						obstvect[d].push_back(map.phi2(dd));
					dd = map.phi1(dd);
				} while(dd != d);

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

	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(m.isMarked(d) && !buildingMark.isMarked(d))
			m.unmark(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))
				obst.push_back(dd);
			else
				obst.push_back(map.phi2(dd));
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

//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 f(map, FACE);
//	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) && agentvect[oldFace].size() < nbAgentsToSimplify / 4)
	{
		coarsenMark.mark(oldFace);
		coarsenCandidate.push_back(map.faceOldestDart(oldFace));
	}
}

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)
		{
			unsigned 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 minDistSq = Agent::neighborDistSq_ ; // 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() < minDistSq)
					{
						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) ;
				CellMarkerStore newF(map, FACE);
				newF.mark(old);
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;

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

				unsigned int degree = 0 ;
				Dart dd = old ;
				do
				{
					++degree ;
					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) ;
				}
			}
		}
	}

	for(std::vector<Dart>::iterator it = coarsenCandidate.begin(); it != coarsenCandidate.end(); ++it)
	{
		Dart old = (*it) ;
		coarsenMark.unmark(old) ;

//		Dart old = map.faceOldestDart(d) ;
		unsigned int fLevel = map.faceLevel(old) ;

		if(fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

			if(map.faceIsSubdividedOnce(old))
			{
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
				do
				{
					nbAgents += agentvect[fit].size() ;
					++degree ;
					fit = map.phi1(fit) ;
				} while(fit != old) ;
				if(degree == 3)
				{
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
					map.setCurrentLevel(fLevel - 1) ;
				}

				if(nbAgents < nbAgentsToSimplify)
				{
					PFP::AGENTS agents ;
					Dart fit = old ;
					do
					{
						map.setCurrentLevel(fLevel) ;
						coarsenMark.unmark(fit) ;
						std::vector<Dart>::iterator start = it + 1 ;
						unsigned int fEmb = map.getEmbedding(FACE, fit) ;
						while(start != coarsenCandidate.end())
						{
							if(map.getEmbedding(FACE, *start) == fEmb)
							{
								*start = coarsenCandidate.back() ;
								coarsenCandidate.pop_back() ;
							}
							else
								++start ;
						}
						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)) ;
						coarsenMark.unmark(centerFace) ;
						std::vector<Dart>::iterator start = it + 1 ;
						unsigned int fEmb = map.getEmbedding(FACE, centerFace) ;
						while(start != coarsenCandidate.end())
						{
							if(map.getEmbedding(FACE, *start) == fEmb)
							{
								*start = coarsenCandidate.back() ;
								coarsenCandidate.pop_back() ;
							}
							else
								++start ;
						}
						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) ;
					}

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

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

					map.setCurrentLevel(map.getMaxLevel());

					agentvect[old].clear();
					for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
					{
						(*it)->part_.d = old ;
						pushAgentInCells(*it, 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);

				}
			}
			map.setCurrentLevel(cur) ;
		}
	}

	map.setCurrentLevel(map.getMaxLevel()) ;
}

void EnvMap::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.m_position;
	agent->part_.m_position = Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position);
	agent->part_.state = FACE;
	agent->part_.move(pos);
}