#include "TileCacheNavMesh.hpp"
#include "PartitionedMesh.hpp"
#include "fastlz.h"
#include <Recast.h>
#include <DetourCommon.h>
#include <DetourNavMesh.h>
#include <DetourNavMeshBuilder.h>
#include <DetourNavMeshQuery.h>
#include <DetourTileCache.h>
#include <DetourTileCacheBuilder.h>
#include <OgreLogManager.h>
#include <cstring>
#include <cfloat>
#include <algorithm>

static bool isNavMeshGeometry(const Ogre::String &meshName)
{
	Ogre::String lower = meshName;
	std::transform(lower.begin(), lower.end(), lower.begin(),
		       [](unsigned char c) { return std::tolower(c); });

	// Exclude geometry that doesn't affect floor-level navigation.
	// Ceilings are above walkable floors and don't block movement.
	// Furniture is handled separately (or excluded to keep navmesh
	// focused on architectural walkable surfaces).
	// Frames placed via StaticGeometry aren't traversed by the
	// SceneNode walker anyway.
	static const char *exclude[] = { "ceiling", "frame", "furniture" };
	for (const char *pat : exclude) {
		if (lower.find(pat) != Ogre::String::npos)
			return false;
	}
	return true;
}

// ------------------------------------------------------------------
// dtTileCache callbacks
// ------------------------------------------------------------------

struct LinearAllocator : public dtTileCacheAlloc {
	unsigned char *buffer = nullptr;
	size_t capacity = 0;
	size_t top = 0;
	size_t high = 0;

	LinearAllocator(size_t cap)
	{
		buffer = (unsigned char *)dtAlloc(cap, DT_ALLOC_PERM);
		capacity = cap;
	}
	~LinearAllocator()
	{
		dtFree(buffer);
	}

	void *alloc(const size_t size) override
	{
		if (!buffer)
			return nullptr;
		if (top + size > capacity)
			return nullptr;
		unsigned char *mem = &buffer[top];
		top += size;
		if (top > high)
			high = top;
		return mem;
	}

	void free(void *ptr) override
	{
		(void)ptr;
		// Linear allocator does not free individual allocations
	}

	void reset()
	{
		top = 0;
		high = 0;
	}
};

struct FastLZCompressor : public dtTileCacheCompressor {
	int maxCompressedSize(const int bufferSize) override
	{
		return (int)(bufferSize * 1.05f);
	}
	dtStatus compress(const unsigned char *buffer, const int bufferSize,
			  unsigned char *compressed,
			  const int maxCompressedSize,
			  int *compressedSize) override
	{
		*compressedSize = fastlz_compress((const void *)buffer,
						  bufferSize, compressed);
		return DT_SUCCESS;
	}
	dtStatus decompress(const unsigned char *compressed,
			    const int compressedSize, unsigned char *buffer,
			    const int maxBufferSize, int *bufferSize) override
	{
		*bufferSize = fastlz_decompress(compressed, compressedSize,
						buffer, maxBufferSize);
		return *bufferSize < 0 ? DT_FAILURE : DT_SUCCESS;
	}
};

static const unsigned char SAMPLE_POLYAREA_GROUND = 0;
static const unsigned short SAMPLE_POLYFLAGS_WALK = 0x01;

struct MeshProcess : public dtTileCacheMeshProcess {
	void process(struct dtNavMeshCreateParams *params,
		     unsigned char *polyAreas,
		     unsigned short *polyFlags) override
	{
		// Mark walkable ground
		for (int i = 0; i < params->polyCount; ++i) {
			if (polyAreas[i] == DT_TILECACHE_WALKABLE_AREA) {
				polyAreas[i] = SAMPLE_POLYAREA_GROUND;
				polyFlags[i] = SAMPLE_POLYFLAGS_WALK;
			}
		}
	}
};

// ------------------------------------------------------------------
// Tile cache data helper
// ------------------------------------------------------------------

static const int MAX_LAYERS = 32;

struct TileCacheData {
	unsigned char *data;
	int dataSize;
};

struct RasterizationContext {
	rcHeightfield *solid = nullptr;
	unsigned char *triareas = nullptr;
	rcCompactHeightfield *chf = nullptr;
	rcHeightfieldLayerSet *lset = nullptr;
	TileCacheData tiles[MAX_LAYERS];
	int ntiles = 0;

	~RasterizationContext()
	{
		rcFreeHeightField(solid);
		delete[] triareas;
		rcFreeCompactHeightfield(chf);
		rcFreeHeightfieldLayerSet(lset);
		for (int i = 0; i < ntiles; ++i) {
			dtFree(tiles[i].data);
		}
	}
};

