// 此文件是供“停车机器人-合并测试”闭环测试维护者复制阅读的替换示例，
// 不属于可执行项目，也不读取实车定位或向底盘发送命令。
// 只允许替换轨迹来源；状态源、控制参数、观测器和安全策略仍由原测试拥有。

using System;
using System.Threading;
using MultiWheelC.StateEstimation;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using MyParking.Shared;
using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D;

namespace MultiWheelC.TrajectoryPlanning.Guide;

/// <summary>
/// 闭环控制测试中从人工轨迹切换到 EM 正式轨迹的逐段对照。
/// </summary>
public static class EmPlannerTrajectoryReplacementExample
{
    // 替换前：NewControllerTrackingTests 中的完整人工测试轨迹来源。
    // 它只生成控制器测试数据，不调用正式规划器、地图构建、粗路径、平滑或 EM 服务。
    private static void ArtificialTrajectorySource(
        Pose2D trajectoryStartPose,
        CancellationToken cancellationToken)
    {
        _ = cancellationToken; // 此人工来源不使用取消令牌，保留形参仅便于与替换后位置对照。

        Trajectory2D trajectory =
            TestTrajectoryFactory.CreateStraight4Meters(
                trajectoryStartPose,
                CruiseSpeedMetersPerSecond,
                AccelerationMetersPerSecondSquared,
                DecelerationMetersPerSecondSquared,
                PointSpacingMeters);

        _ = trajectory;
    }

    // 替换后（推荐）：复用 Task 2 的固定离线完整管线输出。
    // Task 2 内部同步调用 EmPlanningService.Plan(request, cancellationToken)，
    // 并在返回前完成 Success/SuccessWithFallback、非空轨迹和可控点的发布闸门检查。
    private static void WrapperReplacement(
        IVehicleStateProvider stateProvider,
        CancellationToken cancellationToken)
    {
        FixedOfflinePlanningOutput planningOutput =
            EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory(
                cancellationToken);

        EmTrajectory emTrajectory = planningOutput.EmTrajectory;
        Trajectory2D trajectory = planningOutput.ControllerTrajectory;

        if (emTrajectory == null || trajectory == null)
        {
            throw new InvalidOperationException(
                "固定离线 EM 包装器未返回完整控制轨迹。");
        }

        RejectGearSwitchForSingleTrackingMovement(emTrajectory);

        // 原测试已有的状态源继续交给动作；仅 trajectory 的来源由人工数据改为 EM 输出。
        var movement = new TrajectoryTrackingMovement
        {
            Trajectory = trajectory,
            StateProvider = stateProvider
        };

        // 若原测试使用组合动作计划，则使用同一条已适配的控制轨迹创建段。
        var segment = new TrackMotionPlanSegment(trajectory);

        _ = movement;
        _ = segment;
    }

    // 直接服务方案：当测试已按当前地图、车辆、状态和活动方向段构造冻结的 EmPlanningRequest 时使用。
    // “请求”就是进程内同步方法调用：Plan 的返回值就是本次 EmPlanningResult；
    // 没有轮询、HTTP、消息队列或轨迹仓库查询，也不能从其他调用的结果中取轨迹。
    private static void DirectServiceReplacement(
        EmPlanningRequest request,
        IVehicleStateProvider stateProvider,
        CancellationToken cancellationToken)
    {
        var planningService = new EmPlanningService(new OsqpNativeSolver());
        EmPlanningResult result = planningService.Plan(request, cancellationToken);

        bool succeeded = result.Status == EmPlanningStatus.Success ||
            result.Status == EmPlanningStatus.SuccessWithFallback;
        if (!succeeded || result.Trajectory == null)
        {
            // 失败、取消、超时或空轨迹不得退化为人工轨迹后继续执行；由测试自己的安全策略停止或报告。
            throw new InvalidOperationException(
                "EM 规划未返回可交给控制器的完整轨迹：" + result.FailureReason);
        }

        EmTrajectory emTrajectory = result.Trajectory;
        RejectGearSwitchForSingleTrackingMovement(emTrajectory);

        // 适配器使用 EM 的世界位姿、车辆曲率和有符号纵向速度，重建 Trajectory2D 的弧长。
        Trajectory2D trajectory =
            new EmControlTrajectoryAdapter().Create(result.Trajectory);

        // 直接单段消费路径：不要在这里覆盖原测试已有的控制器调参、完成容差、观测器或安全检查。
        var movement = new TrajectoryTrackingMovement
        {
            Trajectory = trajectory,
            StateProvider = stateProvider
        };

        // 组合计划消费路径：原测试的 ConfigureTrackingMovement、每段 finish tolerances、
        // SegmentStarted/TrackingCycleObserver 等观测器以及状态与安全检查仍保持测试所有权。
        var segment = new TrackMotionPlanSegment(trajectory);

        _ = movement;
        _ = segment;
    }

    // 单个 TrajectoryTrackingMovement 只能消费一个方向段。
    // GearSwitch 表示该段末端必须先停车；除非外层已实现并经安全评审的“停车—方向确认—请求下一段”序列，
    // 否则拒绝它，绝不能把前进/倒车点拼接进同一个 Trajectory2D 后连续跟踪。
    private static void RejectGearSwitchForSingleTrackingMovement(
        EmTrajectory emTrajectory)
    {
        if (emTrajectory == null)
        {
            throw new InvalidOperationException(
                "EM 规划输出为空，不能创建 TrajectoryTrackingMovement。");
        }

        if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
        {
            throw new InvalidOperationException(
                "EmTerminalType.GearSwitch 必须由已安全评审的停车、方向确认和下一段规划序列处理；" +
                "单个 TrajectoryTrackingMovement 不得跨越换向边界。");
        }
    }

    // CruiseSpeedMetersPerSecond、AccelerationMetersPerSecondSquared、
    // DecelerationMetersPerSecondSquared 和 PointSpacingMeters 是外部现有测试类的成员；
    // 复制替换时保留其原值和测试所有权，本说明文件不会重新定义或修改它们。
}