/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  D E R E L I C T
//
/// // (c) 2003..2025

using Godot;
using System.Collections.Generic;

public static class AStarSolver
{
	public static Vector3[] FindPath( AStarGraph graph, Vector3 startPos, Vector3 endPos )
	{
		int startId = graph.GetClosestPoint( startPos );
		int endId = graph.GetClosestPoint( endPos );

		log.info( $"Finding path from {startPos.Log}->{endPos.Log} ({startId}->{endId})" );

		if( startId == -1 || endId == -1 )
			return new Vector3[0];

		var openSet = new PriorityQueue<int, float>();
		var cameFrom = new Dictionary<int, int>();

		var gScore = new Dictionary<int, float>();
		gScore[startId] = 0;

		var fScore = new Dictionary<int, float>();
		fScore[startId] = startPos.DistanceTo( endPos );

		openSet.Enqueue( startId, fScore[startId] );

		while( openSet.Count > 0 )
		{
			int currentId = openSet.Dequeue();
			if( currentId == endId )
			{
				log.debug( $"Found path!" );
				return ReconstructPath( startPos, endPos, cameFrom, currentId, graph );
			}

			foreach( int neighborId in graph.GetPointConnections( currentId ) )
			{
				Vector3 currentPos = graph.GetPointPosition( currentId );
				Vector3 neighborPos = graph.GetPointPosition( neighborId );
				float tentativeGScore = gScore.GetValueOrDefault( currentId, float.MaxValue ) + currentPos.DistanceTo( neighborPos );

				if( tentativeGScore < gScore.GetValueOrDefault( neighborId, float.MaxValue ) )
				{
					cameFrom[neighborId] = currentId;
					gScore[neighborId] = tentativeGScore;
					fScore[neighborId] = tentativeGScore + neighborPos.DistanceTo( endPos );

					openSet.Enqueue( neighborId, fScore[neighborId] );
				}
			}
		}

		return new Vector3[0]; // No path found
	}

	private static Vector3[] ReconstructPath( Vector3 startPos, Vector3 endPos, Dictionary<int, int> cameFrom, int currentId, AStarGraph graph )
	{
		var path = new List<Vector3> { endPos, graph.GetPointPosition( currentId ) };
		while( cameFrom.ContainsKey( currentId ) )
		{
			currentId = cameFrom[currentId];
			path.Add( graph.GetPointPosition( currentId ) );
		}
		path.Add( startPos );

		path.Reverse();

		log.debug( $"Path is {path.Count} long." );


		return path.ToArray();
	}
}