/// @par
///
/// The agent's position will be constrained to the surface of the navigation mesh.
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
{
	// Find empty slot.
	int idx = -1;
	for (int i = 0; i < m_maxAgents; ++i)
	{
		if (!m_agents[i].active)
		{
			idx = i;
			break;
		}
	}
	if (idx == -1)
		return -1;
	
	dtCrowdAgent* ag = &m_agents[idx];		

	updateAgentParameters(idx, params);
	
	// Find nearest position on navmesh and place the agent there.
	float nearest[3];
	dtPolyRef ref = 0;
	dtVcopy(nearest, pos);
	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
	if (dtStatusFailed(status))
	{
		dtVcopy(nearest, pos);
		ref = 0;
	}
	
	ag->corridor.reset(ref, nearest);
	ag->boundary.reset();
	ag->partial = false;

	ag->topologyOptTime = 0;
	ag->targetReplanTime = 0;
	ag->nneis = 0;
	
	dtVset(ag->dvel, 0,0,0);
	dtVset(ag->nvel, 0,0,0);
	dtVset(ag->vel, 0,0,0);
	dtVcopy(ag->npos, nearest);
	
	ag->desiredSpeed = 0;

	if (ref)
		ag->state = DT_CROWDAGENT_STATE_WALKING;
	else
		ag->state = DT_CROWDAGENT_STATE_INVALID;
	
	ag->targetState = DT_CROWDAGENT_TARGET_NONE;
	
	ag->active = true;

    // Clockwork: added to fix illegal memory access when ncorners is queried before the agent has updated
    ag->ncorners = 0;

	return idx;
}

bool OgreDetourTileCache::buildTile(const int tx, const int ty, InputGeom *inputGeom)
{
    if (tx < 0 || tx >= m_tw)
        return false;

    if (ty < 0 || ty >= m_th)
        return false;

//TODO maybe I want to keep these values up to date
    /*
    m_cacheLayerCount = 0;
    m_cacheCompressedSize = 0;
    m_cacheRawSize = 0;
    */

    TileCacheData tiles[MAX_LAYERS];
    memset(tiles, 0, sizeof(tiles));
    int ntiles = rasterizeTileLayers(inputGeom, tx, ty, m_cfg, tiles, MAX_LAYERS);  // This is where the tile is built

    dtStatus status;

    // I don't know exactly why this can still be multiple tiles (??)
    for (int i = 0; i < ntiles; ++i)
    {
        TileCacheData* tile = &tiles[i];

        dtTileCacheLayerHeader* header = (dtTileCacheLayerHeader*)tile->data;
        // Important: if a tile already exists at this position, first remove the old one or it will not be updated!
        removeTile( m_tileCache->getTileRef(m_tileCache->getTileAt(header->tx, header->ty,header->tlayer)) );
        status = m_tileCache->addTile(tile->data, tile->dataSize, DT_COMPRESSEDTILE_FREE_DATA, 0);  // Add compressed tiles to tileCache
        if (dtStatusFailed(status))
        {
            dtFree(tile->data);
            tile->data = 0;
            continue;       // TODO maybe return false here?
        }

// TODO this has to be recalculated differently when rebuilding a tile
        /*
        m_cacheLayerCount++;
        m_cacheCompressedSize += tile->dataSize;
        m_cacheRawSize += calcLayerBufferSize(m_tcparams.width, m_tcparams.height);
        */
    }

//TODO add a deferred command for this?
    // Build navmesh tile from cached tile
    m_tileCache->buildNavMeshTilesAt(tx,ty, m_recast->m_navMesh); // This immediately builds the tile, without the need of a dtTileCache::update()

//TODO update this value?
    //m_cacheBuildMemUsage = m_talloc->high;


// TODO extract debug drawing to a separate class
    drawDetail(tx, ty);


    return true;
}

void dtPathQueue::update(const int maxIters)
{
	static const int MAX_KEEP_ALIVE = 2; // in update ticks.

	// Update path request until there is nothing to update
	// or upto maxIters pathfinder iterations has been consumed.
	int iterCount = maxIters;
	
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		PathQuery& q = m_queue[m_queueHead % MAX_QUEUE];
		
		// Skip inactive requests.
		if (q.ref == DT_PATHQ_INVALID)
		{
			m_queueHead++;
			continue;
		}
		
		// Handle completed request.
		if (dtStatusSucceed(q.status) || dtStatusFailed(q.status))
		{
			// If the path result has not been read in few frames, free the slot.
			q.keepAlive++;
			if (q.keepAlive > MAX_KEEP_ALIVE)
			{
				q.ref = DT_PATHQ_INVALID;
				q.status = 0;
			}
			
			m_queueHead++;
			continue;
		}

		m_navquery->updateLinkFilter(q.linkFilter.Get());

		// Handle query start.
		if (q.status == 0)
		{
			q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter);
		}		
		// Handle query in progress.
		if (dtStatusInProgress(q.status))
		{
			int iters = 0;
			q.status = m_navquery->updateSlicedFindPath(iterCount, &iters);
			iterCount -= iters;
		}
		if (dtStatusSucceed(q.status))
		{
			q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize);
		}

		if (iterCount <= 0)
			break;

		m_queueHead++;
	}
}

