using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;

/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
public enum CoarsePathTestScenario
{
    /// <summary>明确允许的空地图直达场景。</summary>
    ExplicitEmpty,

    /// <summary>由中央矩形阻断直线的绕行场景。</summary>
    RectangleDetour,

    /// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
    ManualAndTwoLeg,

    /// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
    CacheHit,

    /// <summary>起步前进、终点倒车进入的换向场景。</summary>
    ReverseGearSwitch,

    /// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
    NoFeasiblePath,
}

/// <summary>手动障碍物输入支持的世界几何类型。</summary>
public enum ManualCoarsePathObstacleKind
{
    /// <summary>由世界中心和半径定义的圆形障碍物。</summary>
    Circle,

    /// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
    AxisAlignedRectangle,
}

/// <summary>
/// 手动粗路径测试的不可变障碍物输入。
/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
/// </summary>
public sealed class ManualCoarsePathObstacle
{
    private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
        double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
    {
        Kind = kind;
        CenterXMillimeters = centerXMillimeters;
        CenterYMillimeters = centerYMillimeters;
        SizeXMillimeters = sizeXMillimeters;
        SizeYMillimeters = sizeYMillimeters;
    }

    /// <summary>障碍物的支持几何类型。</summary>
    public ManualCoarsePathObstacleKind Kind { get; }

    /// <summary>几何中心世界 X 坐标，单位 mm。</summary>
    public double CenterXMillimeters { get; }

    /// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
    public double CenterYMillimeters { get; }

    /// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
    public double SizeXMillimeters { get; }

    /// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
    public double SizeYMillimeters { get; }

    /// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
    public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
        double radiusMillimeters)
    {
        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
            centerYMillimeters, radiusMillimeters, radiusMillimeters);
    }

    /// <summary>
    /// 创建轴对齐矩形障碍物。
    /// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
    /// </summary>
    public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
        double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
    {
        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
        EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
        EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
            centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
    }

    private static void EnsureFinite(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
    }

    private static void EnsurePositiveFinite(double value, string parameterName)
    {
        EnsureFinite(value, parameterName);
        if (value <= 0d)
            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
    }
}

/// <summary>
/// Clumsy 粗路径测试的纯输入工厂。
/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
/// </summary>
public static class CoarsePathScenarioFactory
{
    private const float MapXMinMillimeters = 0f;
    private const float MapXMaxMillimeters = 6000f;
    private const float MapYMinMillimeters = 0f;
    private const float MapYMaxMillimeters = 4000f;
    private const float ResolutionMillimeters = 50f;
    private const double MillimetersPerMeter = 1000d;
    private const double DegreesToRadians = Math.PI / 180d;
    private const double ManualMapPaddingMillimeters = 8000d;
    private const int MaximumManualObstacleCount = 20;

    /// <summary>
    /// 创建一个新的固定测试业务请求。
    /// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
    /// </summary>
    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
    {
        return CreateCore(scenario, null);
    }