// ------------------------------------------------------------------
// Ogre mesh extraction
// ------------------------------------------------------------------

static void extractEntityMesh(Ogre::Entity *ent, std::vector<float> &verts,
			      std::vector<int> &tris)
{
	Ogre::MeshPtr mesh = ent->getMesh();
	Ogre::SceneNode *node = ent->getParentSceneNode();
	Ogre::Matrix4 transform = node ? node->_getFullTransform() :
					 Ogre::Matrix4::IDENTITY;

	bool addedShared = false;
	size_t sharedOffset = 0;
	size_t currentOffset = 0;
	size_t nextOffset = 0;

	// Global offset of this entity's vertices in the output arrays.
	// Indices read from the mesh are relative to submesh/shared vertex
	// data; we must shift them so they point into the unified buffers.
	size_t baseVertex = verts.size() / 3;

	for (unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i) {
		Ogre::SubMesh *sub = mesh->getSubMesh(i);
		Ogre::VertexData *vd = sub->useSharedVertices ?
					       mesh->sharedVertexData :
					       sub->vertexData;

		if ((!sub->useSharedVertices) ||
		    (sub->useSharedVertices && !addedShared)) {
			if (sub->useSharedVertices) {
				addedShared = true;
				sharedOffset = currentOffset;
			}

			const Ogre::VertexElement *posElem =
				vd->vertexDeclaration->findElementBySemantic(
					Ogre::VES_POSITION);
			if (!posElem)
				continue;

			Ogre::HardwareVertexBufferSharedPtr vbuf =
				vd->vertexBufferBinding->getBuffer(
					posElem->getSource());

			unsigned char *vptr =
				static_cast<unsigned char *>(vbuf->lock(
					Ogre::HardwareBuffer::HBL_READ_ONLY));

			for (size_t v = 0; v < vd->vertexCount;
			     ++v, vptr += vbuf->getVertexSize()) {
				float *pReal;
				posElem->baseVertexPointerToElement(vptr,
								    &pReal);
				Ogre::Vector3 pt(pReal[0], pReal[1], pReal[2]);
				pt = transform * pt;
				verts.push_back(pt.x);
				verts.push_back(pt.y);
				verts.push_back(pt.z);
			}
			vbuf->unlock();
			nextOffset += vd->vertexCount;
		}

		Ogre::IndexData *id = sub->indexData;
		if (!id || id->indexCount == 0) {
			currentOffset = nextOffset;
			continue;
		}
		Ogre::HardwareIndexBufferSharedPtr ibuf = id->indexBuffer;
		if (!ibuf) {
			currentOffset = nextOffset;
			continue;
		}
		bool use32 = (ibuf->getType() ==
			      Ogre::HardwareIndexBuffer::IT_32BIT);

		void *idata = ibuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY);
		size_t offset = baseVertex +
				(sub->useSharedVertices ? sharedOffset :
								currentOffset);

		if (use32) {
			unsigned int *p = static_cast<unsigned int *>(idata);
			for (size_t k = 0; k < id->indexCount; ++k)
				tris.push_back(static_cast<int>(p[k]) +
					       static_cast<int>(offset));
		} else {
			unsigned short *p =
				static_cast<unsigned short *>(idata);
			for (size_t k = 0; k < id->indexCount; ++k)
				tris.push_back(static_cast<int>(p[k]) +
					       static_cast<int>(offset));
		}
		ibuf->unlock();

		currentOffset = nextOffset;
	}
}

// ------------------------------------------------------------------
// TileCacheNavMesh
// ------------------------------------------------------------------

TileCacheNavMesh::TileCacheNavMesh(Ogre::SceneManager *sceneMgr,
				   const BuildParams &params)
	: m_sceneMgr(sceneMgr)
	, m_params(params)
	, m_talloc(nullptr)
	, m_tcomp(nullptr)
	, m_tmproc(nullptr)
	, m_tileCache(nullptr)
	, m_navMesh(nullptr)
	, m_navQuery(nullptr)
	, m_filter(nullptr)
	, m_debugMO(nullptr)
	, m_debugNode(nullptr)
	, m_ctx(nullptr)
{
	m_ctx = new rcContext(true);
	m_filter = new dtQueryFilter();
	m_filter->setIncludeFlags(0xFFFF);
	m_filter->setExcludeFlags(0);
	m_filter->setAreaCost(SAMPLE_POLYAREA_GROUND, 1.0f);

	m_extents[0] = 32.0f;
	m_extents[1] = 32.0f;
	m_extents[2] = 32.0f;
}