bool NavPath::visitNext()
{
   U32 s = mVisitPoints.size();
   if(s < 2)
      return false;

   // Current leg of journey.
   Point3F &start = mVisitPoints[s-1];
   Point3F &end = mVisitPoints[s-2];

   // Drop to height of statics.
   RayInfo info;
   if(getContainer()->castRay(start, start - Point3F(0, 0, mMesh->mWalkableHeight * 2.0f), StaticObjectType, &info))
      start = info.point;
   if(getContainer()->castRay(end + Point3F(0, 0, 0.1f), end - Point3F(0, 0, mMesh->mWalkableHeight * 2.0f), StaticObjectType, &info))
      end = info.point;

   // Convert to Detour-friendly coordinates and data structures.
   F32 from[] = {start.x, start.z, -start.y};
   F32 to[] =   {end.x,   end.z,   -end.y};
   F32 extx = mMesh->mWalkableRadius * 4.0f;
   F32 extz = mMesh->mWalkableHeight;
   F32 extents[] = {extx, extz, extx};
   dtPolyRef startRef, endRef;

   if(dtStatusFailed(mQuery->findNearestPoly(from, extents, &mFilter, &startRef, NULL)) || !startRef)
   {
      Con::errorf("No NavMesh polygon near visit point (%g, %g, %g) of NavPath %s",
         start.x, start.y, start.z, getIdString());
      return false;
   }

   if(dtStatusFailed(mQuery->findNearestPoly(to, extents, &mFilter, &endRef, NULL)) || !endRef)
   {
      Con::errorf("No NavMesh polygon near visit point (%g, %g, %g) of NavPath %s",
         end.x, end.y, end.z, getIdString());
      return false;
   }

   // Init sliced pathfind.
   mStatus = mQuery->initSlicedFindPath(startRef, endRef, from, to, &mFilter);
   if(dtStatusFailed(mStatus))
      return false;

   return true;
}

bool nav_mesh::get_random_pos(const float* center, float radius, float* pos)
{
	dtPolyRef centerRef = 0;
	float polyPickExt[3] = { 2, 4, 2 };

	dtStatus status = query_->findNearestPoly(center, polyPickExt, &filter_, &centerRef, nullptr);
	if (dtStatusFailed(status))
	{
		return false;
	}

	dtPolyRef posRef = 0;

	status = query_->findRandomPointAroundCircle(centerRef, center, radius, &filter_, frand, &posRef, pos);
	if (dtStatusFailed(status))
	{
		return false;
	}

	return true;
}

bool BotFindNearestPoly( Bot_t *bot, rVec coord, dtPolyRef *nearestPoly, rVec &nearPoint )
{
	rVec start( coord );
	rVec extents( 640, 96, 640 );
	dtStatus status;
	dtNavMeshQuery* navQuery = bot->nav->query;
	dtQueryFilter* navFilter = &bot->nav->filter;

	status = navQuery->findNearestPoly( start, extents, navFilter, nearestPoly, nearPoint );
	if ( dtStatusFailed( status ) || *nearestPoly == 0 )
	{
		//try larger extents
		extents[ 1 ] += 900;
		status = navQuery->findNearestPoly( start, extents, navFilter, nearestPoly, nearPoint );
		if ( dtStatusFailed( status ) || *nearestPoly == 0 )
		{
			return false; // failed
		}
	}
	return true;
}

bool nav_mesh::get_height(dtPolyRef posRef, float* pos)
{
	float h = 0;
	dtStatus status = query_->getPolyHeight(posRef, pos, &h);
	if (dtStatusFailed(status))
	{
		return false;
	}

	pos[1] = h;
	return true;
}

