using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using System.Threading.Tasks;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Clumsy.Pilot;
using MultiWheelC.Trajectory;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using MyParking.Shared;
using TrajectoryPlanningVisualization;
using PlanningPose2D = MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D;

namespace MultiWheelC;

/// <summary>规划一次并冻结首个 EM 方向段，再使用现有几何控制器完成实车状态反馈闭环。</summary>
[MovementTest(name = "EM闭环测试")]
public sealed class EmClosedLoopMovementTest : MovementTest
{
    private const int MaximumManualObstacleCount = 20;
    private static long nextObstacleSnapshotVersion;

    public double GoalXmm = double.NaN;
    public double GoalYmm = double.NaN;
    public double GoalYawDeg;
    public double MapPaddingMeters = 2d;
    public float MapResolutionMm = 50f;
    public double SolverTimeoutSeconds = 5d;
    public int MaximumOsqpIterations = 100000;
    public double OutputTimeStepSeconds = 0.10d;
    public double VehicleLengthMeters = 0.80d;
    public double VehicleWidthMeters = 0.60d;
    public double SafetyMarginMeters = 0.05d;
    public double MaximumCurvaturePerMeter = 1d / 1.20d;
    public bool EnableWebVisualization = true;
    public bool AutoOpenWebVisualization = true;
    public int WebVisualizationPort;
    public double WebRefreshRateHz = 10d;
    public int VisualizationHistoryCycleLimit = 60;
    public bool EnableNativePainterVisualization = true;
    public double MaximumCommandSpeedMetersPerSecond = 1.00d;
    public double MaximumDistanceToTrajectoryMeters = 0.30d;
    public double ExecutionTimeoutSeconds = 120d;
    public float WheelAlignmentToleranceDegrees = 2f;

    /// <summary>读取冻结输入并启动一次规划、一次控制器接管的后台会话。</summary>
    public override void Test()
    {
        try
        {
            double goalXMillimeters = GoalXmm;
            double goalYMillimeters = GoalYmm;
            double goalYawDegrees = GoalYawDeg;
            if (!IsFinite(goalXMillimeters) || !IsFinite(goalYMillimeters))
            {
                goalXMillimeters = ReadFiniteInput("EM闭环终点 X（世界 mm）");
                goalYMillimeters = ReadFiniteInput("EM闭环终点 Y（世界 mm）");
                goalYawDegrees = ReadFiniteInput("EM闭环终点航向（世界 deg）");
            }

            EnsureFinite(goalXMillimeters, nameof(GoalXmm));
            EnsureFinite(goalYMillimeters, nameof(GoalYmm));
            EnsureFinite(goalYawDegrees, nameof(GoalYawDeg));
            EnsurePositive(MaximumCommandSpeedMetersPerSecond,
                nameof(MaximumCommandSpeedMetersPerSecond));
            EnsurePositive(MaximumDistanceToTrajectoryMeters,
                nameof(MaximumDistanceToTrajectoryMeters));
            EnsurePositive(ExecutionTimeoutSeconds, nameof(ExecutionTimeoutSeconds));
            EnsurePositive(WheelAlignmentToleranceDegrees,
                nameof(WheelAlignmentToleranceDegrees));

            var settings = new TrajectoryObservationSettings
            {
                PlanningScope = EmPlanningScope.FullDirectionSegment,
                MapPaddingMeters = MapPaddingMeters,
                MapResolutionMillimeters = MapResolutionMm,
                SolverTimeoutSeconds = SolverTimeoutSeconds,
                MaximumOsqpIterations = MaximumOsqpIterations,
                OutputTimeStepSeconds = OutputTimeStepSeconds,
                VehicleLengthMeters = VehicleLengthMeters,
                VehicleWidthMeters = VehicleWidthMeters,
                SafetyMarginMeters = SafetyMarginMeters,
                MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
                EnableWebVisualization = EnableWebVisualization,
                AutoOpenWebVisualization = AutoOpenWebVisualization,
                WebVisualizationPort = WebVisualizationPort,
                WebRefreshRateHz = WebRefreshRateHz,
                VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
                EnableNativePainterVisualization = EnableNativePainterVisualization,
            }.CreateValidatedSnapshot();

            IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
            long obstacleSnapshotVersion = obstacles.Count == 0
                ? 0L
                : Interlocked.Increment(ref nextObstacleSnapshotVersion);
            var goal = new PlanningPose2D(
                goalXMillimeters / 1000d,
                goalYMillimeters / 1000d,
                goalYawDegrees * Math.PI / 180d);

            EmClosedLoopMovementTestRunner.Start(goal, settings, obstacles,
                obstacleSnapshotVersion, MaximumCommandSpeedMetersPerSecond,
                MaximumDistanceToTrajectoryMeters, ExecutionTimeoutSeconds,
                WheelAlignmentToleranceDegrees);
        }
        catch (Exception exception)
        {
            EmClosedLoopMovementTestRunner.ShowFailure("测试未启动", exception);
        }
    }

