/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "nav_detour/navmesh.h"
		#include "core/geometry.h"
		#include "Recast.h"
		#include "DetourNavMeshQuery.h"
		#include "DetourNavMeshBuilder.h"
		#include "log/log.h"

		using namespace GS::Core;
		using namespace GS::Nav;


//------------------------------------------------------------------------------
bool				Mesh::Build(const Geometry *geo, const BuildConfig &cfg)
{
	// Convert geometry to triangle.
	uint nverts = geo->vtx.GetCount();

	Array <float> verts(nverts);
	if	(float *pverts = verts.c_ptr())
		for	(uint n = 0; n < nverts; ++n)
		{
			*pverts++ = geo->vtx[n][0];
			*pverts++ = geo->vtx[n][1];
			*pverts++ = geo->vtx[n][2];
		}

	uint ntris = geo->GetTriangleCount();

	Array <int> tris(ntris * 3);
	if	(int *ptris = tris.c_ptr())
		for	(uint n = 0; n < geo->pol.GetCount(); ++n)
		{
			Polygon &p = geo->pol[n];
			for	(int i = 1; i < (p.vtx_count - 1); ++i)
			{
				*ptris++ = p.binding[0];
				*ptris++ = p.binding[i];
				*ptris++ = p.binding[i + 1];
			}
		}

	// Init build configuration from GUI
	rcConfig m_cfg;

	m_cfg.cs = 0.5f;					// Cell size.
	m_cfg.ch = 0.2f;					// Cell height.
	m_cfg.walkableSlopeAngle = 40.f;	// Max slope.
	m_cfg.walkableHeight = (int)Math::Ceil(cfg.agent.height / m_cfg.ch);
	m_cfg.walkableClimb = (int)Math::Floor(cfg.agent.max_climb / m_cfg.ch);
	m_cfg.walkableRadius = (int)Math::Ceil(cfg.agent.radius / m_cfg.cs);

/*
	m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
	m_cfg.maxSimplificationError = m_edgeMaxError;
	m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize);		// Note: area = size*size
	m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize);	// Note: area = size*size
	m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
	m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
	m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError;
*/
	/*
		Set the area where the navigation will be build.
		Here the bounds of the input mesh are used, but the area could be
		specified by an user defined box, etc.
	*/

	MinMax mm = geo->ComputeMinMax();
	rcVcopy(m_cfg.bmin, &mm.mn.x);
	rcVcopy(m_cfg.bmax, &mm.mx.x);
	rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);

	// Allocate voxel heightfield where we rasterize our input data to.
	AutoPtr <rcHeightfield> solid(rcAllocHeightfield());
	if	(solid.IsNull())
		__ERR__(__LOG_E__ << "Failed to allocate heightfield.\n", false)

	rcContext	ctx;
	if	(!rcCreateHeightfield(&ctx, *solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
		__ERR__(__LOG_E__ << "Failed to create heightfield.\n", false)

	/*
		Allocate array that can hold triangle area types.
		If you have multiple meshes you need to process, allocate an array which
		can hold the max number of triangles you need to process.
	*/
	Array <unsigned char> triareas(ntris);
	if	(triareas.IsNull())
		__ERR__(__LOG_E__ << "Failed to allocate triangle areas.\n", false)
	
	/*
		Find triangles which are walkable based on their slope and rasterize
		them. If your input data is multiple meshes, you can transform them
		here, calculate the are type for each of the meshes and rasterize them.
	*/
	Memory::Set(triareas, 0, ntris * sizeof(unsigned char));
	rcMarkWalkableTriangles(&ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, triareas);
	rcRasterizeTriangles(&ctx, verts, nverts, tris, triareas, ntris, *solid, m_cfg.walkableClimb);

	triareas.Free();
	
	/*
		Once all geometry is rasterized, we do initial pass of filtering to
		remove unwanted overhangs caused by the conservative rasterization
		as well as filter spans where the character cannot possibly stand.
	*/
	rcFilterLowHangingWalkableObstacles(&ctx, m_cfg.walkableClimb, *solid);
	rcFilterLedgeSpans(&ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *solid);
	rcFilterWalkableLowHeightSpans(&ctx, m_cfg.walkableHeight, *solid);

	/*
		Compact the heightfield so that it is faster to handle from now on.
		This will result more cache coherent data as well as the neighbours
		between walkable cells will be calculated.
	*/
	rcCompactHeightfield *chf = rcAllocCompactHeightfield();
	if	(!chf)
		__ERR__(__LOG_E__ << "Failed to allocate compact heightfield.\n", false)
	if	(!rcBuildCompactHeightfield(&ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *solid, *chf))
		__ERR__(__LOG_E__ << "Failed to build compact heightfield.\n", false)
	
	solid = NULL;
		
	// Erode the walkable area by agent radius.
	if	(!rcErodeWalkableArea(&ctx, m_cfg.walkableRadius, *chf))
		__ERR__(__LOG_E__ << "Failed to erode walkable area.\n", false)

	// (Optional) Mark areas.
/*
	const ConvexVolume	*vols = m_geom->getConvexVolumes();
	for (int i  = 0; i < m_geom->getConvexVolumeCount(); ++i)
		rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
*/
	// Prepare for region partitioning, by calculating distance field along the walkable surface.
	if	(!rcBuildDistanceField(&ctx, *chf))
		__ERR__(__LOG_E__ << "Failed to build distance fields.\n", false)

	// Partition the walkable surface into simple regions without holes.
	if	(!rcBuildRegions(&ctx, *chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
		__ERR__(__LOG_E__ << "Failed to build regions.\n", false)

	// Create contours.
	rcContourSet	*cset = rcAllocContourSet();
	if	(!cset)
		__ERR__(__LOG_E__ << "Failed to allocate contour set.\n", false)
	if (!rcBuildContours(&ctx, *chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *cset))
		__ERR__(__LOG_E__ << "Failed to create contour set.\n", false)

	// Build polygon navmesh from the contours.
	rcPolyMesh		*pmesh = rcAllocPolyMesh();
	if	(!pmesh)
		__ERR__(__LOG_E__ << "Failed to allocate navmesh.\n", false)
	if (!rcBuildPolyMesh(&ctx, *cset, m_cfg.maxVertsPerPoly, *pmesh))
		__ERR__(__LOG_E__ << "Failed to build navmesh.\n", false)

	rcPolyMeshDetail *dmesh = rcAllocPolyMeshDetail();
	if	(!dmesh)
		__ERR__(__LOG_E__ << "Failed to allocate detail mesh.\n", false)
	if	(!rcBuildPolyMeshDetail(&ctx, *pmesh, *chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *dmesh))
		__ERR__(__LOG_E__ << "Failed to build detail mesh.\n", false)

	rcFreeCompactHeightfield(chf);
	chf = 0;
	rcFreeContourSet(cset);
	cset = 0;

	// The GUI may allow more max points per polygon than Detour can handle.
	// Only build the detour navmesh if we do not exceed the limit.
	if	(m_cfg.maxVertsPerPoly <= DT_VERTS_PER_POLYGON)
	{
		// Update poly flags from areas.
/*
		for (int i = 0; i < pmesh->npolys; ++i)
		{
			if (pmesh->areas[i] == RC_WALKABLE_AREA)
				pmesh->areas[i] = SAMPLE_POLYAREA_GROUND;
				
			if (pmesh->areas[i] == SAMPLE_POLYAREA_GROUND || pmesh->areas[i] == SAMPLE_POLYAREA_GRASS || pmesh->areas[i] == SAMPLE_POLYAREA_ROAD)
				pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK;

			else if	(pmesh->areas[i] == SAMPLE_POLYAREA_WATER)
				pmesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;

			else if	(pmesh->areas[i] == SAMPLE_POLYAREA_DOOR)
				pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
		}
*/
		dtNavMeshCreateParams params;
		Memory::Set(&params, 0, sizeof(params));
		params.verts = pmesh->verts;
		params.vertCount = pmesh->nverts;
		params.polys = pmesh->polys;
		params.polyAreas = pmesh->areas;
		params.polyFlags = pmesh->flags;
		params.polyCount = pmesh->npolys;
		params.nvp = pmesh->nvp;
		params.detailMeshes = dmesh->meshes;
		params.detailVerts = dmesh->verts;
		params.detailVertsCount = dmesh->nverts;
		params.detailTris = dmesh->tris;
		params.detailTriCount = dmesh->ntris;
/*
		params.offMeshConVerts = m_geom->getOffMeshConnectionVerts();
		params.offMeshConRad = m_geom->getOffMeshConnectionRads();
		params.offMeshConDir = m_geom->getOffMeshConnectionDirs();
		params.offMeshConAreas = m_geom->getOffMeshConnectionAreas();
		params.offMeshConFlags = m_geom->getOffMeshConnectionFlags();
		params.offMeshConUserID = m_geom->getOffMeshConnectionId();
		params.offMeshConCount = m_geom->getOffMeshConnectionCount();
*/
		params.walkableHeight = cfg.agent.height;
		params.walkableRadius = cfg.agent.radius;
		params.walkableClimb = cfg.agent.max_climb;
		rcVcopy(params.bmin, pmesh->bmin);
		rcVcopy(params.bmax, pmesh->bmax);
		params.cs = m_cfg.cs;
		params.ch = m_cfg.ch;
		params.buildBvTree = true;

		unsigned char *navData = 0;
		int		navDataSize = 0;
		if	(!dtCreateNavMeshData(&params, &navData, &navDataSize))
			__ERR__(__LOG_E__ << "Failed to create Detour navmesh.\n", false)
		
		dtNavMesh *navMesh = dtAllocNavMesh();
		if	(!navMesh)
		{
			dtFree(navData);
			__ERR__(__LOG_E__ << "Failed to build Detour navmesh.\n", false)
		}
		
		dtStatus	status = navMesh->init(navData, navDataSize, DT_TILE_FREE_DATA);
		if	(dtStatusFailed(status))
		{
			dtFree(navData);
			__ERR__(__LOG_E__ << "Failed to initialize Detour navmesh.\n", false)
		}
	/*
		status = navQuery->init(navMesh, 2048);
		if	(dtStatusFailed(status))
			__ERR__(__LOG_E__ << "Failed to initialize Detour navmesh query.\n", false)
	*/
	}
	return true;
}
//------------------------------------------------------------------------------