bool NavPath::init()
{
   mStatus = DT_FAILURE;

   // Check that all the right data is provided.
   if(!mMesh || !mMesh->getNavMesh())
      return false;
   if(!(mFromSet && mToSet) && !(mWaypoints && mWaypoints->size()))
      return false;

   // Initialise our query.
   if(dtStatusFailed(mQuery->init(mMesh->getNavMesh(), MaxPathLen)))
      return false;

   mPoints.clear();
   mFlags.clear();
   mVisitPoints.clear();
   mLength = 0.0f;

   if(isServerObject())
      setMaskBits(PathMask);

   // Add points we need to visit in reverse order.
   if(mWaypoints && mWaypoints->size())
   {
      if(mIsLooping && mFromSet)
         mVisitPoints.push_back(mFrom);
      if(mToSet)
         mVisitPoints.push_front(mTo);
      for(S32 i = mWaypoints->size() - 1; i >= 0; i--)
      {
         SceneObject *s = dynamic_cast<SceneObject*>(mWaypoints->at(i));
         if(s)
         {
            mVisitPoints.push_back(s->getPosition());
            // This is potentially slow, but safe.
            if(!i && mIsLooping && !mFromSet)
               mVisitPoints.push_front(s->getPosition());
         }
      }
      if(mFromSet)
         mVisitPoints.push_back(mFrom);
   }
   else
   {
      if(mIsLooping)
         mVisitPoints.push_back(mFrom);
      mVisitPoints.push_back(mTo);
      mVisitPoints.push_back(mFrom);
   }

   return true;
}

dtNavMesh* Sample_TileMesh::loadAll(const char* path)
{
	FILE* fp = fopen(path, "rb");
	if (!fp) return 0;
	
	// Read header.
	NavMeshSetHeader header;
	fread(&header, sizeof(NavMeshSetHeader), 1, fp);
	if (header.magic != NAVMESHSET_MAGIC)
	{
		fclose(fp);
		return 0;
	}
	if (header.version != NAVMESHSET_VERSION)
	{
		fclose(fp);
		return 0;
	}
	
	dtNavMesh* mesh = dtAllocNavMesh();
	if (!mesh)
	{
		fclose(fp);
		return 0;
	}
	dtStatus status = mesh->init(&header.params);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return 0;
	}
		
	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		NavMeshTileHeader tileHeader;
		fread(&tileHeader, sizeof(tileHeader), 1, fp);
		if (!tileHeader.tileRef || !tileHeader.dataSize)
			break;

		unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!data) break;
		memset(data, 0, tileHeader.dataSize);
		fread(data, tileHeader.dataSize, 1, fp);
		
		mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
	}
	
	fclose(fp);
	
	return mesh;
}

bool nav_mesh::is_valid(const float* pos)
{
	dtPolyRef posRef = 0;
	float polyPickExt[3] = { 2, 4, 2 };

	dtStatus status = query_->findNearestPoly(pos, polyPickExt, &filter_, &posRef, nullptr);
	if (dtStatusFailed(status))
	{
		return false;
	}

	return true;
}

bool nav_mesh::adjust_pos(float* pos)
{
	dtPolyRef posRef = 0;
	float polyPickExt[3] = { 2, 4, 2 };

	dtStatus status = query_->findNearestPoly(pos, polyPickExt, &filter_, &posRef, pos);
	if (dtStatusFailed(status))
	{
		return false;
	}

	return true;
}

void Sample_TileMesh::buildAllTiles()
{
	if (!m_geom) return;
	if (!m_navMesh) return;
	
	const float* bmin = m_geom->getNavMeshBoundsMin();
	const float* bmax = m_geom->getNavMeshBoundsMax();
	int gw = 0, gh = 0;
	rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
	const int ts = (int)m_tileSize;
	const int tw = (gw + ts-1) / ts;
	const int th = (gh + ts-1) / ts;
	const float tcs = m_tileSize*m_cellSize;

	
	// Start the build process.
	m_ctx->startTimer(RC_TIMER_TEMP);

	for (int y = 0; y < th; ++y)
	{
		for (int x = 0; x < tw; ++x)
		{
			m_lastBuiltTileBmin[0] = bmin[0] + x*tcs;
			m_lastBuiltTileBmin[1] = bmin[1];
			m_lastBuiltTileBmin[2] = bmin[2] + y*tcs;
			
			m_lastBuiltTileBmax[0] = bmin[0] + (x+1)*tcs;
			m_lastBuiltTileBmax[1] = bmax[1];
			m_lastBuiltTileBmax[2] = bmin[2] + (y+1)*tcs;
			
			int dataSize = 0;
			unsigned char* data = buildTileMesh(x, y, m_lastBuiltTileBmin, m_lastBuiltTileBmax, dataSize);
			if (data)
			{
				// Remove any previous data (navmesh owns and deletes the data).
				m_navMesh->removeTile(m_navMesh->getTileRefAt(x,y,0),0,0);
				// Let the navmesh own the data.
				dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
				if (dtStatusFailed(status))
					dtFree(data);
			}
		}
	}
	
	// Start the build process.	
	m_ctx->stopTimer(RC_TIMER_TEMP);

	m_totalBuildTimeMs = m_ctx->getAccumulatedTime(RC_TIMER_TEMP)/1000.0f;
	
}

