/*
===========================================================================

IceTech GPL Source Code
Copyright (C) 2026 Justin Marshall 

This file is part of the IceTech GPL Source Code (?IceTech Source Code?).  

IceTech Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

IceTech Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with IceTech Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the IceTech Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the IceTech Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing Justin Marshall, justinmarshall20@gmail.com

===========================================================================
*/

#include "precompiled.h"
#pragma hdrstop

#include "NAV_local.h"

#define SUBSAMPLE_WALK_PATH		1
#define SUBSAMPLE_FLY_PATH		0

const int		maxWalkPathIterations		= 10;
const float		maxWalkPathDistance			= 500.0f;
const float		walkPathSampleDistance		= 8.0f;

const int		maxFlyPathIterations		= 10;
const float		maxFlyPathDistance			= 500.0f;
const float		flyPathSampleDistance		= 8.0f;


/*
============
idNAVLocal::EdgeSplitPoint

  calculates split point of the edge with the plane
  returns true if the split point is between the edge vertices
============
*/
bool idNAVLocal::EdgeSplitPoint( idVec3 &split, int edgeNum, const idPlane &plane ) const {
	const aasEdge_t *edge;
	idVec3 v1, v2;
	float d1, d2;

	edge = &file->GetEdge( edgeNum );
	v1 = file->GetVertex( edge->vertexNum[0] );
	v2 = file->GetVertex( edge->vertexNum[1] );
	d1 = v1 * plane.Normal() - plane.Dist();
	d2 = v2 * plane.Normal() - plane.Dist();

	//if ( (d1 < CM_CLIP_EPSILON && d2 < CM_CLIP_EPSILON) || (d1 > -CM_CLIP_EPSILON && d2 > -CM_CLIP_EPSILON) ) {
	if ( FLOATSIGNBITSET( d1 ) == FLOATSIGNBITSET( d2 ) ) {
		return false;
	}
	split = v1 + (d1 / (d1 - d2)) * (v2 - v1);
	return true;
}

/*
============
idNAVLocal::FloorEdgeSplitPoint

  calculates either the closest or furthest point on the floor of the area which also lies on the pathPlane
  the point has to be on the front side of the frontPlane to be valid
============
*/
bool idNAVLocal::FloorEdgeSplitPoint( idVec3 &bestSplit, int areaNum, const idPlane &pathPlane, const idPlane &frontPlane, bool closest ) const {
	int i, j, faceNum, edgeNum;
	const aasArea_t *area;
	const aasFace_t *face;
	idVec3 split;
	float dist, bestDist;

	if ( closest ) {
		bestDist = maxWalkPathDistance;
	} else {
		bestDist = -0.1f;
	}

	area = &file->GetArea( areaNum );

	for ( i = 0; i < area->numFaces; i++ ) {
		faceNum = file->GetFaceIndex( area->firstFace + i );
		face = &file->GetFace( abs(faceNum) );

		if ( !(face->flags & FACE_FLOOR ) ) {
			continue;
		}

		for ( j = 0; j < face->numEdges; j++ ) {
			edgeNum = file->GetEdgeIndex( face->firstEdge + j );

			if ( !EdgeSplitPoint( split, abs( edgeNum ), pathPlane ) ) {
				continue;
			}
			dist = frontPlane.Distance( split );
			if ( closest ) {
				if ( dist >= -0.1f && dist < bestDist ) {
					bestDist = dist;
					bestSplit = split;
				}
			} else {
				if ( dist > bestDist ) {
					bestDist = dist;
					bestSplit = split;
				}
			}
		}
	}

	if ( closest ) {
		return ( bestDist < maxWalkPathDistance );
	} else {
		return ( bestDist > -0.1f );
	}
}