TileCacheNavMesh::~TileCacheNavMesh()
{
	clearDebugDraw();
	dtFreeTileCache(m_tileCache);
	dtFreeNavMesh(m_navMesh);
	dtFreeNavMeshQuery(m_navQuery);
	delete m_filter;
	delete m_ctx;
	delete m_talloc;
	delete m_tcomp;
	delete m_tmproc;
}

bool TileCacheNavMesh::extractMeshData(
	const std::vector<Ogre::Entity *> &entities)
{
	m_verts.clear();
	m_tris.clear();
	m_partitionedMesh.reset();

	int processed = 0;
	for (auto *ent : entities) {
		if (!ent)
			continue;
		if (!isNavMeshGeometry(ent->getMesh()->getName())) {
			Ogre::LogManager::getSingleton().logMessage(
				"[NavMesh] Skipping " + ent->getName() +
				" mesh=" + ent->getMesh()->getName());
			continue;
		}
		int vbefore = static_cast<int>(m_verts.size() / 3);
		int tbefore = static_cast<int>(m_tris.size() / 3);
		extractEntityMesh(ent, m_verts, m_tris);
		int vafter = static_cast<int>(m_verts.size() / 3);
		int tafter = static_cast<int>(m_tris.size() / 3);
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] Entity " + ent->getName() + " verts=" +
			Ogre::StringConverter::toString(vafter - vbefore) +
			" tris=" +
			Ogre::StringConverter::toString(tafter - tbefore));
		processed++;
	}

	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] Processed " +
		Ogre::StringConverter::toString(processed) +
		" entities, total verts=" +
		Ogre::StringConverter::toString(
			static_cast<int>(m_verts.size() / 3)) +
		" tris=" +
		Ogre::StringConverter::toString(
			static_cast<int>(m_tris.size() / 3)));

	if (m_verts.empty() || m_tris.empty())
		return false;

	int nverts = static_cast<int>(m_verts.size() / 3);
	int ntris = static_cast<int>(m_tris.size() / 3);

	// Compute bounds
	m_bmin[0] = m_bmin[1] = m_bmin[2] = FLT_MAX;
	m_bmax[0] = m_bmax[1] = m_bmax[2] = -FLT_MAX;
	for (int i = 0; i < nverts; ++i) {
		float x = m_verts[i * 3 + 0];
		float y = m_verts[i * 3 + 1];
		float z = m_verts[i * 3 + 2];
		if (x < m_bmin[0])
			m_bmin[0] = x;
		if (y < m_bmin[1])
			m_bmin[1] = y;
		if (z < m_bmin[2])
			m_bmin[2] = z;
		if (x > m_bmax[0])
			m_bmax[0] = x;
		if (y > m_bmax[1])
			m_bmax[1] = y;
		if (z > m_bmax[2])
			m_bmax[2] = z;
	}

	// Build spatial partitioning
	m_partitionedMesh = std::make_unique<PartitionedMesh>();
	m_partitionedMesh->PartitionMesh(m_verts.data(), m_tris.data(), ntris,
					 256);

	return true;
}