dtStatus dtTileCache::buildNavMeshTilesAt(const int tx, const int ty, dtNavMesh* navmesh)
{
	const int MAX_TILES = 32;
	dtCompressedTileRef tiles[MAX_TILES];
	const int ntiles = getTilesAt(tx,ty,tiles,MAX_TILES);
	
	for (int i = 0; i < ntiles; ++i)
	{
		dtStatus status = buildNavMeshTile(tiles[i], navmesh);
		if (dtStatusFailed(status))
			return status;
	}
	
	return DT_SUCCESS;
}

int fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery)
{
    if (npath < 3)
        return npath;

    // Get connected polygons
    static const int maxNeis = 16;
    dtPolyRef neis[maxNeis];
    int nneis = 0;

    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
        return npath;

    for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
    {
        const dtLink* link = &tile->links[k];
        if (link->ref != 0)
        {
            if (nneis < maxNeis)
                neis[nneis++] = link->ref;
        }
    }

    // If any of the neighbour polygons is within the next few polygons
    // in the path, short cut to that polygon directly.
    static const int maxLookAhead = 6;
    int cut = 0;
    for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {
        for (int j = 0; j < nneis; j++)
        {
            if (path[i] == neis[j]) {
                cut = i;
                break;
            }
        }
    }
    if (cut > 1)
    {
        int offset = cut - 1;
        npath -= offset;
        for (int i = 1; i < npath; i++)
            path[i] = path[i + offset];
    }

    return npath;
}

bool OgreDetourTileCache::TileCacheBuild(InputGeom *inputGeom)
{
    // Init configuration for specified geometry
    configure(inputGeom);

    dtStatus status;

    // Preprocess tiles.
    // Prepares navmesh tiles in a 2D intermediary format that allows quick conversion to a 3D navmesh

    m_cacheLayerCount = 0;
    m_cacheCompressedSize = 0;
    m_cacheRawSize = 0;

    for (int y = 0; y < m_th; ++y)
    {
        for (int x = 0; x < m_tw; ++x)
        {
            TileCacheData tiles[MAX_LAYERS];
            memset(tiles, 0, sizeof(tiles));
            int ntiles = rasterizeTileLayers(m_geom, x, y, m_cfg, tiles, MAX_LAYERS);  // This is where the tile is built

            for (int i = 0; i < ntiles; ++i)
            {
                TileCacheData* tile = &tiles[i];
                status = m_tileCache->addTile(tile->data, tile->dataSize, DT_COMPRESSEDTILE_FREE_DATA, 0);  // Add compressed tiles to tileCache
                if (dtStatusFailed(status))
                {
                    dtFree(tile->data);
                    tile->data = 0;
                    continue;
                }

                m_cacheLayerCount++;
                m_cacheCompressedSize += tile->dataSize;
                m_cacheRawSize += calcLayerBufferSize(m_tcparams.width, m_tcparams.height);
            }
        }
    }

    buildInitialNavmesh();

    return true;
}

bool nav_mesh::los(const float* start, const float* end, float* hitPos)
{
	float t = 0;
	float m_hitNormal[3] = { 0, };

	dtPolyRef startRef = 0;
	float polyPickExt[3] = { 2, 4, 2 };

	dtStatus status = query_->findNearestPoly(start, polyPickExt, &filter_, &startRef, nullptr);
	if (dtStatusFailed(status))
	{
		return false;
	}

	dtPolyRef polys[MAX_POLYS] = { 0, };
	int npolys = 0;

	query_->raycast(startRef, start, end, &filter_, &t, m_hitNormal, polys, &npolys, sizeof(polys) / sizeof(polys[0]));
	if (t > 1)
	{
		dtVcopy(hitPos, end);
		return true;
	}
	else
	{
		if (npolys > 0)
		{
			dtVlerp(hitPos, start, end, t);

			float temp[3] = { 0, };
			dtStatus statu = query_->closestPointOnPoly(polys[npolys - 1], hitPos, temp, nullptr);
			if (dtStatusSucceed(statu))
			{
				dtVcopy(hitPos, temp);
				return false;
			}
		}
	}

	dtVcopy(hitPos, start);
	return false;
}

