using System;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;

/// <summary>
/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
/// </summary>
public sealed class FootprintCollisionChecker
{
    /// <summary>创建连续车辆碰撞检查器。</summary>
    public FootprintCollisionChecker()
    {
    }

    /// <summary>
    /// 判断单个车辆位姿是否无碰撞。
    /// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
    /// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
    /// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
    /// </summary>
    public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
        double additionalMarginMeters, out double bodyClearanceMeters)
    {
        bodyClearanceMeters = 0d;
        if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
            !VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
            !VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
            return false;

        if (!AreCornersInsideMap(checkedFootprint, map)) return false;

        double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
        bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
        if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;

        return !IntersectsOccupiedCell(checkedFootprint, map);
    }

    /// <summary>
    /// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
    /// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
    /// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
    /// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
    /// </summary>
    public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
        double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
    {
        minimumBodyClearanceMeters = 0d;
        if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
            !NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
            !NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
            !VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
            return false;

        double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
        if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;

        if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
        minimumBodyClearanceMeters = fromClearanceMeters;

        double deltaX = to.X - from.X;
        double deltaY = to.Y - from.Y;
        double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
        double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
        if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
        double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
        if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
        int segmentCount = Math.Max(1, (int)rawSegmentCount);

        Pose2D previousPose = from;
        for (int segment = 1; segment <= segmentCount; segment++)
        {
            double endFraction = (double)segment / segmentCount;
            double middleFraction = ((double)segment - 0.5d) / segmentCount;
            var currentPose = new Pose2D(
                from.X + deltaX * endFraction,
                from.Y + deltaY * endFraction,
                from.Heading + headingDeltaRadians * endFraction);
            var middlePose = new Pose2D(
                from.X + deltaX * middleFraction,
                from.Y + deltaY * middleFraction,
                from.Heading + headingDeltaRadians * middleFraction);
            double segmentDeltaX = currentPose.X - previousPose.X;
            double segmentDeltaY = currentPose.Y - previousPose.Y;
            double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
            double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
            double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
                bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
            if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
                !IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
                return false;
            minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
            previousPose = currentPose;
        }

        if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
        minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
        return true;
    }

    private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
    {
        for (int index = 0; index < 4; index++)
        {
            footprint.GetCorner(index, out double cornerX, out double cornerY);
            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
        }
        return true;
    }

    private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
    {
        if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
        if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
        return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
    }

    private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
    {
        GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
            out int firstRow, out int lastRow, out int firstCol, out int lastCol);
        for (int row = firstRow; row <= lastRow; row++)
        for (int col = firstCol; col <= lastCol; col++)
        {
            if (!map.IsOccupied(row, col)) continue;
            GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
            if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
        }
        return false;
    }

    private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
        out int firstRow, out int lastRow, out int firstCol, out int lastCol)
    {
        double minimumMapX = map.Bounds.XMin / 1000d;
        double minimumMapY = map.Bounds.YMin / 1000d;
        firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
        lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
        firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
        lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
    }

    private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
        out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
    {
        cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
        cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
        cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
        cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
    }

    private static int Clamp(int value, int minimum, int maximum)
    {
        return value < minimum ? minimum : value > maximum ? maximum : value;
    }
}