/*
============
idNAVLocal::WalkPathValid

  returns true if one can walk in a straight line between origin and goalOrigin
============
*/
bool idNAVLocal::WalkPathValid( int areaNum, const idVec3 &origin, int goalAreaNum, const idVec3 &goalOrigin, int travelFlags, idVec3 &endPos, int &endAreaNum ) const {
	int curAreaNum, lastAreaNum, lastAreas[4], lastAreaIndex;
	idPlane pathPlane, frontPlane, farPlane;
	idReachability *reach;
	const aasArea_t *area;
	idVec3 p, dir;

	if ( recastNavMesh.IsLoaded() ) {
		return recastNavMesh.PathValid( areaNum, origin, goalAreaNum, goalOrigin, travelFlags, endPos, endAreaNum );
	}

	if ( file == NULL ) {
		endPos = goalOrigin;
		endAreaNum = 0;
		return true;
	}

	lastAreas[0] = lastAreas[1] = lastAreas[2] = lastAreas[3] = areaNum;
	lastAreaIndex = 0;

	pathPlane.SetNormal( (goalOrigin - origin).Cross( file->GetSettings().gravityDir ) );
	pathPlane.Normalize();
	pathPlane.FitThroughPoint( origin );

	frontPlane.SetNormal( goalOrigin - origin );
	frontPlane.Normalize();
	frontPlane.FitThroughPoint( origin );

	farPlane.SetNormal( frontPlane.Normal() );
	farPlane.FitThroughPoint( goalOrigin );

	curAreaNum = areaNum;
	lastAreaNum = curAreaNum;

	while ( 1 ) {

		// find the furthest floor face split point on the path
		if ( !FloorEdgeSplitPoint( endPos, curAreaNum, pathPlane, frontPlane, false ) ) {
			endPos = origin;
		}

		// if we found a point near or further than the goal we're done
		if ( farPlane.Distance( endPos ) > -0.5f ) {
			break;
		}

		// if we reached the goal area we're done
		if ( curAreaNum == goalAreaNum ) {
			break;
		}

		frontPlane.SetDist( frontPlane.Normal() * endPos );

		area = &file->GetArea( curAreaNum );

		for ( reach = area->reach; reach; reach = reach->next ) {
			if ( reach->travelType != TFL_WALK ) {
				continue;
			}

			// if the reachability goes back to a previous area
			if ( reach->toAreaNum == lastAreas[0] || reach->toAreaNum == lastAreas[1] ||
					reach->toAreaNum == lastAreas[2] || reach->toAreaNum == lastAreas[3] ) {
				continue;
			}

			// if undesired travel flags are required to travel through the area
			if ( file->GetArea( reach->toAreaNum ).travelFlags & ~travelFlags ) {
				continue;
			}

			// don't optimize through an area near a ledge
			if ( file->GetArea( reach->toAreaNum ).flags & AREA_LEDGE ) {
				continue;
			}

			// find the closest floor face split point on the path
			if ( !FloorEdgeSplitPoint( p, reach->toAreaNum, pathPlane, frontPlane, true ) ) {
				continue;
			}

			// direction parallel to gravity
			dir = ( file->GetSettings().gravityDir * endPos * file->GetSettings().gravityDir ) -
						( file->GetSettings().gravityDir * p * file->GetSettings().gravityDir );
			if ( dir.LengthSqr() > Square( file->GetSettings().maxStepHeight ) ) {
				continue;
			}

			// direction orthogonal to gravity
			dir = endPos - p - dir;
			if ( dir.LengthSqr() > Square( 0.2f ) ) {
				continue;
			}

			break;
		}

		if ( !reach ) {
			return false;
		}

		lastAreas[lastAreaIndex] = curAreaNum;
		lastAreaIndex = ( lastAreaIndex + 1 ) & 3;

		curAreaNum = reach->toAreaNum;
	}

	endAreaNum = curAreaNum;

	return true;
}

/*
============
idNAVLocal::SubSampleWalkPath
============
*/
idVec3 idNAVLocal::SubSampleWalkPath( int areaNum, const idVec3 &origin, const idVec3 &start, const idVec3 &end, int travelFlags, int &endAreaNum ) const {
	int i, numSamples, curAreaNum;
	idVec3 dir, point, nextPoint, endPos;

	dir = end - start;
	numSamples = (int) (dir.Length() / walkPathSampleDistance) + 1;

	point = start;
	for ( i = 1; i < numSamples; i++ ) {
		nextPoint = start + dir * ((float) i / numSamples);
		if ( (point - nextPoint).LengthSqr() > Square( maxWalkPathDistance ) ) {
			return point;
		}
		if ( !idNAVLocal::WalkPathValid( areaNum, origin, 0, nextPoint, travelFlags, endPos, curAreaNum ) ) {
			return point;
		}
		point = nextPoint;
		endAreaNum = curAreaNum;
	}
	return point;
}

