using System;
using System.Collections.Generic;
using System.Drawing;
using System.Globalization;
using System.Threading;
using System.Threading.Tasks;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Pilot;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC;

/// <summary>
/// 显式空图的粗路径规划测试入口。
/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
/// </summary>
[MovementTest(name = "粗路径规划-显式空图")]
public sealed class CoarsePathExplicitEmptyTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 单矩形绕行的粗路径规划测试入口。
/// </summary>
[MovementTest(name = "粗路径规划-单矩形绕行")]
// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
public sealed class CoarsePathRectangleDetourTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
/// </summary>
[MovementTest(name = "粗路径规划-多来源障碍")]
public sealed class CoarsePathManualAndTwoLegTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 重复输入地图缓存命中的粗路径规划测试入口。
/// </summary>
[MovementTest(name = "粗路径规划-缓存命中")]
public sealed class CoarsePathCacheHitTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
/// </summary>
[MovementTest(name = "粗路径规划-倒车换向")]
public sealed class CoarsePathReverseGearSwitchTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
/// </summary>
[MovementTest(name = "粗路径规划-无解")]
public sealed class CoarsePathNoFeasiblePathTest : MovementTest
{
    /// <inheritdoc />
    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
}

/// <summary>
/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
/// </summary>
[MovementTest(name = "粗路径规划")]
public sealed class CoarsePathPlanningTest : MovementTest
{
    private const int MaximumManualObstacleCount = 20;
    private static long _nextManualObstacleSnapshotVersion;

    /// <summary>
    /// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
    /// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
    /// </summary>
    public override void Test()
    {
        try
        {
            var amrPose = DetourInterface.getCartLocation();
            double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
            double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
            double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
            TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
            IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
            long snapshotVersion = obstacles.Count == 0 ? 0L :
                Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
            CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
                amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
                obstacles, snapshotVersion);
            job.Configuration.SearchTimeout = searchTimeout;
            CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
        }
        catch (Exception exception)
        {
            CoarsePathPlanningTestRunner.ShowInputFailure(exception);
        }
    }

    /// <inheritdoc />
    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();

    private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
    {
        int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
        var obstacles = new List<ManualCoarsePathObstacle>(count);
        for (int index = 0; index < count; index++)
        {
            string label = "障碍物 " + (index + 1);
            int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
            double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
            double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
            if (kind == 1)
            {
                double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
                obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
            }
            else
            {
                double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
                double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
                obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
                    lengthXmm, widthYmm));
            }
        }
        return obstacles;
    }

    private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
    {
        object raw = UI.GetInput(prompt);
        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
        int value;
        if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
            !int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
            throw new ArgumentException("输入必须是整数：" + prompt);
        if (value < minimum || value > maximum)
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
        return value;
    }

    private static double ReadPositiveFiniteInput(string prompt)
    {
        double value = ReadFiniteInput(prompt);
        if (value <= 0d)
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
        return value;
    }

    private static TimeSpan ReadPositiveTimeoutInput(string prompt)
    {
        double timeoutSeconds = ReadFiniteInput(prompt);
        if (timeoutSeconds <= 0d)
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
        try
        {
            return TimeSpan.FromSeconds(timeoutSeconds);
        }
        catch (OverflowException)
        {
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
        }
    }

    private static double ReadFiniteInput(string prompt)
    {
        object raw = UI.GetInput(prompt);
        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
        double value;
        if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
            !double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
            throw new ArgumentException("输入必须是有限数字：" + prompt);
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentException("输入必须是有限数字：" + prompt);
        return value;
    }
}

/// <summary>
/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
/// </summary>
internal static class CoarsePathPlanningTestRunner
{
    private const string PainterLayerName = "CoarsePathPlanningV1";
    private const float MillimetersPerMeter = 1000f;
    private const int MaximumVisibleGridLines = 100;