bool TileCacheNavMesh::initTileCache()
{
	m_talloc = new LinearAllocator(32 * 1024 * 1024);
	m_tcomp = new FastLZCompressor();
	m_tmproc = new MeshProcess();

	// Calculate grid size
	m_cellSize = m_params.cellSize;
	float ts = m_params.tileSize * m_cellSize;
	m_tw = (int)ceilf((m_bmax[0] - m_bmin[0]) / ts);
	m_th = (int)ceilf((m_bmax[2] - m_bmin[2]) / ts);
	if (m_tw < 1)
		m_tw = 1;
	if (m_th < 1)
		m_th = 1;

	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] Grid: " + Ogre::StringConverter::toString(m_tw) +
		"x" + Ogre::StringConverter::toString(m_th) + " bounds=[" +
		Ogre::StringConverter::toString(m_bmin[0]) + "," +
		Ogre::StringConverter::toString(m_bmin[1]) + "," +
		Ogre::StringConverter::toString(m_bmin[2]) + "]-[" +
		Ogre::StringConverter::toString(m_bmax[0]) + "," +
		Ogre::StringConverter::toString(m_bmax[1]) + "," +
		Ogre::StringConverter::toString(m_bmax[2]) + "]");

	// Tile cache params
	dtTileCacheParams tcparams;
	memset(&tcparams, 0, sizeof(tcparams));
	rcVcopy(tcparams.orig, m_bmin);
	tcparams.cs = m_params.cellSize;
	tcparams.ch = m_params.cellHeight;
	tcparams.width = m_params.tileSize;
	tcparams.height = m_params.tileSize;
	tcparams.walkableHeight = m_params.agentHeight;
	tcparams.walkableRadius = m_params.agentRadius;
	tcparams.walkableClimb = m_params.agentMaxClimb;
	tcparams.maxSimplificationError = m_params.edgeMaxError;
	tcparams.maxTiles = m_tw * m_th * MAX_LAYERS;
	tcparams.maxObstacles = 128;

	m_tileCache = dtAllocTileCache();
	if (!m_tileCache) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] initTileCache failed: dtAllocTileCache");
		return false;
	}

	dtStatus status =
		m_tileCache->init(&tcparams, m_talloc, m_tcomp, m_tmproc);
	if (dtStatusFailed(status)) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] initTileCache failed: tileCache init");
		return false;
	}

	// Navmesh params
	// Cap tileBits at 14 and derive polyBits so that
	// tileBits + polyBits <= 22, leaving >= 10 salt bits.
	// This matches the upstream RecastDemo calculation.
	int tileBits = (int)dtIlog2(
		dtNextPow2((unsigned int)(m_tw * m_th * MAX_LAYERS)));
	if (tileBits > 14)
		tileBits = 14;
	int polyBits = 22 - tileBits;

	dtNavMeshParams nmparams;
	memset(&nmparams, 0, sizeof(nmparams));
	rcVcopy(nmparams.orig, m_bmin);
	nmparams.tileWidth = m_params.tileSize * m_params.cellSize;
	nmparams.tileHeight = m_params.tileSize * m_params.cellSize;
	nmparams.maxTiles = 1 << tileBits;
	nmparams.maxPolys = 1 << polyBits;

	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] tileBits=" +
		Ogre::StringConverter::toString(tileBits) + " polyBits=" +
		Ogre::StringConverter::toString(polyBits) + " maxTiles=" +
		Ogre::StringConverter::toString(nmparams.maxTiles) +
		" maxPolys=" +
		Ogre::StringConverter::toString(nmparams.maxPolys));

	m_navMesh = dtAllocNavMesh();
	if (!m_navMesh) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] initTileCache failed: dtAllocNavMesh");
		return false;
	}

	status = m_navMesh->init(&nmparams);
	if (dtStatusFailed(status)) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] initTileCache failed: navMesh init");
		return false;
	}

	m_navQuery = dtAllocNavMeshQuery();
	status = m_navQuery->init(m_navMesh, 2048);
	if (dtStatusFailed(status)) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] initTileCache failed: navQuery init");
		return false;
	}

	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] initTileCache success");
	return true;
}

bool TileCacheNavMesh::build(const std::vector<Ogre::Entity *> &entities)
{
	if (!extractMeshData(entities))
		return false;

	if (!initTileCache())
		return false;

	// Build all tiles
	int builtTiles = 0;
	for (int y = 0; y < m_th; ++y) {
		for (int x = 0; x < m_tw; ++x) {
			if (buildTile(x, y))
				builtTiles++;
		}
	}

	int tilesWithLayers = 0;
	int totalPolys = 0;
	const dtNavMesh *navMesh = m_navMesh;
	for (int i = 0; i < navMesh->getMaxTiles(); ++i) {
		const dtMeshTile *tile = navMesh->getTile(i);
		if (tile && tile->header) {
			tilesWithLayers++;
			totalPolys += tile->header->polyCount;
		}
	}

	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] Built " +
		Ogre::StringConverter::toString(builtTiles) + "/" +
		Ogre::StringConverter::toString(m_tw * m_th) + " tiles, " +
		Ogre::StringConverter::toString(tilesWithLayers) +
		" with layers, " + Ogre::StringConverter::toString(totalPolys) +
		" polygons");

	return true;
}