/*
============
idNAVLocal::WalkPathToGoal

  FIXME: don't stop optimizing on first failure ?
============
*/
bool idNAVLocal::WalkPathToGoal( aasPath_t &path, int areaNum, const idVec3 &origin, int goalAreaNum, const idVec3 &goalOrigin, int travelFlags ) const {
	int i, travelTime, curAreaNum, lastAreas[4], lastAreaIndex, endAreaNum;
	idReachability *reach;
	idVec3 endPos;

	if ( recastNavMesh.IsLoaded() ) {
		return recastNavMesh.PathToGoal( path, areaNum, origin, goalAreaNum, goalOrigin, travelFlags );
	}

	path.type = PATHTYPE_WALK;
	path.moveGoal = origin;
	path.moveAreaNum = areaNum;
	path.secondaryGoal = origin;
	path.reachability = NULL;

	if ( file == NULL || areaNum == goalAreaNum ) {
		path.moveGoal = goalOrigin;
		return true;
	}

	lastAreas[0] = lastAreas[1] = lastAreas[2] = lastAreas[3] = areaNum;
	lastAreaIndex = 0;

	curAreaNum = areaNum;

	for ( i = 0; i < maxWalkPathIterations; i++ ) {

		if ( !idNAVLocal::RouteToGoalArea( curAreaNum, path.moveGoal, goalAreaNum, travelFlags, travelTime, &reach ) ) {
			break;
		}

		if ( !reach ) {
			return false;
		}

		// no need to check through the first area
		if ( areaNum != curAreaNum ) {
			// only optimize a limited distance ahead
			if ( (reach->start - origin).LengthSqr() > Square( maxWalkPathDistance ) ) {
#if SUBSAMPLE_WALK_PATH
				path.moveGoal = SubSampleWalkPath( areaNum, origin, path.moveGoal, reach->start, travelFlags, path.moveAreaNum );
#endif
				return true;
			}

			if ( !idNAVLocal::WalkPathValid( areaNum, origin, 0, reach->start, travelFlags, endPos, endAreaNum ) ) {
#if SUBSAMPLE_WALK_PATH
				path.moveGoal = SubSampleWalkPath( areaNum, origin, path.moveGoal, reach->start, travelFlags, path.moveAreaNum );
#endif
				return true;
			}
		}

		path.moveGoal = reach->start;
		path.moveAreaNum = curAreaNum;

		if ( reach->travelType != TFL_WALK ) {
			break;
		}

		if ( !idNAVLocal::WalkPathValid( areaNum, origin, 0, reach->end, travelFlags, endPos, endAreaNum ) ) {
			return true;
		}

		path.moveGoal = reach->end;
		path.moveAreaNum = reach->toAreaNum;

		if ( reach->toAreaNum == goalAreaNum ) {
			if ( !idNAVLocal::WalkPathValid( areaNum, origin, 0, goalOrigin, travelFlags, endPos, endAreaNum ) ) {
#if SUBSAMPLE_WALK_PATH
				path.moveGoal = SubSampleWalkPath( areaNum, origin, path.moveGoal, goalOrigin, travelFlags, path.moveAreaNum );
#endif
				return true;
			}
			path.moveGoal = goalOrigin;
			path.moveAreaNum = goalAreaNum;
			return true;
		}

		lastAreas[lastAreaIndex] = curAreaNum;
		lastAreaIndex = ( lastAreaIndex + 1 ) & 3;

		curAreaNum = reach->toAreaNum;

		if ( curAreaNum == lastAreas[0] || curAreaNum == lastAreas[1] ||
				curAreaNum == lastAreas[2] || curAreaNum == lastAreas[3] ) {
			common->Warning( "idNAVLocal::WalkPathToGoal: local routing minimum going from area %d to area %d", areaNum, goalAreaNum );
			break;
		}
	}

	if ( !reach ) {
		return false;
	}

	switch( reach->travelType ) {
		case TFL_WALKOFFLEDGE:
			path.type = PATHTYPE_WALKOFFLEDGE;
			path.secondaryGoal = reach->end;
			path.reachability = reach;
			break;
		case TFL_BARRIERJUMP:
			path.type |= PATHTYPE_BARRIERJUMP;
			path.secondaryGoal = reach->end;
			path.reachability = reach;
			break;
		case TFL_JUMP:
			path.type |= PATHTYPE_JUMP;
			path.secondaryGoal = reach->end;
			path.reachability = reach;
			break;
		default:
			break;
	}

	return true;
}