    /// <summary>取消尚未完成的规划并停止正在运行的控制任务。</summary>
    public override void TestStop()
    {
        EmClosedLoopMovementTestRunner.Stop();
    }

    private static IReadOnlyList<TrajectoryObservationObstacle> ReadManualObstacles()
    {
        int count = ReadBoundedIntegerInput("EM闭环手动障碍物数量（0-20）",
            0, MaximumManualObstacleCount);
        var obstacles = new List<TrajectoryObservationObstacle>(count);
        for (int index = 0; index < count; index++)
        {
            string label = "障碍物 " + (index + 1).ToString(CultureInfo.InvariantCulture);
            int kind = ReadBoundedIntegerInput(label + " 类型（1=圆形，2=轴对齐矩形）", 1, 2);
            double centerXMillimeters = ReadFiniteInput(label + " 中心 X（世界 mm）");
            double centerYMillimeters = ReadFiniteInput(label + " 中心 Y（世界 mm）");
            if (kind == 1)
            {
                double radiusMillimeters = ReadPositiveFiniteInput(label + " 半径（mm）");
                obstacles.Add(TrajectoryObservationObstacle.Circle(
                    centerXMillimeters, centerYMillimeters, radiusMillimeters));
            }
            else
            {
                double lengthXMillimeters = ReadPositiveFiniteInput(label + " X 方向长度（mm）");
                double widthYMillimeters = ReadPositiveFiniteInput(label + " Y 方向宽度（mm）");
                obstacles.Add(TrajectoryObservationObstacle.Rectangle(
                    centerXMillimeters - lengthXMillimeters / 2d,
                    centerXMillimeters + lengthXMillimeters / 2d,
                    centerYMillimeters - widthYMillimeters / 2d,
                    centerYMillimeters + widthYMillimeters / 2d));
            }
        }

        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);
        EnsurePositive(value, prompt);
        return value;
    }

    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);
        }

        EnsureFinite(value, prompt);
        return value;
    }

    private static void EnsurePositive(double value, string name)
    {
        EnsureFinite(value, name);
        if (value <= 0d)
            throw new ArgumentOutOfRangeException(name, "输入必须是正数：" + name);
    }

    private static void EnsureFinite(double value, string name)
    {
        if (!IsFinite(value))
            throw new ArgumentException("输入必须是有限数字：" + name);
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}

/// <summary>拥有单次规划和单个控制任务的会话生命周期，并保证停止操作幂等。</summary>
internal static class EmClosedLoopMovementTestRunner
{
    private static readonly object SessionSync = new object();
    private static CancellationTokenSource activeCancellation;
    private static Task activeTask;
    private static DriveTask activeDriveTask;
    private static TrajectoryObservationPresentation activePresentation;
    private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
    private static long nextSessionId;
    private static long activeSessionId;
    private static long stateSequence;