int TileCacheNavMesh::rasterizeTileLayers(int tx, int ty, TileCacheData *tiles,
					  int maxTiles)
{
	if (!m_partitionedMesh)
		return 0;

	FastLZCompressor comp;
	RasterizationContext rc;

	const float *verts = m_verts.data();
	const int nverts = static_cast<int>(m_verts.size() / 3);

	const float tcs = m_params.tileSize * m_params.cellSize;

	rcConfig tcfg;
	memset(&tcfg, 0, sizeof(tcfg));
	tcfg.cs = m_params.cellSize;
	tcfg.ch = m_params.cellHeight;
	tcfg.walkableSlopeAngle = m_params.agentMaxSlope;
	tcfg.walkableHeight = (int)ceilf(m_params.agentHeight / tcfg.ch);
	tcfg.walkableClimb = (int)floorf(m_params.agentMaxClimb / tcfg.ch);
	tcfg.walkableRadius = (int)ceilf(m_params.agentRadius / tcfg.cs);
	tcfg.maxEdgeLen = (int)(m_params.edgeMaxLen / tcfg.cs);
	tcfg.maxSimplificationError = m_params.edgeMaxError;
	tcfg.minRegionArea = (int)rcSqr(m_params.regionMinSize);
	tcfg.mergeRegionArea = (int)rcSqr(m_params.regionMergeSize);
	tcfg.maxVertsPerPoly = m_params.vertsPerPoly;
	tcfg.tileSize = m_params.tileSize;
	tcfg.borderSize = tcfg.walkableRadius + 3;
	tcfg.width = tcfg.tileSize + tcfg.borderSize * 2;
	tcfg.height = tcfg.tileSize + tcfg.borderSize * 2;
	tcfg.detailSampleDist = tcfg.cs * m_params.detailSampleDist;
	tcfg.detailSampleMaxError = tcfg.ch * m_params.detailSampleMaxError;

	tcfg.bmin[0] = m_bmin[0] + tx * tcs;
	tcfg.bmin[1] = m_bmin[1];
	tcfg.bmin[2] = m_bmin[2] + ty * tcs;
	tcfg.bmax[0] = m_bmin[0] + (tx + 1) * tcs;
	tcfg.bmax[1] = m_bmax[1];
	tcfg.bmax[2] = m_bmin[2] + (ty + 1) * tcs;
	tcfg.bmin[0] -= tcfg.borderSize * tcfg.cs;
	tcfg.bmin[2] -= tcfg.borderSize * tcfg.cs;
	tcfg.bmax[0] += tcfg.borderSize * tcfg.cs;
	tcfg.bmax[2] += tcfg.borderSize * tcfg.cs;

	// Allocate heightfield
	rc.solid = rcAllocHeightfield();
	if (!rc.solid ||
	    !rcCreateHeightfield(m_ctx, *rc.solid, tcfg.width, tcfg.height,
				 tcfg.bmin, tcfg.bmax, tcfg.cs, tcfg.ch))
		return 0;

	rc.triareas = new unsigned char[m_partitionedMesh->maxTrisPerChunk];
	if (!rc.triareas)
		return 0;

	float tbmin[2], tbmax[2];
	tbmin[0] = tcfg.bmin[0];
	tbmin[1] = tcfg.bmin[2];
	tbmax[0] = tcfg.bmax[0];
	tbmax[1] = tcfg.bmax[2];

	std::vector<int> overlappingNodes;
	m_partitionedMesh->GetNodesOverlappingRect(tbmin, tbmax,
						   overlappingNodes);
	if (overlappingNodes.empty())
		return 0;

	for (int nodeIndex : overlappingNodes) {
		const PartitionedMesh::Node &node =
			m_partitionedMesh->nodes[nodeIndex];
		const int *tris = &m_partitionedMesh->tris[node.triIndex * 3];
		const int ntris = node.numTris;

		memset(rc.triareas, 0, ntris * sizeof(unsigned char));
		rcMarkWalkableTriangles(m_ctx, tcfg.walkableSlopeAngle, verts,
					nverts, tris, ntris, rc.triareas);

		// After rcMarkWalkableTriangles has set area based on slope,
		// additionally clear triangles whose normal faces downward
		// (negative Y in world space).  Some meshes (e.g. Lot base
		// boxes) have bottom faces wound the same way as top faces,
		// causing rcMarkWalkableTriangles to compute an upward-pointing
		// normal and mark them as walkable.  We detect this by
		// computing the world-space normal and forcing RC_NULL_AREA.
		for (int i = 0; i < ntris; ++i) {
			const int *tri = &tris[i * 3];
			const float *v0 = &verts[tri[0] * 3];
			const float *v1 = &verts[tri[1] * 3];
			const float *v2 = &verts[tri[2] * 3];
			float ux = v1[0] - v0[0];
			float uy = v1[1] - v0[1];
			float uz = v1[2] - v0[2];
			float vx = v2[0] - v0[0];
			float vy = v2[1] - v0[1];
			float vz = v2[2] - v0[2];
			float ny = uz * vx - ux * vz;
			if (ny < -1e-6f)
				rc.triareas[i] = RC_NULL_AREA;
		}

		rcRasterizeTriangles(m_ctx, verts, nverts, tris, rc.triareas,
				     ntris, *rc.solid, tcfg.walkableClimb);
	}

	rcFilterLowHangingWalkableObstacles(m_ctx, tcfg.walkableClimb,
					    *rc.solid);
	rcFilterLedgeSpans(m_ctx, tcfg.walkableHeight, tcfg.walkableClimb,
			   *rc.solid);
	rcFilterWalkableLowHeightSpans(m_ctx, tcfg.walkableHeight, *rc.solid);

	rc.chf = rcAllocCompactHeightfield();
	if (!rc.chf ||
	    !rcBuildCompactHeightfield(m_ctx, tcfg.walkableHeight,
				       tcfg.walkableClimb, *rc.solid, *rc.chf))
		return 0;

	rcFreeHeightField(rc.solid);
	rc.solid = nullptr;

	if (!rcErodeWalkableArea(m_ctx, tcfg.walkableRadius, *rc.chf))
		return 0;

	if (!rcBuildDistanceField(m_ctx, *rc.chf))
		return 0;

	if (!rcBuildRegions(m_ctx, *rc.chf, tcfg.borderSize, tcfg.minRegionArea,
			    tcfg.mergeRegionArea))
		return 0;

	rc.lset = rcAllocHeightfieldLayerSet();
	if (!rc.lset ||
	    !rcBuildHeightfieldLayers(m_ctx, *rc.chf, tcfg.borderSize,
				      tcfg.walkableHeight, *rc.lset))
		return 0;

	for (int i = 0; i < rcMin(rc.lset->nlayers, maxTiles); ++i) {
		TileCacheData *tile = &rc.tiles[rc.ntiles++];
		const rcHeightfieldLayer *layer = &rc.lset->layers[i];

		dtTileCacheLayerHeader header;
		header.magic = DT_TILECACHE_MAGIC;
		header.version = DT_TILECACHE_VERSION;
		header.tx = tx;
		header.ty = ty;
		header.tlayer = i;
		rcVcopy(header.bmin, layer->bmin);
		rcVcopy(header.bmax, layer->bmax);
		header.width = (unsigned char)layer->width;
		header.height = (unsigned char)layer->height;
		header.minx = (unsigned char)layer->minx;
		header.maxx = (unsigned char)layer->maxx;
		header.miny = (unsigned char)layer->miny;
		header.maxy = (unsigned char)layer->maxy;
		header.hmin = (unsigned short)layer->hmin;
		header.hmax = (unsigned short)layer->hmax;

		dtStatus status = dtBuildTileCacheLayer(
			&comp, &header, layer->heights, layer->areas,
			layer->cons, &tile->data, &tile->dataSize);
		if (dtStatusFailed(status))
			return 0;
	}

	int n = 0;
	for (int i = 0; i < rcMin(rc.ntiles, maxTiles); ++i) {
		tiles[n++] = rc.tiles[i];
		rc.tiles[i].data = nullptr;
		rc.tiles[i].dataSize = 0;
	}

	return n;
}