/*
============
idNAVLocal::FlyPathValid

  returns true if one can fly in a straight line between origin and goalOrigin
============
*/
bool idNAVLocal::FlyPathValid( int areaNum, const idVec3 &origin, int goalAreaNum, const idVec3 &goalOrigin, int travelFlags, idVec3 &endPos, int &endAreaNum ) const {
	aasTrace_t trace;

	if ( recastNavMesh.IsLoaded() ) {
		return recastNavMesh.PathValid( areaNum, origin, goalAreaNum, goalOrigin, travelFlags, endPos, endAreaNum );
	}

	if ( file == NULL ) {
		endPos = goalOrigin;
		endAreaNum = 0;
		return true;
	}

	file->Trace( trace, origin, goalOrigin );

	endPos = trace.endpos;
	endAreaNum = trace.lastAreaNum;

	if ( trace.fraction >= 1.0f ) {
		return true;
	}

	return false;
}

/*
============
idNAVLocal::SubSampleFlyPath
============
*/
idVec3 idNAVLocal::SubSampleFlyPath( int areaNum, const idVec3 &origin, const idVec3 &start, const idVec3 &end, int travelFlags, int &endAreaNum ) const {
	int i, numSamples, curAreaNum;
	idVec3 dir, point, nextPoint, endPos;

	dir = end - start;
	numSamples = (int) (dir.Length() / flyPathSampleDistance) + 1;

	point = start;
	for ( i = 1; i < numSamples; i++ ) {
		nextPoint = start + dir * ((float) i / numSamples);
		if ( (point - nextPoint).LengthSqr() > Square( maxFlyPathDistance ) ) {
			return point;
		}
		if ( !idNAVLocal::FlyPathValid( areaNum, origin, 0, nextPoint, travelFlags, endPos, curAreaNum ) ) {
			return point;
		}
		point = nextPoint;
		endAreaNum = curAreaNum;
	}
	return point;
}

