using System.Collections.Generic;
using Godot;

public static class Pathfinding
{
	private static AStar3D aStar = new AStar3D();
	private static Dictionary<Vector3I, long> coordToId = new Dictionary<Vector3I, long>();
	private static Dictionary<long, Vector3I> idToCoord = new Dictionary<long, Vector3I>();
	private static long nextId = 1;
	private static Dictionary<int, (long fromId, long toId)> verticalConnections = new Dictionary<int, (long fromId, long toId)>();

	private static long GetOrCreateId(Vector3I coord)
	{
		if (coordToId.TryGetValue(coord, out long id)) return id;

		id = nextId++;
		coordToId[coord] = id;
		idToCoord[id] = coord;

		return id;
	}

	public static void BuildAStarGraph()
	{
		aStar.Clear();
		coordToId.Clear();
		idToCoord.Clear();
		verticalConnections.Clear();
		nextId = 1;

		for (int y = 0; y < GameData.ruinSize; y++)
		{
			for (int x = 0; x < GameData.layerSize; x++)
			{
				for (int z = 0; z < GameData.layerSize; z++)
				{
					AddPointIfValid(x, y, z);
				}
			}
		}

		foreach (KeyValuePair<Vector3I, long> kvp in coordToId)
		{
			ConnectPoint(kvp.Key, kvp.Value);
		}

		for (int y = 0; y < GameData.ruinSize; y++)
		{
			UpdateGatePoint(y, false);
		}
	}

	private static void AddPointIfValid(int x, int y, int z)
	{
		Vector3I coord = new Vector3I(x, y, z);
		Tile tile = GameData.map[y].tiles[x, z];
		if (tile == null || tile.collapsedMesh == null) return;

		long id = GetOrCreateId(coord);
		aStar.AddPoint(id, tile.Position);
	}

	private static void ConnectPoint(Vector3I from, long fromId)
	{
		foreach (Vector3I offset in WFC.offsets3D)
		{
			Vector3I to = new Vector3I(
				from.X + offset.X,
				from.Y + offset.Y,
				from.Z + offset.Z
			);

			if (!coordToId.ContainsKey(to)) continue;
			if (!WFC.CanWalk3D(from, to)) continue;

			long toId = coordToId[to];
			if (TryRegisterGateConnection(from, to, fromId, toId)) continue;

			ConnectPointsIfNeeded(fromId, toId);
		}
	}

	private static bool TryRegisterGateConnection(Vector3I from, Vector3I to, long fromId, long toId)
	{
		if (from.Y == to.Y) return false;
		if (GameData.map[from.Y].tiles[from.X, from.Z].collapsedMesh != "gate") return false;

		verticalConnections[from.Y] = (fromId, toId);
		if (GameData.map[from.Y].isGateOpen)
		{
			ConnectPointsIfNeeded(fromId, toId);
		}

		return true;
	}

	private static void ConnectPointsIfNeeded(long fromId, long toId)
	{
		if (aStar.ArePointsConnected(fromId, toId)) return;

		aStar.ConnectPoints(fromId, toId, true);
	}

	public static void UpdateGatePoint(int layer, bool isOpen)
	{
		if (!verticalConnections.ContainsKey(layer)) return;

		(long fromId, long toId) = verticalConnections[layer];

		if (isOpen)
		{
			ConnectPointsIfNeeded(fromId, toId);
			return;
		}

		if (aStar.ArePointsConnected(fromId, toId))
		{
			aStar.DisconnectPoints(fromId, toId);
		}
	}

	public static List<Vector3> GetPath(Vector3I start, Vector3I end)
	{
		if (!coordToId.ContainsKey(start) || !coordToId.ContainsKey(end)) return new List<Vector3>();

		long startId = coordToId[start];
		long endId = coordToId[end];

		return new List<Vector3>(aStar.GetPointPath(startId, endId));
	}

	public static Vector3I GetClosestStartPoint(Vector3 robotPosition)
	{
		return idToCoord[aStar.GetClosestPoint(robotPosition)];
	}
}