using System;
using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D;

namespace MultiWheelC.TrajectoryPlanning.Guide;

/// <summary>固定离线示例的只读输出，同时保留 EM 时间轨迹和控制器几何轨迹。</summary>
public sealed class FixedOfflinePlanningOutput
{
    public FixedOfflinePlanningOutput(
        EmTrajectory emTrajectory, // 来源：EM 发布闸门后的非空时间轨迹。
        Trajectory2D controllerTrajectory) // 来源：同一 EM 轨迹经控制适配器生成的 SI 轨迹。
    {
        EmTrajectory = emTrajectory ??
            throw new ArgumentNullException(nameof(emTrajectory));
        ControllerTrajectory = controllerTrajectory ??
            throw new ArgumentNullException(nameof(controllerTrajectory));
    }

    public EmTrajectory EmTrajectory { get; }

    public Trajectory2D ControllerTrajectory { get; }
}

/// <summary>
/// 展示固定、静态、单方向的离线规划数据流；本类不读取实时定位，也不发送底盘命令。
/// </summary>
public static class EmPlannerFullPipelineDemo
{
    public static FixedOfflinePlanningOutput CreateFixedOfflineTrajectory(
        CancellationToken cancellationToken)
    {
        // 步骤 1：定义固定世界起点和终点
        var startPose = new Pose2D(
            1.50d, // 来源：固定场景起点世界 X；单位 m。
            1.50d, // 来源：固定场景起点世界 Y；单位 m。
            0d); // 来源：固定场景起点车头航向；单位 rad。
        var goalPose = new Pose2D(
            6.50d, // 来源：固定场景终点世界 X；单位 m。
            1.50d, // 来源：固定场景终点世界 Y；单位 m。
            0d); // 来源：固定场景终点车头航向；单位 rad。

        // 步骤 2：定义车辆、曲率和搜索预算
        const double vehicleLengthMeters = 1.00d;
        const double vehicleWidthMeters = 0.60d;
        const double safetyMarginMeters = 0.10d;
        const double maximumCurvaturePerMeter = 0.80d;
        var vehicle = new VehicleParameters
        {
            LengthMeters = vehicleLengthMeters, // 来源：固定车辆本体长度；单位 m。
            WidthMeters = vehicleWidthMeters, // 来源：固定车辆本体宽度；单位 m。
            SafetyMarginMeters = safetyMarginMeters, // 来源：固定碰撞安全余量；单位 m。
            MaximumCurvaturePerMeter = maximumCurvaturePerMeter, // 来源：固定运动学上限；单位 1/m。
            MinimumTurningRadiusMeters = null, // 来源：本示例已直接给出最大曲率；单位 m。
        };
        var coarseConfiguration = new HybridAStarConfiguration
        {
            PrimitiveLengthMeters = 0.40d, // 来源：固定搜索原语长度；单位 m。
            IntegrationStepMeters = 0.05d, // 来源：固定原语积分间距；单位 m。
            MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定连续碰撞检查步长；单位 m。
            HeadingResolutionRadians = Math.PI / 36d, // 来源：固定航向离散；单位 rad。
            CurvatureLevelCount = 5, // 来源：固定曲率离散级数；无单位。
            GoalPositionToleranceMeters = 0.15d, // 来源：固定终点位置容差；单位 m。
            GoalHeadingToleranceRadians = Math.PI / 36d, // 来源：固定终点航向容差；单位 rad。
            MaximumExpandedNodes = 100000, // 来源：固定节点预算；单位为节点数。
            SearchTimeout = TimeSpan.FromSeconds(5d), // 来源：固定搜索时间预算；单位 s。
            HeuristicWeight = 1d, // 来源：固定启发式权重；无单位。
            ReverseCostMultiplier = 1.5d, // 来源：固定倒车代价倍率；无单位。
            GearSwitchPenaltyMeters = 1d, // 来源：固定换向惩罚；单位 m 等价代价。
            CurvatureMagnitudeWeight = 0.10d, // 来源：固定曲率代价权重；无单位。
            CurvatureChangePenaltyMetersPerLevel = 0.05d, // 来源：固定曲率变化惩罚；单位 m/级。
            ClearanceCostWeight = 0.20d, // 来源：固定净空代价权重；无单位。
            ClearanceCostDistanceMeters = 0.50d, // 来源：固定净空代价作用距离；单位 m。
            AllowReverse = false, // 来源：固定场景只允许前进；无单位。
        };

        // 步骤 3：建立静态地图
        var bounds = new MapBoundsMm(
            0f, // 来源：固定地图 X 下界；单位 mm。
            8000f, // 来源：固定地图 X 上界；单位 mm。
            0f, // 来源：固定地图 Y 下界；单位 mm。
            5000f); // 来源：固定地图 Y 上界；单位 mm。
        const float resolutionMm = 50f;
        IReadOnlyList<IMapObstacle> obstacles = new IMapObstacle[]
        {
            new CircleObstacle(
                4000f, // 来源：固定圆障碍圆心 X；单位 mm。
                3500f, // 来源：固定圆障碍圆心 Y；单位 mm。
                400f), // 来源：固定圆障碍半径；单位 mm。
        };
        var obstacleSource = new ManualObstacleSource(
            "fixed-offline-obstacles", // 来源：固定障碍快照身份；无单位。
            1L, // 来源：固定障碍快照版本；非负版本号。
            true, // 来源：障碍来源必须成功投影；无单位。
            obstacles); // 来源：上方固定世界障碍列表；几何坐标单位 mm。
        var mapRequest = new PlanningMapRequest
        {
            Bounds = bounds, // 来源：固定地图边界；单位 mm。
            ResolutionMm = resolutionMm, // 来源：固定栅格边长；单位 mm。
            ObstacleSources = new IMapObstacleSource[] { obstacleSource }, // 来源：固定障碍来源快照。
            AllowExplicitEmptyMap = false, // 来源：本场景要求障碍物真实参与建图；无单位。
        };
        PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
        if (!mapResult.Succeeded || mapResult.Map == null || !mapResult.Map.PlanningReady)
        {
            string reason = mapResult.Map != null && !mapResult.Map.PlanningReady
                ? mapResult.Map.PlanningBlockReason
                : mapResult.FailureReason;
            throw new InvalidOperationException("固定离线地图不可用于规划：" + reason);
        }
        PlanningGridMap map = mapResult.Map;

        // 步骤 4：建立粗路径请求
        var coarseRequest = new PlanningRequest
        {
            Map = map, // 来源：步骤 3 的冻结地图快照。
            Start = startPose, // 来源：步骤 1 的起点；位置 m、航向 rad。
            Goal = goalPose, // 来源：步骤 1 的终点；位置 m、航向 rad。
            Vehicle = vehicle, // 来源：步骤 2 的车辆；尺寸 m、曲率 1/m。
            Configuration = coarseConfiguration, // 来源：步骤 2 的固定搜索配置。
            StartVehicleCurvature = 0d, // 来源：固定起步直行状态；单位 1/m。
            StartDirection = TravelDirection.Forward, // 来源：固定场景仅前进；无单位。
            GoalDirection = GoalDirectionConstraint.Forward, // 来源：固定终点以前进方向到达；无单位。
        };

        // 步骤 5：搜索粗路径
        PlanningResult coarseResult = new HybridAStarPlanner().Plan(
            coarseRequest,
            cancellationToken);
        if (coarseResult.Status != PlanningStatus.Success)
        {
            throw new InvalidOperationException(
                "Hybrid A* 粗路径失败：" +
                coarseResult.Diagnostics.TerminationReason);
        }

        // 步骤 6：请求并执行 Local G2 平滑
        var smoothingConfiguration = new PathSmoothingConfiguration
        {
            OutputSpacingMeters = 0.025d, // 来源：固定平滑输出间距；单位 m。
            MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定扫掠复核步长；单位 m。
            MinimumClearanceReserveMeters = 0.02d, // 来源：固定额外净空；单位 m。
            CurvatureLimitRadiusToleranceMeters = 0.002d, // 来源：固定曲率半径容差；单位 m。
        };
        smoothingConfiguration.LocalG2Quintic.MinimumWindowLengthMeters = 0.20d; // 最短窗口避免过短过渡放大曲率变化；单位 m。
        smoothingConfiguration.LocalG2Quintic.PreferredWindowLengthMeters = 0.50d; // 优先用半米窗口平衡平滑范围与局部性；单位 m。
        smoothingConfiguration.LocalG2Quintic.MaximumWindowLengthMeters = 0.80d; // 限制窗口不侵入过远路径几何；单位 m。
        smoothingConfiguration.LocalG2Quintic.MaximumDeviationMeters = 0.10d; // 候选最多偏离粗路径十厘米；单位 m。
        smoothingConfiguration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = 0.001d; // 忽略低于数值噪声量级的曲率跳变；单位 1/m。
        smoothingConfiguration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = 0.05d; // 以车辆最大曲率的 5% 识别相对跳变；无单位。
        smoothingConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d; // 候选至少改善 20% 峰值曲率梯度；无单位。
        smoothingConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d; // 最多容许 2% 曲率变化代价退化；无单位。
        smoothingConfiguration.LocalG2Quintic.MaximumCandidatesPerRegion = 12; // 限制每个区域的搜索工作量；单位为候选数。
        var smoothingRequest = new PathSmoothingRequest(
            coarseResult.Path, // 来源：成功粗路径；位置与弧长单位 m、航向 rad。
            coarseResult.Segments, // 来源：成功粗路径的方向分段；无单位。
            map, // 来源：步骤 3 的同一冻结地图快照。
            vehicle, // 来源：步骤 2 的同一车辆约束；尺寸 m、曲率 1/m。
            smoothingConfiguration); // 来源：本步骤显式固定的 Local G2 配置。
        PathSmoothingResult smoothingResult = new PathSmoothingService().Smooth(
            smoothingRequest,
            cancellationToken);
        bool smoothingSucceeded =
            smoothingResult.Status == PathSmoothingStatus.Complete ||
            smoothingResult.Status == PathSmoothingStatus.PartialImprovement ||
            smoothingResult.Status == PathSmoothingStatus.NotNeeded ||
            smoothingResult.Status == PathSmoothingStatus.Unchanged;
        if (!smoothingSucceeded ||
            smoothingResult.Path == null || smoothingResult.Path.Count == 0 ||
            smoothingResult.Segments == null || smoothingResult.Segments.Count == 0)
        {
            throw new InvalidOperationException(
                "Local G2 平滑没有发布完整路径：" +
                smoothingResult.Diagnostics.TerminationReason);
        }

        // 步骤 7：选择一个方向段
        IReadOnlyList<DirectionSegmentView> directionSegments =
            ReferencePathSegmenter.Create(smoothingResult);
        if (directionSegments.Count != 1)
        {
            throw new InvalidOperationException(
                "固定控制器演示不得跨越换挡边界，平滑路径必须恰好包含一个方向段。");
        }
        DirectionSegmentView activeSegment = directionSegments[0];
        if (activeSegment.Direction != TravelDirection.Forward)
        {
            throw new InvalidOperationException("固定离线演示只接受前进方向段。");
        }

        // 步骤 8：建立初始运动状态
        DateTimeOffset planningTimeUtc = DateTimeOffset.UtcNow;
        var vehicleState = new VehicleMotionState(
            startPose, // 来源：步骤 1 的固定起点；位置 m、航向 rad。
            0d, // 来源：固定静止起步状态；有符号纵向速度单位 m/s。
            0d, // 来源：固定静止起步状态；纵向加速度单位 m/s²。
            planningTimeUtc, // 来源：本次离线调用的 UTC 快照时刻。
            0L); // 来源：固定离线状态版本；非负序列号。

        // 步骤 9：冻结 EM 配置
        EmPlannerConfiguration emConfiguration = EmPlannerConfiguration.CreateDefault();
        emConfiguration.Scheduling.TimeHorizonSeconds = 20d; // 仅供 RollingHorizon 裁剪预测时间；单位 s；FullDirectionSegment 使用完整方向段与自适应时间表，不以此截断。
        emConfiguration.Scheduling.DistanceHorizonMeters = 10d; // 仅供 RollingHorizon 裁剪参考距离；单位 m；完整方向段范围不受此值截断。
        emConfiguration.Scheduling.OutputTimeStepSeconds = 0.10d; // 固定发布间隔；单位 s。
        emConfiguration.Scheduling.SolverTimeoutSeconds = 5d; // 给离线 OSQP 足够的求解预算上限；单位 s。
        emConfiguration.Scheduling.MaximumVehicleStateAgeSeconds = 1d; // 固定状态时效；单位 s。
        emConfiguration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d; // 自适应全段时间结点的最大间隔；单位 s。
        emConfiguration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d; // 自适应全段空间结点的最大间隔；单位 m。
        emConfiguration.Scheduling.MaximumOptimizationKnotCount = 401; // 限制完整方向段自适应离散的资源上限；单位为结点数。
        emConfiguration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.60d; // 限制固定停车场景的前进峰值速度；单位 m/s。
        emConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.40d; // 采用低于上限的低速巡航目标；单位 m/s。
        emConfiguration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.40d; // 限制起步加速以保持低速平顺；单位 m/s²。
        emConfiguration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.40d; // 为终点精确停车限制减速度幅值；单位 m/s²。
        emConfiguration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1.00d; // 限制加速度变化以避免突变；单位 m/s³。
        emConfiguration.Solver.MaximumOuterIterations = 5; // 限制 LS/ST 顺序凸化的外层资源；单位为迭代次数。
        emConfiguration.Solver.MaximumOsqpIterations = 10000; // 为每次 QP 收敛提供明确上限；单位为 OSQP 迭代次数。

        // 步骤 10：建立 EM 请求
        var emRequest = new EmPlanningRequest(
            smoothingResult, // 来源：步骤 6 的完整平滑路径；位置与弧长单位 m。
            map, // 来源：步骤 3 的冻结地图快照。
            vehicle, // 来源：步骤 2 的车辆约束；尺寸 m、曲率 1/m。
            vehicleState, // 来源：步骤 8 的固定运动状态；SI 单位、UTC。
            emConfiguration, // 来源：步骤 9 的显式低速泊车配置。
            0, // 来源：步骤 7 已验证的唯一方向段；零基索引。
            null, // 来源：首次固定离线规划没有上一条轨迹。
            planningTimeUtc, // 来源：本次请求发起 UTC 时刻。
            planningTimeUtc, // 来源：本次轨迹生效 UTC 时刻。
            "fixed-offline-em-trajectory", // 来源：固定非空输出轨迹身份。
            "fixed-offline-reference-path", // 来源：固定非空参考路径身份。
            string.Empty, // 来源：没有上一条轨迹，因此没有前序身份。
            EmMotionModel.NonholonomicForwardReverse, // 来源：当前 EM 支持的非完整运动模型。
            EmPlanningScope.FullDirectionSegment, // 来源：固定示例规划完整单方向段。
            null, // 来源：离线示例不另设循环截止剩余时间；单位 s。
            cancellationToken, // 来源：方法调用方的取消令牌。
            CancellationToken.None); // 来源：离线示例没有独立循环截止令牌。

        // 步骤 11：调用 EM 服务
        var emPlanningService = new EmPlanningService(
            new OsqpNativeSolver()); // 来源：仓库现有原生 OSQP 求解器；无单位。
        EmPlanningResult emResult = emPlanningService.Plan(
            emRequest,
            cancellationToken);

        // 步骤 12：判断是否可发布
        bool emSucceeded = emResult.Status == EmPlanningStatus.Success ||
            emResult.Status == EmPlanningStatus.SuccessWithFallback;
        if (!emSucceeded ||
            emResult.Trajectory == null ||
            emResult.Trajectory.Points.Count < 2)
        {
            throw new InvalidOperationException(
                "EM 规划没有返回完整可控轨迹：" + emResult.FailureReason);
        }
        EmTrajectory emTrajectory = emResult.Trajectory;

        // 步骤 13：读取输出契约
        // 以下局部变量只用于逐字段检查与诊断示范，不是车辆命令，也绝不能直接下发到底盘。
        string trajectoryId = emTrajectory.Metadata.TrajectoryId;
        string referencePathId = emTrajectory.Metadata.ReferencePathId;
        TravelDirection publishedDirection = emTrajectory.Metadata.Direction;
        EmTerminalType terminalType = emTrajectory.Metadata.TerminalType;
        EmPlanningScope publishedScope = emTrajectory.Metadata.PlanningScope;
        _ = trajectoryId;
        _ = referencePathId;
        _ = publishedDirection;
        _ = terminalType;
        _ = publishedScope;
        foreach (EmTrajectoryPoint point in emTrajectory.Points)
        {
            double timeFromStartSeconds = point.TimeFromStart;
            double worldXMeters = point.X;
            double worldYMeters = point.Y;
            double yawRadians = point.Yaw;
            double vehicleCurvaturePerMeter = point.VehicleCurvature;
            double pathSMeters = point.PathS;
            double signedSpeedMetersPerSecond = point.SignedLongitudinalVelocity;
            double velocityXMetersPerSecond = point.VelocityX;
            double velocityYMetersPerSecond = point.VelocityY;
            double longitudinalAccelerationMetersPerSecondSquared = point.LongitudinalAcceleration;
            double longitudinalJerkMetersPerSecondCubed = point.LongitudinalJerk;
            double yawRateRadiansPerSecond = point.YawRate;
            _ = timeFromStartSeconds;
            _ = worldXMeters;
            _ = worldYMeters;
            _ = yawRadians;
            _ = vehicleCurvaturePerMeter;
            _ = pathSMeters;
            _ = signedSpeedMetersPerSecond;
            _ = velocityXMetersPerSecond;
            _ = velocityYMetersPerSecond;
            _ = longitudinalAccelerationMetersPerSecondSquared;
            _ = longitudinalJerkMetersPerSecondCubed;
            _ = yawRateRadiansPerSecond;
        }

        // 步骤 14：适配为控制轨迹
        Trajectory2D controllerTrajectory =
            new EmControlTrajectoryAdapter().Create(emTrajectory);

        // 步骤 15：交给闭环动作
        // 本示例只返回轨迹输入；StateProvider、控制器参数、执行保护和停车策略仍由宿主拥有。
        return new FixedOfflinePlanningOutput(
            emTrajectory, // 来源：步骤 12 的完整 EM 时间轨迹。
            controllerTrajectory); // 来源：步骤 14 的控制器 SI 轨迹。
    }
}