/*
============
idNAVLocal::FlyPathToGoal

  FIXME: don't stop optimizing on first failure ?
============
*/
bool idNAVLocal::FlyPathToGoal( aasPath_t &path, int areaNum, const idVec3 &origin, int goalAreaNum, const idVec3 &goalOrigin, int travelFlags ) const {
	int i, travelTime, curAreaNum, lastAreas[4], lastAreaIndex, endAreaNum;
	idReachability *reach;
	idVec3 endPos;

	if ( recastNavMesh.IsLoaded() ) {
		return recastNavMesh.PathToGoal( path, areaNum, origin, goalAreaNum, goalOrigin, travelFlags );
	}

	path.type = PATHTYPE_WALK;
	path.moveGoal = origin;
	path.moveAreaNum = areaNum;
	path.secondaryGoal = origin;
	path.reachability = NULL;

	if ( file == NULL || areaNum == goalAreaNum ) {
		path.moveGoal = goalOrigin;
		return true;
	}

	lastAreas[0] = lastAreas[1] = lastAreas[2] = lastAreas[3] = areaNum;
	lastAreaIndex = 0;

	curAreaNum = areaNum;

	for ( i = 0; i < maxFlyPathIterations; i++ ) {

		if ( !idNAVLocal::RouteToGoalArea( curAreaNum, path.moveGoal, goalAreaNum, travelFlags, travelTime, &reach ) ) {
			break;
		}

		if ( !reach ) {
			return false;
		}

		// no need to check through the first area
		if ( areaNum != curAreaNum ) {
			if ( (reach->start - origin).LengthSqr() > Square( maxFlyPathDistance ) ) {
#if SUBSAMPLE_FLY_PATH
				path.moveGoal = SubSampleFlyPath( areaNum, origin, path.moveGoal, reach->start, travelFlags, path.moveAreaNum );
#endif
				return true;
			}

			if ( !idNAVLocal::FlyPathValid( areaNum, origin, 0, reach->start, travelFlags, endPos, endAreaNum ) ) {
#if SUBSAMPLE_FLY_PATH
				path.moveGoal = SubSampleFlyPath( areaNum, origin, path.moveGoal, reach->start, travelFlags, path.moveAreaNum );
#endif
				return true;
			}
		}

		path.moveGoal = reach->start;
		path.moveAreaNum = curAreaNum;

		if ( !idNAVLocal::FlyPathValid( areaNum, origin, 0, reach->end, travelFlags, endPos, endAreaNum ) ) {
			return true;
		}

		path.moveGoal = reach->end;
		path.moveAreaNum = reach->toAreaNum;

		if ( reach->toAreaNum == goalAreaNum ) {
			if ( !idNAVLocal::FlyPathValid( areaNum, origin, 0, goalOrigin, travelFlags, endPos, endAreaNum ) ) {
#if SUBSAMPLE_FLY_PATH
				path.moveGoal = SubSampleFlyPath( areaNum, origin, path.moveGoal, goalOrigin, travelFlags, path.moveAreaNum );
#endif
				return true;
			}
			path.moveGoal = goalOrigin;
			path.moveAreaNum = goalAreaNum;
			return true;
		}

		lastAreas[lastAreaIndex] = curAreaNum;
		lastAreaIndex = ( lastAreaIndex + 1 ) & 3;

		curAreaNum = reach->toAreaNum;

		if ( curAreaNum == lastAreas[0] || curAreaNum == lastAreas[1] ||
				curAreaNum == lastAreas[2] || curAreaNum == lastAreas[3] ) {
			common->Warning( "idNAVLocal::FlyPathToGoal: local routing minimum going from area %d to area %d", areaNum, goalAreaNum );
			break;
		}
	}

	if ( !reach ) {
		return false;
	}

	return true;
}

typedef struct wallEdge_s {
	int					edgeNum;
	int					verts[2];
	struct wallEdge_s *	next;
} wallEdge_t;

/*
============
idNAVLocal::SortWallEdges
============
*/
void idNAVLocal::SortWallEdges( int *edges, int numEdges ) const {
	int i, j, k, numSequences;
	wallEdge_t **sequenceFirst, **sequenceLast, *wallEdges, *wallEdge;

	wallEdges = (wallEdge_t *) _alloca16( numEdges * sizeof( wallEdge_t ) );
	sequenceFirst = (wallEdge_t **)_alloca16( numEdges * sizeof( wallEdge_t * ) );
	sequenceLast = (wallEdge_t **)_alloca16( numEdges * sizeof( wallEdge_t * ) );

	for ( i = 0; i < numEdges; i++ ) {
		wallEdges[i].edgeNum = edges[i];
		GetEdgeVertexNumbers( edges[i], wallEdges[i].verts );
		wallEdges[i].next = NULL;
		sequenceFirst[i] = &wallEdges[i];
		sequenceLast[i] = &wallEdges[i];
	}
	numSequences = numEdges;

	for ( i = 0; i < numSequences; i++ ) {
		for ( j = i+1; j < numSequences; j++ ) {
			if ( sequenceFirst[i]->verts[0] == sequenceLast[j]->verts[1] ) {
				sequenceLast[j]->next = sequenceFirst[i];
				sequenceFirst[i] = sequenceFirst[j];
				break;
			}
			if ( sequenceLast[i]->verts[1] == sequenceFirst[j]->verts[0] ) {
				sequenceLast[i]->next = sequenceFirst[j];
				break;
			}
		}
		if ( j < numSequences ) {
			numSequences--;
			for ( k = j; k < numSequences; k++ ) {
				sequenceFirst[k] = sequenceFirst[k+1];
				sequenceLast[k] = sequenceLast[k+1];
			}
			i = -1;
		}
	}

	k = 0;
	for ( i = 0; i < numSequences; i++ ) {
		for ( wallEdge = sequenceFirst[i]; wallEdge; wallEdge = wallEdge->next ) {
			edges[k++] = wallEdge->edgeNum;
		}
	}
}