    internal static void Start(PlanningPose2D goal, TrajectoryObservationSettings settings,
        IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
        double maximumCommandSpeedMetersPerSecond,
        double maximumDistanceToTrajectoryMeters,
        double executionTimeoutSeconds,
        float wheelAlignmentToleranceDegrees)
    {
        if (goal == null) throw new ArgumentNullException(nameof(goal));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));

        var cancellation = new CancellationTokenSource();
        CancellationTokenSource previousCancellation;
        DriveTask previousDriveTask;
        TrajectoryObservationPresentation previousPresentation;
        TrajectoryObservationVisualizationPublisher previousWebPublisher;
        long sessionId;
        lock (SessionSync)
        {
            previousCancellation = activeCancellation;
            previousDriveTask = activeDriveTask;
            previousPresentation = activePresentation;
            previousWebPublisher = activeWebPublisher;
            activeCancellation = cancellation;
            activeDriveTask = null;
            activePresentation = null;
            activeWebPublisher = null;
            activeTask = null;
            activeSessionId = sessionId = ++nextSessionId;
        }

        Cancel(previousCancellation);
        StopDriveTask(previousDriveTask);
        ClearPresentation(previousPresentation);
        StopWebPublisher(previousWebPublisher);
        PrintStatus("会话 " + sessionId.ToString(CultureInfo.InvariantCulture) +
            " 已启动；车辆运动前将只规划一次。");

        Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings,
            obstacles, obstacleSnapshotVersion, maximumCommandSpeedMetersPerSecond,
            maximumDistanceToTrajectoryMeters, executionTimeoutSeconds,
            wheelAlignmentToleranceDegrees, cancellation.Token), cancellation.Token);
        lock (SessionSync)
        {
            if (activeSessionId == sessionId)
                activeTask = task;
        }

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

    internal static void Stop()
    {
        CancellationTokenSource cancellation;
        DriveTask driveTask;
        TrajectoryObservationPresentation presentation;
        TrajectoryObservationVisualizationPublisher webPublisher;
        lock (SessionSync)
        {
            cancellation = activeCancellation;
            driveTask = activeDriveTask;
            presentation = activePresentation;
            webPublisher = activeWebPublisher;
            activeCancellation = null;
            activeDriveTask = null;
            activePresentation = null;
            activeWebPublisher = null;
            activeTask = null;
            activeSessionId = 0L;
        }

        Cancel(cancellation);
        StopDriveTask(driveTask);
        ClearPresentation(presentation);
        StopWebPublisher(webPublisher);
        PrintStatus("已请求取消规划并停止控制任务。");
    }

    internal static void ShowFailure(string context, Exception exception)
    {
        PrintStatus(context + "：" + (exception == null ? "未知错误" : exception.Message));
    }

    private static async Task RunSessionAsync(long sessionId, PlanningPose2D goal,
        TrajectoryObservationSettings settings,
        IReadOnlyList<TrajectoryObservationObstacle> obstacles,
        long obstacleSnapshotVersion,
        double maximumCommandSpeedMetersPerSecond,
        double maximumDistanceToTrajectoryMeters,
        double executionTimeoutSeconds,
        float wheelAlignmentToleranceDegrees,
        CancellationToken token)
    {
        token.ThrowIfCancellationRequested();
        VehicleMotionState initialState = ReadPlanningState();
        using var planningDeadline = new TrajectoryObservationPlanningDeadline(
            TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), token);
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
        TrajectoryObservationBootstrapResult bootstrap =
            new TrajectoryObservationBootstrapper().Bootstrap(job, planningDeadline, token);
        if (!bootstrap.Succeeded)
            throw new InvalidOperationException(bootstrap.FailureReason);

        var controller = new TrajectoryObservationController(
            bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
            "em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
        PlanningCycleResult cycle = await controller.StartCycle(
            initialState.CapturedAtUtc, initialState, planningDeadline, token).ConfigureAwait(false);
        if (!cycle.Published || controller.PublishedTrajectory == null)
        {
            string reason = cycle.Result == null ? string.Empty : cycle.Result.FailureReason;
            throw new InvalidOperationException(string.IsNullOrWhiteSpace(reason)
                ? cycle.Diagnostic
                : reason);
        }

        EmTrajectory emTrajectory = controller.PublishedTrajectory;
        Trajectory2D controlTrajectory = new EmControlTrajectoryAdapter().Create(emTrajectory);
        TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualization(
            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion, token);
        TrajectoryObservationPresentation presentation = settings.EnableNativePainterVisualization
            ? new TrajectoryObservationPresentation()
            : null;
        if (!TryRegisterPresentation(sessionId, presentation, token))
        {
            ClearPresentation(presentation);
            token.ThrowIfCancellationRequested();
            return;
        }
        UpdateVisualization(sessionId, presentation, bootstrap, controller, emTrajectory,
            initialState, cycle, "规划已冻结，尚未向底盘下发控制指令。");
        PrintStatus("规划完成：EM点数=" + emTrajectory.Points.Count.ToString(CultureInfo.InvariantCulture) +
            "，控制点数=" + controlTrajectory.Count.ToString(CultureInfo.InvariantCulture) +
            "，长度=" + controlTrajectory.TotalLengthMeters.ToString("F3", CultureInfo.InvariantCulture) +
            " m，方向=" + emTrajectory.Metadata.Direction +
            "，终端=" + emTrajectory.Metadata.TerminalType + "。");

        token.ThrowIfCancellationRequested();
        EmClosedLoopWheelSafety.RequireWheelsForward(wheelAlignmentToleranceDegrees);
        var movement = new TrajectoryTrackingMovement
        {
            Trajectory = controlTrajectory,
            MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
            MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
            ExecutionTimeoutSeconds = executionTimeoutSeconds,
            CycleObserver = control => UpdatePresentationFromControlCycle(
                sessionId, presentation, bootstrap, controller, emTrajectory, cycle, control),
        };
        var driveTask = new DriveTask(movement.Get());
        if (!TryRegisterDriveTask(sessionId, driveTask, token))
        {
            StopDriveTask(driveTask);
            token.ThrowIfCancellationRequested();
            return;
        }

        try
        {
            PrintStatus("冻结轨迹已交给现有控制器，开始单方向段闭环跟踪。");
            driveTask.Wait();
            if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
                PrintStatus("已在换向边界停车；本测试不启动下一方向段。");
            else
                PrintStatus("首个方向段已完成并停车。");
        }
        finally
        {
            StopDriveTask(driveTask);
            ClearDriveTask(sessionId, driveTask);
        }
    }

    private static void UpdatePresentationFromControlCycle(long sessionId,
        TrajectoryObservationPresentation presentation,
        TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationController planningController,
        EmTrajectory trajectory,
        PlanningCycleResult cycle,
        MultiWheelC.Control.Execution.ParkingGeometricController control)
    {
        if (control == null || !control.LastVehicleState.HasValue)
            return;

        var state = control.LastVehicleState.Value;
        var planningState = new VehicleMotionState(
            new PlanningPose2D(state.PoseInWorld.XMeters, state.PoseInWorld.YMeters,
                state.PoseInWorld.YawRadians),
            state.TwistInBody.VxMetersPerSecond, null, DateTimeOffset.UtcNow,
            Interlocked.Increment(ref stateSequence));
        string diagnostic = "现有控制器正在执行冻结轨迹";
        if (control.LastCommand.HasValue)
        {
            var command = control.LastCommand.Value;
            diagnostic += "：底盘速度=" +
                command.SpeedMetersPerSecond.ToString("F3", CultureInfo.InvariantCulture) +
                " m/s，前GCP=" +
                (command.FrontAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) +
                " deg，后GCP=" +
                (command.RearAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) + " deg。";
        }

        UpdateVisualization(sessionId, presentation, bootstrap, planningController,
            trajectory, planningState, cycle, diagnostic);
    }

    private static void UpdateVisualization(long sessionId,
        TrajectoryObservationPresentation presentation,
        TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationController controller,
        EmTrajectory trajectory,
        VehicleMotionState state,
        PlanningCycleResult cycle,
        string diagnostic)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId)
                return;
            var observation = new TrajectoryObservationObservation(
                DateTimeOffset.UtcNow, state, trajectory, null, null, null);
            DirectionSegmentView segment = controller.ActiveSegment;
            TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
                trajectory, segment,
                controller.CreateEffectiveConfigurationSnapshot().Frenet.MaximumProjectionDistanceMeters);
            if (presentation != null && ReferenceEquals(activePresentation, presentation))
            {
                presentation.DrawWorld(bootstrap, observation,
                    TrajectoryObservationRuntimeState.Create(DateTimeOffset.UtcNow, trajectory), diagnostic);
                presentation.DrawLs(charts, diagnostic);
                presentation.DrawSt(charts, diagnostic);
            }

            TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
            if (publisher != null)
            {
                var tick = new TrajectoryObservationLoopTick(observation, cycle, TimeSpan.Zero,
                    false, false, true, false, controller.SegmentState);
                publisher.TryPublish(observation.ObservedAtUtc, () =>
                {
                    PlanningVisualizationDynamicSnapshot source =
                        new TrajectoryObservationDynamicSnapshotBuilder().Build(
                            Interlocked.Increment(ref stateSequence), tick, segment,
                            controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
                            controller.CreateEffectiveConfigurationSnapshot());
                    return new PlanningVisualizationDynamicSnapshot(source.Sequence,
                        source.ObservedAtUtc, "EM闭环控制中", source.ActiveSegmentIndex,
                        source.ActiveDirection, source.VehiclePose, source.DynamicPolylines,
                        source.DynamicMarkers, source.Charts, source.StatusValues, source.CycleSummary);
                });
            }
        }
    }

    private static TrajectoryObservationVisualizationPublisher StartWebVisualization(
        long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationController controller, TrajectoryObservationSettings settings,
        long obstacleSnapshotVersion, CancellationToken token)
    {
        if (!settings.EnableWebVisualization)
            return null;
        var publisher = new TrajectoryObservationVisualizationPublisher(settings,
            new PlanningVisualizationSessionSink(), PrintStatus);
        try
        {
            PlanningVisualizationStaticSnapshot source =
                new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
                    controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
            var closedLoopSnapshot = new PlanningVisualizationStaticSnapshot("EM闭环测试",
                source.WorldBounds, source.OccupancyGrid, source.StaticPolylines,
                source.StaticMarkers, source.DirectionSegments, source.ConfigurationGroups);
            PlanningVisualizationSessionInfo info = publisher.Start(closedLoopSnapshot);
            if (info == null)
                return null;
            lock (SessionSync)
            {
                if (activeSessionId != sessionId || token.IsCancellationRequested)
                {
                    publisher.Stop();
                    return null;
                }
                activeWebPublisher = publisher;
            }
            PrintStatus("网页可视化地址（含会话令牌）：" + info.Uri.AbsoluteUri);
            if (settings.AutoOpenWebVisualization)
            {
                Process.Start(new ProcessStartInfo
                {
                    FileName = info.Uri.AbsoluteUri,
                    UseShellExecute = true,
                });
            }
            return publisher;
        }
        catch (Exception exception)
        {
            publisher.Disable(exception);
            PrintStatus("网页可视化未启动：" + exception.Message);
            return null;
        }
    }

    private static bool TryRegisterPresentation(long sessionId,
        TrajectoryObservationPresentation presentation, CancellationToken token)
    {
        if (presentation == null)
            return true;
        lock (SessionSync)
        {
            if (activeSessionId != sessionId || activeCancellation == null ||
                activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
                return false;
            activePresentation = presentation;
            return true;
        }
    }

    private static VehicleMotionState ReadPlanningState()
    {
        var location = DetourInterface.getCartLocation();
        if (location == null)
            throw new InvalidOperationException("实时定位不可用。");
        if (BasicPilotBase.Chassis == null)
            throw new InvalidOperationException("实时底盘读接口不可用。");

        var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
        return new VehicleMotionState(
            new PlanningPose2D(location.x / 1000d, location.y / 1000d,
                location.th * Math.PI / 180d),
            speed.Vx, null, DateTimeOffset.UtcNow,
            Interlocked.Increment(ref stateSequence));
    }

    private static bool TryRegisterDriveTask(long sessionId, DriveTask driveTask,
        CancellationToken token)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId || activeCancellation == null ||
                activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
            {
                return false;
            }

            activeDriveTask = driveTask;
            return true;
        }
    }

    private static void ClearDriveTask(long sessionId, DriveTask driveTask)
    {
        lock (SessionSync)
        {
            if (activeSessionId == sessionId && ReferenceEquals(activeDriveTask, driveTask))
                activeDriveTask = null;
                ClearPresentation(activePresentation);
                activePresentation = null;
                StopWebPublisher(activeWebPublisher);
                activeWebPublisher = null;
        }
    }

    private static void Finish(long sessionId, CancellationTokenSource cancellation,
        Task completed)
    {
        lock (SessionSync)
        {
            if (activeSessionId == sessionId)
            {
                activeCancellation = null;
                activeDriveTask = null;
                activeTask = null;
                activeSessionId = 0L;
            }
        }

        if (completed.IsFaulted)
        {
            Exception failure = completed.Exception == null
                ? null
                : completed.Exception.GetBaseException();
            ShowFailure("闭环会话失败并已停车", failure);
        }
        else if (completed.IsCanceled)
        {
            PrintStatus("闭环会话已取消。");
        }

        cancellation.Dispose();
    }

    private static void Cancel(CancellationTokenSource cancellation)
    {
        if (cancellation == null)
            return;
        try
        {
            cancellation.Cancel();
        }
        catch (ObjectDisposedException)
        {
        }
    }

    private static void StopDriveTask(DriveTask driveTask)
    {
        if (driveTask == null)
            return;
        try
        {
            driveTask.Stop();
        }
        catch (Exception exception)
        {
            ShowFailure("停止控制任务失败", exception);
        }
    }

    private static void ClearPresentation(TrajectoryObservationPresentation presentation)
    {
        if (presentation == null)
            return;
        try
        {
            presentation.ClearAll();
        }
        catch (Exception exception)
        {
            ShowFailure("清理闭环可视化失败", exception);
        }
    }

    private static void StopWebPublisher(TrajectoryObservationVisualizationPublisher publisher)
    {
        if (publisher == null)
            return;
        try
        {
            publisher.Stop();
        }
        catch (Exception exception)
        {
            ShowFailure("停止闭环网页可视化失败", exception);
        }
    }

    private static void PrintStatus(string message)
    {
        Console.WriteLine("[EM闭环测试] " + message);
    }
}

/// <summary>在控制器接管底盘前只读检查四个舵轮是否已经与车体前向对齐。</summary>
internal static class EmClosedLoopWheelSafety
{
    internal static void RequireWheelsForward(float toleranceDegrees)
    {
        var chassis = PilotDefinition.Chassis as MultiWheelChassis;
        if (chassis == null)
            throw new InvalidOperationException("当前底盘不是 MultiWheelChassis。");

        var adapter = new MultiWheelChassisAdapter(chassis, PilotDefinition.Self.CarNum);
        double toleranceRadians = AngleMath.DegreesToRadians(toleranceDegrees);
        if (!adapter.AreParallelWheelsAligned(0d, toleranceRadians))
        {
            throw new InvalidOperationException(
                "四个舵轮尚未与车头方向一致，控制器未接管底盘。");
        }
    }
}