bool TileCacheNavMesh::buildTile(int tx, int ty)
{
	if (tx < 0 || tx >= m_tw || ty < 0 || ty >= m_th)
		return false;

	removeTile(tx, ty);

	TileCacheData tiles[MAX_LAYERS];
	int ntiles = rasterizeTileLayers(tx, ty, tiles, MAX_LAYERS);

	for (int i = 0; i < ntiles; ++i) {
		dtStatus status =
			m_tileCache->addTile(tiles[i].data, tiles[i].dataSize,
					     DT_COMPRESSEDTILE_FREE_DATA, 0);
		if (dtStatusFailed(status)) {
			dtFree(tiles[i].data);
			tiles[i].data = nullptr;
		}
	}

	m_tileCache->buildNavMeshTilesAt(tx, ty, m_navMesh);
	return true;
}

bool TileCacheNavMesh::removeTile(int tx, int ty)
{
	if (!m_tileCache)
		return false;

	dtCompressedTileRef refs[MAX_LAYERS];
	int nrefs = m_tileCache->getTilesAt(tx, ty, refs, MAX_LAYERS);

	for (int i = 0; i < nrefs; ++i) {
		unsigned char *data = nullptr;
		int dataSize = 0;
		dtCompressedTileRef ref = refs[i];
		m_tileCache->removeTile(ref, &data, &dataSize);
		if (data)
			dtFree(data);
	}

	return true;
}