/*
============
idNAVLocal::GetWallEdges
============
*/
int idNAVLocal::GetWallEdges( int areaNum, const idBounds &bounds, int travelFlags, int *edges, int maxEdges ) const {
	int i, j, k, l, face1Num, face2Num, edge1Num, edge2Num, numEdges, absEdge1Num;
	int *areaQueue, curArea, queueStart, queueEnd;
	byte *areasVisited;
	const aasArea_t *area;
	const aasFace_t *face1, *face2;
	idReachability *reach;

	if ( !file ) {
		return 0;
	}

	numEdges = 0;

	areasVisited = (byte *) _alloca16( file->GetNumAreas() );
	memset( areasVisited, 0, file->GetNumAreas() * sizeof( byte ) );
	areaQueue = (int *) _alloca16( file->GetNumAreas() * sizeof( int ) );

	queueStart = -1;
	queueEnd = 0;
	areaQueue[0] = areaNum;
	areasVisited[areaNum] = true;

	for ( curArea = areaNum; queueStart < queueEnd; curArea = areaQueue[++queueStart] ) {

		area = &file->GetArea( curArea );

		for ( i = 0; i < area->numFaces; i++ ) {
			face1Num = file->GetFaceIndex( area->firstFace + i );
			face1 = &file->GetFace( abs(face1Num) );

			if ( !(face1->flags & FACE_FLOOR ) ) {
				continue;
			}

			for ( j = 0; j < face1->numEdges; j++ ) {
				edge1Num = file->GetEdgeIndex( face1->firstEdge + j );
				absEdge1Num = abs( edge1Num );

				// test if the edge is shared by another floor face of this area
				for ( k = 0; k < area->numFaces; k++ ) {
					if ( k == i ) {
						continue;
					}
					face2Num = file->GetFaceIndex( area->firstFace + k );
					face2 = &file->GetFace( abs(face2Num) );

					if ( !(face2->flags & FACE_FLOOR ) ) {
						continue;
					}

					for ( l = 0; l < face2->numEdges; l++ ) {
						edge2Num = abs( file->GetEdgeIndex( face2->firstEdge + l ) );
						if ( edge2Num == absEdge1Num ) {
							break;
						}
					}
					if ( l < face2->numEdges ) {
						break;
					}
				}
				if ( k < area->numFaces ) {
					continue;
				}

				// test if the edge is used by a reachability
				for ( reach = area->reach; reach; reach = reach->next ) {
					if ( reach->travelType & travelFlags ) {
						if ( reach->edgeNum == absEdge1Num ) {
							break;
						}
					}
				}
				if ( reach ) {
					continue;
				}

				// test if the edge is already in the list
				for ( k = 0; k < numEdges; k++ ) {
					if ( edge1Num == edges[k] ) {
						break;
					}
				}
				if ( k < numEdges ) {
					continue;
				}

				// add the edge to the list
				edges[numEdges++] = edge1Num;
				if ( numEdges >= maxEdges ) {
					return numEdges;
				}
			}
		}

		// add new areas to the queue
		for ( reach = area->reach; reach; reach = reach->next ) {
			if ( reach->travelType & travelFlags ) {
				// if the area the reachability leads to hasn't been visited yet and the area bounds touch the search bounds
				if ( !areasVisited[reach->toAreaNum] && bounds.IntersectsBounds( file->GetArea( reach->toAreaNum ).bounds ) ) {
					areaQueue[queueEnd++] = reach->toAreaNum;
					areasVisited[reach->toAreaNum] = true;
				}
			}
		}
	}
	return numEdges;
}