    private static readonly object SessionSync = new object();
    private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
    private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);

    private static CancellationTokenSource _activeCancellation;
    private static Task<CoarsePathPlanningJobResult> _activeTask;
    private static long _nextSessionId;
    private static long _activeSessionId;

    /// <summary>
    /// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
    /// </summary>
    private sealed class AmrPoseSnapshot
    {
        public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
        {
            EnsureFiniteAmrValue(xMillimeters, "X");
            EnsureFiniteAmrValue(yMillimeters, "Y");
            EnsureFiniteAmrValue(headingDegrees, "航向");
            XMillimeters = xMillimeters;
            YMillimeters = yMillimeters;
            HeadingDegrees = headingDegrees;
        }

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

        public string DisplayText
        {
            get
            {
                return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
                    " mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
                    " mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
            }
        }
    }

    /// <summary>
    /// 创建指定固定场景并将其提交给共享后台服务。
    /// </summary>
    internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
    {
        try
        {
            var pose = DetourInterface.getCartLocation();
            if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
            var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
            CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
                snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
            Run(scenarioName, job, snapshot);
        }
        catch (Exception exception)
        {
            ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
        }
    }

    /// <summary>
    /// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
    /// </summary>
    internal static void Run(string scenarioName, CoarsePathPlanningJob job)
    {
        Run(scenarioName, job, null);
    }

    private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
    {
        if (job == null) throw new ArgumentNullException(nameof(job));

        var cancellation = new CancellationTokenSource();
        CancellationTokenSource previousCancellation;
        long sessionId;
        lock (SessionSync)
        {
            previousCancellation = _activeCancellation;
            _activeCancellation = cancellation;
            _activeTask = null;
            sessionId = ++_nextSessionId;
            _activeSessionId = sessionId;
        }

        // 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
        if (previousCancellation != null) previousCancellation.Cancel();
        Painter.Clear();
        DrawPending(scenarioName, job, amrPose);

        Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
        lock (SessionSync)
        {
            if (_activeSessionId == sessionId) _activeTask = task;
        }

        _ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
            TaskScheduler.Default);
    }

    /// <summary>
    /// 取消当前会话并清空专用图层；不等待后台任务结束。
    /// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
    /// </summary>
    internal static void Stop()
    {
        CancellationTokenSource cancellation;
        lock (SessionSync)
        {
            cancellation = _activeCancellation;
            _activeCancellation = null;
            _activeTask = null;
            _activeSessionId = 0;
        }

        if (cancellation != null) cancellation.Cancel();
        Painter.Clear();
        Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
    }

    /// <summary>
    /// 显示输入读取或校验失败，且不改变现有规划任务。
    /// </summary>
    internal static void ShowInputFailure(Exception exception)
    {
        string message = exception == null ? "未知输入错误。" : exception.Message;
        Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
        Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
    }

    private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
        CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
    {
        try
        {
            CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
            bool isCurrent;
            lock (SessionSync)
            {
                isCurrent = _activeSessionId == sessionId;
                if (isCurrent)
                {
                    _activeTask = null;
                    _activeCancellation = null;
                }
            }

            if (!isCurrent) return;
            DrawResult(scenarioName, job, amrPose, result);
            Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
        }
        catch (Exception exception)
        {
            bool isCurrent;
            lock (SessionSync)
            {
                isCurrent = _activeSessionId == sessionId;
                if (isCurrent)
                {
                    _activeTask = null;
                    _activeCancellation = null;
                }
            }

            if (isCurrent)
                Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
                    PainterLayerName);
        }
        finally
        {
            cancellation.Dispose();
        }
    }

    private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
    {
        DrawPose(job.Start, Color.LimeGreen, "起点");
        DrawPose(job.Goal, Color.Orange, "终点");
        Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
        if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
    }

    private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
        CoarsePathPlanningJobResult result)
    {
        Painter.Clear();
        PlanningGridMap map = result.MapResult.Map;
        if (map != null) DrawMap(map);

        DrawPose(job.Start, Color.LimeGreen, "起点");
        DrawGoal(job.Goal, job.Configuration, Color.Orange);
        if (result.PlanningResult.Status == PlanningStatus.Success)
            DrawPath(result.PlanningResult, job.Vehicle);

        DrawLegend(map);
        DrawStatus(scenarioName, job, result, map, amrPose);
    }

    private static void DrawMap(PlanningGridMap map)
    {
        float xMin = map.Bounds.XMin;
        float xMax = map.Bounds.XMax;
        float yMin = map.Bounds.YMin;
        float yMax = map.Bounds.YMax;
        float resolution = map.ResolutionMm;
        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));

        // 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
        for (int col = 0; col <= map.Cols; col += gridStride)
        {
            float x = Math.Min(xMax, xMin + col * resolution);
            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
        }
        for (int row = 0; row <= map.Rows; row += gridStride)
        {
            float y = Math.Min(yMax, yMin + row * resolution);
            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
        }

        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
        if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
        if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);

        for (int row = 0; row < map.Rows; row++)
        {
            for (int col = 0; col < map.Cols; col++)
            {
                if (!map.IsOccupied(row, col)) continue;
                float x = xMin + col * resolution + resolution / 2f;
                float y = yMin + row * resolution;
                Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
                    width: Math.Max(1, (int)Math.Round(resolution)));
            }
        }
    }

    private static void DrawPose(Pose2D pose, Color color, string label)
    {
        if (pose == null) return;
        float x = ToMillimeters(pose.X);
        float y = ToMillimeters(pose.Y);
        Painter.DrawCircle(color, x, y, 80f);
        DrawHeadingArrow(x, y, pose.Heading, color, 260f);
        Painter.DrawText(color, label, x + 100f, y + 100f);
    }

    private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
    {
        DrawPose(goal, color, "终点");
        if (goal == null || configuration == null) return;
        Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
            ToMillimeters(configuration.GoalPositionToleranceMeters));
    }

    private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
    {
        if (planningResult.Path == null || planningResult.Path.Count == 0) return;

        int frameStride = Math.Max(1, planningResult.Path.Count / 10);
        for (int index = 1; index < planningResult.Path.Count; index++)
        {
            CoarsePathPoint previous = planningResult.Path[index - 1];
            CoarsePathPoint current = planningResult.Path[index];
            Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
            Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
                ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);

            if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
                DrawVehicleFrame(current, vehicle);
            if (index % Math.Max(1, frameStride / 2) == 0)
                DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
                    current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
            if (!current.IsGearSwitchPoint) continue;

            float x = ToMillimeters(current.X);
            float y = ToMillimeters(current.Y);
            Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
            Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
        }

        DrawVehicleFrame(planningResult.Path[0], vehicle);
    }

    private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
    {
        if (point == null || vehicle == null) return;
        float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
        float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
        float centerX = ToMillimeters(point.X);
        float centerY = ToMillimeters(point.Y);
        double cos = Math.Cos(point.Heading);
        double sin = Math.Sin(point.Heading);

        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
        Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
        Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
        Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
        Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
    }

    private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
        float longitudinal, float lateral, out float x, out float y)
    {
        x = centerX + (float)(cos * longitudinal - sin * lateral);
        y = centerY + (float)(sin * longitudinal + cos * lateral);
    }

    private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
    {
        float endX = x + (float)Math.Cos(headingRadians) * length;
        float endY = y + (float)Math.Sin(headingRadians) * length;
        Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
    }

    private static void DrawLegend(PlanningGridMap map)
    {
        float x = map == null ? 0f : map.Bounds.XMin + 150f;
        float y = map == null ? 250f : map.Bounds.YMax - 180f;
        Painter.DrawText(Color.White, "图例", x, y);
        DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
        DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
        DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
        DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
        DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
        DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
        DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
    }

    private static void DrawLegendItem(float x, float y, Color color, string text)
    {
        Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
        Painter.DrawText(color, text, x + 120f, y - 30f);
    }

    private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
    {
        float x = map == null ? 0f : map.Bounds.XMin + 150f;
        float y = map == null ? -250f : map.Bounds.YMin + 150f;
        string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
        string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
        string reason = diagnostics.TerminationReason ?? string.Empty;
        string turningRadius = "无";
        if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
            turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
        Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
        float detailOffset = 0f;
        if (amrPose != null)
        {
            Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
            detailOffset = 120f;
        }
        Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
            x, y + 120f + detailOffset);
        Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
        Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
            "，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
            diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
        if (job != null && job.Vehicle != null)
        {
            Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
                " m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
                job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
                turningRadius, x, y + 480f + detailOffset);
        }
        if (!string.IsNullOrEmpty(reason))
            Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
    }

    private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
    {
        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
        string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
            result.PlanningResult.Status + "，总耗时=" +
            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
        if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
            message += "，原因=" + diagnostics.TerminationReason;
        return message + "。";
    }

    private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
    {
        Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
        Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
        Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
        Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
    }

    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);

    private static void EnsureFiniteAmrValue(double value, string name)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentException(name + " 必须是有限数。");
    }
}