using System.Collections.Generic;
using Godot;

public 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++)
                {
                    Vector3I coord = new Vector3I(x, y, z);
                    Tile tile = GameData.map[y].tiles[x, z];

                    if (tile == null || tile.collapsedMesh == null) continue;

                    long id = GetOrCreateId(coord);

                    aStar.AddPoint(id, tile.Position);
                }
            }
        }

        foreach (KeyValuePair<Vector3I, long> kvp in coordToId)
        {
            Vector3I from = kvp.Key;
            long fromId = kvp.Value;

            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 (from.Y != to.Y && GameData.map[from.Y].tiles[from.X, from.Z].collapsedMesh == "gate")
                {
                    verticalConnections[from.Y] = (fromId, toId);
                    if (GameData.map[from.Y].isGateOpen)
                    {
                        if (!aStar.ArePointsConnected(fromId, toId))
                        {
                            aStar.ConnectPoints(fromId, toId, true);
                        }
                    }
                    continue;
                }

                if (!aStar.ArePointsConnected(fromId, toId))
                {
                    aStar.ConnectPoints(fromId, toId, true);
                }
            }
        }

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

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

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

        if (isOpen)
        {
            if (!aStar.ArePointsConnected(fromId, toId))
            {
                aStar.ConnectPoints(fromId, toId, true);
            }
        }
        else
        {
            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)];
    }
}