void TileCacheNavMesh::rebuildTilesInArea(const Ogre::AxisAlignedBox &area)
{
	if (!m_tileCache)
		return;

	float amin[3] = { area.getMinimum().x, area.getMinimum().y,
			  area.getMinimum().z };
	float amax[3] = { area.getMaximum().x, area.getMaximum().y,
			  area.getMaximum().z };

	// Expand by border size
	float border = m_params.agentRadius + m_params.cellSize * 3;
	amin[0] -= border;
	amin[2] -= border;
	amax[0] += border;
	amax[2] += border;

	int minTx, minTy, maxTx, maxTy;
	getTileCoords(amin, minTx, minTy);
	getTileCoords(amax, maxTx, maxTy);

	for (int ty = minTy; ty <= maxTy; ++ty) {
		for (int tx = minTx; tx <= maxTx; ++tx) {
			buildTile(tx, ty);
		}
	}
}

void TileCacheNavMesh::getTileCoords(const float *pos, int &tx, int &ty)
{
	float ts = m_params.tileSize * m_cellSize;
	tx = (int)((pos[0] - m_bmin[0]) / ts);
	ty = (int)((pos[2] - m_bmin[2]) / ts);
	if (tx < 0)
		tx = 0;
	if (ty < 0)
		ty = 0;
	if (tx >= m_tw)
		tx = m_tw - 1;
	if (ty >= m_th)
		ty = m_th - 1;
}

// ------------------------------------------------------------------
// Queries
// ------------------------------------------------------------------

bool TileCacheNavMesh::findPath(const Ogre::Vector3 &start,
				const Ogre::Vector3 &end,
				std::vector<Ogre::Vector3> &path)
{
	path.clear();
	if (!m_navQuery)
		return false;

	float s[3] = { start.x, start.y, start.z };
	float e[3] = { end.x, end.y, end.z };

	dtPolyRef startPoly, endPoly;
	float startNearest[3], endNearest[3];

	dtStatus status = m_navQuery->findNearestPoly(s, m_extents, m_filter,
						      &startPoly, startNearest);
	if (dtStatusFailed(status))
		return false;

	status = m_navQuery->findNearestPoly(e, m_extents, m_filter, &endPoly,
					     endNearest);
	if (dtStatusFailed(status))
		return false;

	dtPolyRef polyPath[256];
	int npoly = 0;
	status = m_navQuery->findPath(startPoly, endPoly, startNearest,
				      endNearest, m_filter, polyPath, &npoly,
				      256);
	if (dtStatusFailed(status) || npoly == 0)
		return false;

	float straightPath[256 * 3];
	unsigned char straightPathFlags[256];
	dtPolyRef straightPathPolys[256];
	int nvert = 0;
	status = m_navQuery->findStraightPath(startNearest, endNearest,
					      polyPath, npoly, straightPath,
					      straightPathFlags,
					      straightPathPolys, &nvert, 256);
	if (dtStatusFailed(status) || nvert == 0)
		return false;

	path.reserve(nvert);
	for (int i = 0; i < nvert; ++i) {
		path.emplace_back(straightPath[i * 3 + 0],
				  straightPath[i * 3 + 1],
				  straightPath[i * 3 + 2]);
	}
	return true;
}

bool TileCacheNavMesh::findNearestPoint(const Ogre::Vector3 &pos,
					Ogre::Vector3 &out)
{
	if (!m_navQuery)
		return false;

	float p[3] = { pos.x, pos.y, pos.z };
	dtPolyRef poly;
	float nearest[3];

	dtStatus status = m_navQuery->findNearestPoly(p, m_extents, m_filter,
						      &poly, nearest);
	if (dtStatusFailed(status))
		return false;

	float h = 0;
	status = m_navQuery->getPolyHeight(poly, nearest, &h);
	if (dtStatusFailed(status))
		return false;

	out = Ogre::Vector3(nearest[0], h, nearest[2]);
	return true;
}

Ogre::Vector3 TileCacheNavMesh::getRandomPoint()
{
	if (!m_navQuery)
		return Ogre::Vector3::ZERO;

	dtPolyRef poly;
	float pt[3];
	dtStatus status = m_navQuery->findRandomPoint(
		m_filter,
		[]() -> float { return (float)rand() / (float)RAND_MAX; },
		&poly, pt);
	if (dtStatusFailed(status))
		return Ogre::Vector3::ZERO;

	return Ogre::Vector3(pt[0], pt[1], pt[2]);
}

// ------------------------------------------------------------------
// Debug draw
// ------------------------------------------------------------------