void Sample_TileMesh::buildTile(const float* pos)
{
	if (!m_geom) return;
	if (!m_navMesh) return;
		
	const float* bmin = m_geom->getNavMeshBoundsMin();
	const float* bmax = m_geom->getNavMeshBoundsMax();
	
	const float ts = m_tileSize*m_cellSize;
	const int tx = (int)((pos[0] - bmin[0]) / ts);
	const int ty = (int)((pos[2] - bmin[2]) / ts);
	
	m_lastBuiltTileBmin[0] = bmin[0] + tx*ts;
	m_lastBuiltTileBmin[1] = bmin[1];
	m_lastBuiltTileBmin[2] = bmin[2] + ty*ts;
	
	m_lastBuiltTileBmax[0] = bmin[0] + (tx+1)*ts;
	m_lastBuiltTileBmax[1] = bmax[1];
	m_lastBuiltTileBmax[2] = bmin[2] + (ty+1)*ts;
	
	m_tileCol = duRGBA(255,255,255,64);
	
	m_ctx->resetLog();
	
	int dataSize = 0;
	unsigned char* data = buildTileMesh(tx, ty, m_lastBuiltTileBmin, m_lastBuiltTileBmax, dataSize);

	// Remove any previous data (navmesh owns and deletes the data).
	m_navMesh->removeTile(m_navMesh->getTileRefAt(tx,ty,0),0,0);

	// Add tile, or leave the location empty.
	if (data)
	{
		// Let the navmesh own the data.
		dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
		if (dtStatusFailed(status))
			dtFree(data);
	}
	
	m_ctx->dumpLog("Build Tile (%d,%d):", tx,ty);
}

   bool NavMesh::createNavMesh(dtNavMeshCreateParams &params)
   {
      unsigned char *tileData = NULL;
      S32 tileDataSize = 0;
      if(!dtCreateNavMeshData(&params, &tileData, &tileDataSize))
      {
         Con::errorf("Could not construct NavMeshData for NavMesh %s", getIdString());
         return false;
      }

      tnm = dtAllocNavMesh();
      if(!tnm)
      {
         Con::errorf("Out of memory allocating dtNavMesh for NavMesh %s", getIdString());
         return false;
      }
      dtStatus s = tnm->init(tileData, tileDataSize, DT_TILE_FREE_DATA);
      if(dtStatusFailed(s))
      {
         Con::errorf("Could not initialise dtNavMesh for NavMesh %s", getIdString());
         return false;
      }

      // Initialise all flags to something helpful.
      for(U32 i = 0; i < tnm->getMaxTiles(); ++i)
      {
         const dtMeshTile* tile = ((const dtNavMesh*)tnm)->getTile(i);
         if(!tile->header) continue;
         const dtPolyRef base = tnm->getPolyRefBase(tile);
         for(U32 j = 0; j < tile->header->polyCount; ++j)
         {
            const dtPolyRef ref = base | j;
            unsigned short f = 0;
            tnm->getPolyFlags(ref, &f);
            tnm->setPolyFlags(ref, f | 1);
         }
      }

      return true;
   }

    EXPORT_API dtStatus dtnmInitTiledNavMesh(dtNavMeshParams* params
        , dtNavMesh** ppNavMesh)
    {
        if (!params)
            return DT_FAILURE + DT_INVALID_PARAM;

        dtNavMesh* pNavMesh = dtAllocNavMesh();
		if (!pNavMesh)
            return DT_FAILURE + DT_OUT_OF_MEMORY;

		dtStatus status = 
            pNavMesh->init(params);
		if (dtStatusFailed(status))
		{
            dtFreeNavMesh(pNavMesh);
            return status;
		}

        *ppNavMesh = pNavMesh;

        return DT_SUCCESS;
    }

dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags)
{
	// Make sure the data is in right format.
	dtMeshHeader* header = (dtMeshHeader*)data;
	if (header->magic != DT_NAVMESH_MAGIC)
		return DT_FAILURE | DT_WRONG_MAGIC;
	if (header->version != DT_NAVMESH_VERSION)
		return DT_FAILURE | DT_WRONG_VERSION;

	dtNavMeshParams params;
	dtVcopy(params.orig, header->bmin);
	params.tileWidth = header->bmax[0] - header->bmin[0];
	params.tileHeight = header->bmax[2] - header->bmin[2];
	params.maxTiles = 1;
	params.maxPolys = header->polyCount;
	
	dtStatus status = init(&params);
	if (dtStatusFailed(status))
		return status;

	return addTile(data, dataSize, flags, 0, 0);
}