namespace StellaOps.ElkSharp;

internal static partial class ElkEdgeRouterAStar8Dir
{
    // E, W, S, N, NE, SW, SE, NW
    private static readonly int[] Dx = [1, -1, 0, 0, 1, -1, 1, -1];
    private static readonly int[] Dy = [0, 0, 1, -1, -1, 1, 1, -1];
    // Direction codes: 1=horizontal, 2=vertical, 3=diagonal45 (NE/SW), 4=diagonal135 (SE/NW)
    private static readonly int[] DirCodes = [1, 1, 2, 2, 3, 3, 4, 4];

    internal static List<ElkPoint>? Route(
        ElkPoint start,
        ElkPoint end,
        (double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
        string sourceId,
        string targetId,
        AStarRoutingParams routingParams,
        IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
        CancellationToken cancellationToken)
    {
        var xs = new SortedSet<double> { start.X, end.X };
        var ys = new SortedSet<double> { start.Y, end.Y };
        foreach (var obstacle in obstacles)
        {
            if (obstacle.Id == sourceId || obstacle.Id == targetId)
            {
                continue;
            }

            xs.Add(obstacle.Left - routingParams.Margin);
            xs.Add(obstacle.Right + routingParams.Margin);
            ys.Add(obstacle.Top - routingParams.Margin);
            ys.Add(obstacle.Bottom + routingParams.Margin);
        }

        if (routingParams.IntermediateGridSpacing > 0d)
        {
            AddIntermediateLines(xs, routingParams.IntermediateGridSpacing);
            AddIntermediateLines(ys, routingParams.IntermediateGridSpacing);
        }

        var xArr = xs.ToArray();
        var yArr = ys.ToArray();
        var xCount = xArr.Length;
        var yCount = yArr.Length;
        if (xCount < 2 || yCount < 2)
        {
            return null;
        }

        var graphMaxY = obstacles.Length > 0
            ? obstacles.Max(obstacle => obstacle.Bottom)
            : double.MaxValue;
        var disallowedBottomY = graphMaxY + 4d;
        var maxDiagonalStepLength = ResolveMaxDiagonalStepLength(obstacles);

        var blockedSegments = BuildBlockedSegments(xArr, yArr, obstacles, sourceId, targetId);
        var softObstacleInfos = BuildSoftObstacleInfos(softObstacles);

        var startIx = Array.BinarySearch(xArr, start.X);
        var startIy = Array.BinarySearch(yArr, start.Y);
        var endIx = Array.BinarySearch(xArr, end.X);
        var endIy = Array.BinarySearch(yArr, end.Y);
        if (startIx < 0 || startIy < 0 || endIx < 0 || endIy < 0)
        {
            return null;
        }

        bool IsBlockedOrthogonal(int ix1, int iy1, int ix2, int iy2)
        {
            if (ix1 == ix2)
            {
                var minIy = Math.Min(iy1, iy2);
                var maxIy = Math.Max(iy1, iy2);
                for (var iy = minIy; iy < maxIy; iy++)
                {
                    if (blockedSegments.IsVerticalBlocked(ix1, iy))
                    {
                        return true;
                    }
                }

                return false;
            }

            if (iy1 == iy2)
            {
                var minIx = Math.Min(ix1, ix2);
                var maxIx = Math.Max(ix1, ix2);
                for (var ix = minIx; ix < maxIx; ix++)
                {
                    if (blockedSegments.IsHorizontalBlocked(ix, iy1))
                    {
                        return true;
                    }
                }

                return false;
            }

            return false;
        }

        const int dirCount = 5;
        var stateCount = xCount * yCount * dirCount;
        var gScore = new double[stateCount];
        Array.Fill(gScore, double.MaxValue);
        var cameFrom = new int[stateCount];
        Array.Fill(cameFrom, -1);

        var blockedEntryDir = 0;
        if (routingParams.EnforceEntryAngle)
        {
            foreach (var obstacle in obstacles)
            {
                if (obstacle.Id != targetId)
                {
                    continue;
                }

                var nodeLeft = obstacle.Left + routingParams.Margin;
                var nodeRight = obstacle.Right - routingParams.Margin;
                var nodeTop = obstacle.Top + routingParams.Margin;
                var nodeBottom = obstacle.Bottom - routingParams.Margin;
                if (Math.Abs(end.X - nodeLeft) < 2d || Math.Abs(end.X - nodeRight) < 2d)
                {
                    blockedEntryDir = 2;
                }
                else if (Math.Abs(end.Y - nodeTop) < 2d || Math.Abs(end.Y - nodeBottom) < 2d)
                {
                    blockedEntryDir = 1;
                }

                break;
            }
        }

        int StateId(int ix, int iy, int dir) => (ix * yCount + iy) * dirCount + dir;

        double Heuristic(int ix, int iy)
        {
            var hdx = xArr[ix] - xArr[endIx];
            var hdy = yArr[iy] - yArr[endIy];
            return Math.Sqrt(hdx * hdx + hdy * hdy);
        }

        var startState = StateId(startIx, startIy, 0);
        gScore[startState] = 0d;
        var openSet = new PriorityQueue<int, double>();
        openSet.Enqueue(startState, Heuristic(startIx, startIy));

        var maxIterations = xCount * yCount * 12;
        var iterations = 0;
        var closed = new bool[stateCount];

        while (openSet.Count > 0 && iterations++ < maxIterations)
        {
            cancellationToken.ThrowIfCancellationRequested();

            var current = openSet.Dequeue();
            if (closed[current])
            {
                continue;
            }

            closed[current] = true;

            var curDir = current % dirCount;
            var curIy = (current / dirCount) % yCount;
            var curIx = (current / dirCount) / yCount;

            if (curIx == endIx && curIy == endIy)
            {
                return ReconstructPath(current, cameFrom, xArr, yArr, yCount, dirCount);
            }

            for (var d = 0; d < 8; d++)
            {
                var nx = curIx + Dx[d];
                var ny = curIy + Dy[d];
                if (nx < 0 || nx >= xCount || ny < 0 || ny >= yCount)
                {
                    continue;
                }

                if (yArr[curIy] > disallowedBottomY || yArr[ny] > disallowedBottomY)
                {
                    continue;
                }

                var isDiagonal = Dx[d] != 0 && Dy[d] != 0;
                if (isDiagonal)
                {
                    if (IsBlockedOrthogonal(curIx, curIy, nx, curIy)
                        || IsBlockedOrthogonal(curIx, curIy, curIx, ny))
                    {
                        continue;
                    }
                }
                else if (IsBlockedOrthogonal(curIx, curIy, nx, ny))
                {
                    continue;
                }

                var newDir = DirCodes[d];
                if (blockedEntryDir > 0 && nx == endIx && ny == endIy && newDir == blockedEntryDir)
                {
                    continue;
                }

                double dist;
                if (isDiagonal)
                {
                    var ddx = xArr[nx] - xArr[curIx];
                    var ddy = yArr[ny] - yArr[curIy];
                    var diagonalStepLength = Math.Sqrt(ddx * ddx + ddy * ddy);
                    if (diagonalStepLength > maxDiagonalStepLength)
                    {
                        continue;
                    }

                    dist = diagonalStepLength + routingParams.DiagonalPenalty;
                }
                else
                {
                    dist = Math.Abs(xArr[nx] - xArr[curIx]) + Math.Abs(yArr[ny] - yArr[curIy]);
                }

                var bend = ComputeBendPenalty(curDir, newDir, routingParams.BendPenalty);
                var softCost = ComputeSoftObstacleCost(
                    xArr[curIx], yArr[curIy], xArr[nx], yArr[ny],
                    softObstacleInfos, routingParams);
                var tentativeG = gScore[current] + dist + bend + softCost;
                var neighborState = StateId(nx, ny, newDir);

                if (tentativeG < gScore[neighborState])
                {
                    gScore[neighborState] = tentativeG;
                    cameFrom[neighborState] = current;
                    openSet.Enqueue(neighborState, tentativeG + Heuristic(nx, ny));
                }
            }
        }

        return null;
    }

    private static List<ElkPoint> ReconstructPath(
        int endState,
        int[] cameFrom,
        double[] xArr,
        double[] yArr,
        int yCount,
        int dirCount)
    {
        var path = new List<ElkPoint>();
        var state = endState;
        while (state >= 0)
        {
            var stateY = (state / dirCount) % yCount;
            var stateX = (state / dirCount) / yCount;
            path.Add(new ElkPoint { X = xArr[stateX], Y = yArr[stateY] });
            state = cameFrom[state];
        }

        path.Reverse();

        var simplified = new List<ElkPoint> { path[0] };
        for (var i = 1; i < path.Count - 1; i++)
        {
            var previous = simplified[^1];
            var next = path[i + 1];
            var dx1 = Math.Sign(path[i].X - previous.X);
            var dy1 = Math.Sign(path[i].Y - previous.Y);
            var dx2 = Math.Sign(next.X - path[i].X);
            var dy2 = Math.Sign(next.Y - path[i].Y);
            if (dx1 != dx2 || dy1 != dy2)
            {
                simplified.Add(path[i]);
            }
        }

        simplified.Add(path[^1]);
        return simplified;
    }
}