    /// <summary>
    /// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
    /// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
    /// </summary>
    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
        double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
    {
        return CreateCore(scenario,
            new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
    }

    private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
    {
        switch (scenario)
        {
            case CoarsePathTestScenario.ExplicitEmpty:
                return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
            case CoarsePathTestScenario.RectangleDetour:
            case CoarsePathTestScenario.CacheHit:
                return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
            case CoarsePathTestScenario.ManualAndTwoLeg:
                return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
            case CoarsePathTestScenario.ReverseGearSwitch:
                return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
            case CoarsePathTestScenario.NoFeasiblePath:
                return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
            default:
                throw new ArgumentOutOfRangeException(nameof(scenario));
        }
    }

    /// <summary>
    /// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
    /// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
    /// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
    /// </summary>
    public static CoarsePathPlanningJob CreateManualGoalDemo(
        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
    {
        return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
            goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
            Array.Empty<ManualCoarsePathObstacle>(), 0L);
    }

    /// <summary>
    /// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
    /// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
    /// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
    /// </summary>
    public static CoarsePathPlanningJob CreateManualObstacleDemo(
        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
        IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
    {
        EnsureFinite(startXMillimeters, nameof(startXMillimeters));
        EnsureFinite(startYMillimeters, nameof(startYMillimeters));
        EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
        EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
        EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
        EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));

        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
        if (obstacles.Count > MaximumManualObstacleCount)
            throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
        if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
            throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");

        return CreateJob(
            CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
                obstacles, obstacleSnapshotVersion),
            ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
            ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
            null,
            GoalDirectionConstraint.Any);
    }

    private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
    {
        return CreateJob(
            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
            transform.Pose(1d, 2d, 0d),
            transform.Pose(5d, 2d, 0d),
            null,
            GoalDirectionConstraint.Forward);
    }

    private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
    {
        IMapObstacleSource[] sources =
        {
            new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
            {
                new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
                    transform.Y(1200f), transform.Y(2800f)),
            }),
        };
        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
            transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
        // 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
        return job;
    }

    private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
    {
        IMapObstacleSource[] sources =
        {
            new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
            {
                new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
                new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
                    transform.Y(2000f), transform.Y(2600f)),
            }),
            new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
                transform.X(3900f), transform.Y(2500f), 0d,
                    -180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
        };
        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
            transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
        // 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
        return job;
    }

    private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
    {
        return CreateJob(
            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
            transform.Pose(1d, 2d, 0d),
            transform.Pose(4d, 2d, 0d),
            TravelDirection.Forward,
            GoalDirectionConstraint.Reverse);
    }

    private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
    {
        IMapObstacleSource[] sources =
        {
            new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
            {
                new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
                    transform.Y(0f), transform.Y(4000f)),
            }),
        };
        return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
            transform.Pose(5d, 2d, 0d),
            null, GoalDirectionConstraint.Forward);
    }

    private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
        TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
    {
        return new CoarsePathPlanningJob
        {
            MapRequest = mapRequest,
            Start = start,
            Goal = goal,
            Vehicle = new VehicleParameters
            {
                LengthMeters = 0.80d,
                WidthMeters = 0.60d,
                SafetyMarginMeters = 0.05d,
                MaximumCurvaturePerMeter = 1d / 1.20d,
            },
            Configuration = new HybridAStarConfiguration(),
            StartDirection = startDirection,
            GoalDirection = goalDirection,
        };
    }

    private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
        FixedScenarioTransform transform)
    {
        return new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
                transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
            ResolutionMm = ResolutionMillimeters,
            ObstacleSources = sources,
            AllowExplicitEmptyMap = allowExplicitEmptyMap,
        };
    }

    private sealed class FixedScenarioAnchor
    {
        public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
        {
            EnsureFinite(xMillimeters, nameof(xMillimeters));
            EnsureFinite(yMillimeters, nameof(yMillimeters));
            EnsureFinite(headingDegrees, nameof(headingDegrees));
            XMillimeters = xMillimeters;
            YMillimeters = yMillimeters;
            HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
        }

        public double XMillimeters { get; }
        public double YMillimeters { get; }
        public double HeadingRadians { get; }
    }

    private sealed class FixedScenarioTransform
    {
        private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
        {
            DeltaXMillimeters = deltaXMillimeters;
            DeltaYMillimeters = deltaYMillimeters;
            HeadingDeltaRadians = headingDeltaRadians;
        }

        public double DeltaXMillimeters { get; }
        public double DeltaYMillimeters { get; }
        public double HeadingDeltaRadians { get; }

        public static FixedScenarioTransform From(double baselineStartXMillimeters,
            double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
        {
            if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
            return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
                anchor.YMillimeters - baselineStartYMillimeters,
                NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
        }

        public float X(float value)
        {
            return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
        }

        public float Y(float value)
        {
            return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
        }

        public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
        {
            return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
                (yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
                NormalizeRadians(headingRadians + HeadingDeltaRadians));
        }
    }

    private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
        double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
        long obstacleSnapshotVersion)
    {
        double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
        double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
        double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
        double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
        for (int index = 0; index < obstacles.Count; index++)
        {
            ManualCoarsePathObstacle obstacle = obstacles[index] ??
                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
            double halfX;
            double halfY;
            switch (obstacle.Kind)
            {
                case ManualCoarsePathObstacleKind.Circle:
                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
                    halfX = obstacle.SizeXMillimeters;
                    halfY = obstacle.SizeYMillimeters;
                    break;
                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
                    EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
                    halfX = obstacle.SizeXMillimeters / 2d;
                    halfY = obstacle.SizeYMillimeters / 2d;
                    break;
                default:
                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
            }

            minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
            maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
            minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
            maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
        }

        float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
        float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
        float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
        float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
        bool isExplicitEmptyMap = obstacles.Count == 0;
        return new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
            ResolutionMm = ResolutionMillimeters,
            ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
                new IMapObstacleSource[]
                {
                    new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
                        ConvertManualObstacles(obstacles)),
                },
            AllowExplicitEmptyMap = isExplicitEmptyMap,
        };
    }

    private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
    {
        var result = new IMapObstacle[obstacles.Count];
        for (int index = 0; index < obstacles.Count; index++)
        {
            ManualCoarsePathObstacle obstacle = obstacles[index] ??
                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
            switch (obstacle.Kind)
            {
                case ManualCoarsePathObstacleKind.Circle:
                    result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
                        ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
                        ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
                    break;
                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
                    double halfX = obstacle.SizeXMillimeters / 2d;
                    double halfY = obstacle.SizeYMillimeters / 2d;
                    result[index] = new AxisAlignedRectangleObstacle(
                        ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
                        ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
                        ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
                        ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
                    break;
                default:
                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
            }
        }
        return result;
    }

    private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
    {
        return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
            headingDegrees * DegreesToRadians);
    }

    private static double NormalizeRadians(double angle)
    {
        double normalized = angle % (2d * Math.PI);
        if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
        return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
    }

    private static float ToGridLowerBound(double millimeters)
    {
        double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
        return ToFiniteFloat(rounded, nameof(millimeters));
    }

    private static float ToGridUpperBound(double millimeters)
    {
        double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
        return ToFiniteFloat(rounded, nameof(millimeters));
    }

    private static float ToFiniteFloat(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
            throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
        return (float)value;
    }

    private static void EnsureFinite(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
    }

    private static void EnsurePositiveFinite(double value, string parameterName)
    {
        EnsureFinite(value, parameterName);
        if (value <= 0d)
            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
    }
}