void TileCacheNavMesh::drawNavMesh()
{
	clearDebugDraw();
	if (!m_navMesh) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] drawNavMesh: no navmesh");
		return;
	}

	static int debugId = 0;
	Ogre::String moName =
		"NavMeshDebug_" + Ogre::StringConverter::toString(debugId++);
	Ogre::String nodeName = "NavMeshDebugNode_" +
				Ogre::StringConverter::toString(debugId++);

	m_debugMO = m_sceneMgr->createManualObject(moName);
	m_debugMO->setDynamic(false);
	m_debugMO->setCastShadows(false);

	Ogre::ColourValue groundCol(0.0f, 0.7f, 0.0f, 1.0f);
	Ogre::ColourValue edgeCol(1.0f, 0.0f, 0.0f, 1.0f);

	Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().getByName(
		"BaseWhiteNoLighting");
	if (!mat) {
		Ogre::LogManager::getSingleton().logMessage(
			"[NavMesh] BaseWhiteNoLighting not found, creating fallback");
		mat = Ogre::MaterialManager::getSingleton().create(
			"NavMeshDebugMat",
			Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
		mat->getTechnique(0)->getPass(0)->setLightingEnabled(false);
		mat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
		mat->getTechnique(0)->getPass(0)->setSceneBlending(
			Ogre::SBT_TRANSPARENT_ALPHA);
	}

	m_debugMO->begin("BaseWhiteNoLighting",
			 Ogre::RenderOperation::OT_TRIANGLE_LIST);

	const dtNavMesh *navMesh = m_navMesh;
	for (int i = 0; i < navMesh->getMaxTiles(); ++i) {
		const dtMeshTile *tile = navMesh->getTile(i);
		if (!tile->header)
			continue;

		const float *verts = tile->verts;
		for (int j = 0; j < tile->header->polyCount; ++j) {
			const dtPoly *p = &tile->polys[j];
			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
				continue;

			// Fan-triangulate
			for (int k = 2; k < p->vertCount; ++k) {
				const float *v0 = &verts[p->verts[0] * 3];
				const float *v1 = &verts[p->verts[k - 1] * 3];
				const float *v2 = &verts[p->verts[k] * 3];
				m_debugMO->position(v0[0], v0[1] + 0.05f,
						    v0[2]);
				m_debugMO->colour(groundCol);
				m_debugMO->position(v1[0], v1[1] + 0.05f,
						    v1[2]);
				m_debugMO->colour(groundCol);
				m_debugMO->position(v2[0], v2[1] + 0.05f,
						    v2[2]);
				m_debugMO->colour(groundCol);
			}
		}
	}

	m_debugMO->end();

	// Edge lines
	m_debugMO->begin("BaseWhiteNoLighting",
			 Ogre::RenderOperation::OT_LINE_LIST);
	const dtNavMesh *navMesh2 = m_navMesh;
	for (int i = 0; i < navMesh2->getMaxTiles(); ++i) {
		const dtMeshTile *tile = navMesh2->getTile(i);
		if (!tile->header)
			continue;

		const float *verts = tile->verts;
		for (int j = 0; j < tile->header->polyCount; ++j) {
			const dtPoly *p = &tile->polys[j];
			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
				continue;

			for (int k = 0, k2 = p->vertCount - 1; k < p->vertCount;
			     k2 = k++) {
				unsigned short v0i = p->verts[k2];
				unsigned short v1i = p->verts[k];
				if (p->neis[k2] != 0)
					continue; // internal edge

				const float *v0 = &verts[v0i * 3];
				const float *v1 = &verts[v1i * 3];
				m_debugMO->position(v0[0], v0[1] + 0.08f,
						    v0[2]);
				m_debugMO->colour(edgeCol);
				m_debugMO->position(v1[0], v1[1] + 0.08f,
						    v1[2]);
				m_debugMO->colour(edgeCol);
			}
		}
	}
	m_debugMO->end();

	m_debugNode =
		m_sceneMgr->getRootSceneNode()->createChildSceneNode(nodeName);
	m_debugNode->attachObject(m_debugMO);

	// Count total polygons for diagnostics
	int polyCount = 0;
	for (int i = 0; i < navMesh->getMaxTiles(); ++i) {
		const dtMeshTile *tile = navMesh->getTile(i);
		if (tile->header)
			polyCount += tile->header->polyCount;
	}
	Ogre::LogManager::getSingleton().logMessage(
		"[NavMesh] drawNavMesh: created " + moName + " with " +
		Ogre::StringConverter::toString(m_debugMO->getNumSections()) +
		" sections, " + Ogre::StringConverter::toString(polyCount) +
		" polygons");
}

void TileCacheNavMesh::clearDebugDraw()
{
	if (m_debugMO) {
		if (m_debugNode) {
			m_debugNode->detachObject(m_debugMO);
			m_sceneMgr->destroySceneNode(m_debugNode);
			m_debugNode = nullptr;
		}
		m_sceneMgr->destroyManualObject(m_debugMO);
		m_debugMO = nullptr;
	}
}
