using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Globalization;
using System.Threading;
using System.Threading.Tasks;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

/// <summary>一次观察会话启动的不可变结果，成功时同时携带冻结的粗路径、平滑路径、地图和方向段。</summary>
public sealed class TrajectoryObservationBootstrapResult
{
    private readonly VehicleParameters vehicle;

    private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
        PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
    {
        Job = job ?? throw new ArgumentNullException(nameof(job));
        vehicle = CopyVehicle(job.Vehicle);
        CoarseResult = coarse;
        SmoothedPath = smoothedPath;
        Segments = CopySegments(segments);
        FailureReason = failureReason ?? string.Empty;
        Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
    }

    public bool Succeeded { get; }

    public CoarsePathPlanningJob Job { get; }

    public VehicleParameters Vehicle => CopyVehicle(vehicle);

    public CoarsePathPlanningJobResult CoarseResult { get; }

    public PathSmoothingResult SmoothedPath { get; }

    public IReadOnlyList<DirectionSegmentView> Segments { get; }

    public string FailureReason { get; }

    public PlanningGridMap Map => CoarseResult?.MapResult?.Map;

    public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
    {
        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
            Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
    }

    public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
        IReadOnlyList<DirectionSegmentView> segments)
    {
        if (coarse == null) throw new ArgumentNullException(nameof(coarse));
        if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
        if (segments == null || segments.Count == 0)
            throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
    }

    private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
    {
        var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
        if (source != null)
        {
            for (int index = 0; index < source.Count; index++)
                copy.Add(source[index]);
        }
        return new ReadOnlyCollection<DirectionSegmentView>(copy);
    }

    private static VehicleParameters CopyVehicle(VehicleParameters source)
    {
        if (source == null) return null;
        return new VehicleParameters
        {
            LengthMeters = source.LengthMeters,
            WidthMeters = source.WidthMeters,
            SafetyMarginMeters = source.SafetyMarginMeters,
            MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
            MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
        };
    }
}

/// <summary>只在会话开始阶段构造并冻结规划输入；后续观察 tick 不会重新读取或改写该快照。</summary>
public sealed class TrajectoryObservationBootstrapper
{
    private readonly CoarsePathPlanningService coarseService;
    private readonly PathSmoothingService smoothingService;

    public TrajectoryObservationBootstrapper()
        : this(new CoarsePathPlanningService(), new PathSmoothingService())
    {
    }

    public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
        PathSmoothingService smoothingService)
    {
        this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
        this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
    }

    public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
        CancellationToken cancellationToken = default)
    {
        if (job == null) throw new ArgumentNullException(nameof(job));

        TimeSpan configuredLimit = job.Configuration == null
            ? TimeSpan.FromSeconds(5d)
            : job.Configuration.SearchTimeout;
        using var deadline = new TrajectoryObservationPlanningDeadline(configuredLimit, cancellationToken);
        return Bootstrap(job, deadline, cancellationToken);
    }

    public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
        TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken = default)
    {
        if (job == null) throw new ArgumentNullException(nameof(job));
        if (deadline == null) throw new ArgumentNullException(nameof(deadline));
        if (DeadlineExpired(deadline))
            return DeadlineFailure(job, null, null, deadline, "coarse");

        CoarsePathPlanningJob effectiveJob = CopyJobWithDeadline(job, deadline);
        bool coarseUsesCycleRemainder = job.Configuration != null &&
            effectiveJob.Configuration.SearchTimeout < job.Configuration.SearchTimeout;
        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
            deadline.Token, cancellationToken);

        CoarsePathPlanningJobResult coarse = coarseService.Plan(effectiveJob, linkedCancellation.Token);
        if (DeadlineExpired(deadline) ||
            (coarseUsesCycleRemainder && coarse.PlanningResult.Status == PlanningStatus.SearchTimeout))
        {
            return DeadlineFailure(job, coarse, null, deadline, "coarse");
        }
        if (coarse.PlanningResult.Status != PlanningStatus.Success)
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);

        if (DeadlineExpired(deadline))
            return DeadlineFailure(job, coarse, null, deadline, "smoothing");

        var smoothingRequest = new PathSmoothingRequest(
            CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
            coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
            new PathSmoothingConfiguration());
        PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, linkedCancellation.Token);
        if (DeadlineExpired(deadline))
            return DeadlineFailure(job, coarse, smooth, deadline, "smoothing");
        if (!IsPublishedSmoothingStatus(smooth.Status))
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, smooth, smooth.Diagnostics.TerminationReason);
        return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
    }

    private static bool DeadlineExpired(TrajectoryObservationPlanningDeadline deadline)
    {
        return deadline.IsExpired && !deadline.CallerCancellationRequested;
    }

    private static TrajectoryObservationBootstrapResult DeadlineFailure(CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult coarse, PathSmoothingResult smooth,
        TrajectoryObservationPlanningDeadline deadline, string phase)
    {
        string diagnostic = "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
            deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture);
        return TrajectoryObservationBootstrapResult.FromFailure(job, coarse, smooth, diagnostic);
    }

    private static CoarsePathPlanningJob CopyJobWithDeadline(CoarsePathPlanningJob source,
        TrajectoryObservationPlanningDeadline deadline)
    {
        return new CoarsePathPlanningJob
        {
            MapRequest = source.MapRequest,
            Start = source.Start,
            Goal = source.Goal,
            Vehicle = source.Vehicle,
            Configuration = CopyConfigurationWithDeadline(source.Configuration, deadline),
            StartVehicleCurvature = source.StartVehicleCurvature,
            StartDirection = source.StartDirection,
            GoalDirection = source.GoalDirection,
            DebugOptions = source.DebugOptions,
        };
    }

    private static HybridAStarConfiguration CopyConfigurationWithDeadline(HybridAStarConfiguration source,
        TrajectoryObservationPlanningDeadline deadline)
    {
        if (source == null) return null;
        return new HybridAStarConfiguration
        {
            PrimitiveLengthMeters = source.PrimitiveLengthMeters,
            IntegrationStepMeters = source.IntegrationStepMeters,
            MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
            HeadingResolutionRadians = source.HeadingResolutionRadians,
            CurvatureLevelCount = source.CurvatureLevelCount,
            GoalPositionToleranceMeters = source.GoalPositionToleranceMeters,
            GoalHeadingToleranceRadians = source.GoalHeadingToleranceRadians,
            MaximumExpandedNodes = source.MaximumExpandedNodes,
            SearchTimeout = deadline.Clamp(source.SearchTimeout),
            HeuristicWeight = source.HeuristicWeight,
            ReverseCostMultiplier = source.ReverseCostMultiplier,
            GearSwitchPenaltyMeters = source.GearSwitchPenaltyMeters,
            CurvatureMagnitudeWeight = source.CurvatureMagnitudeWeight,
            CurvatureChangePenaltyMetersPerLevel = source.CurvatureChangePenaltyMetersPerLevel,
            ClearanceCostWeight = source.ClearanceCostWeight,
            ClearanceCostDistanceMeters = source.ClearanceCostDistanceMeters,
            AllowReverse = source.AllowReverse,
        };
    }

    private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
        PlanningGridMap map)
    {
        if (path == null) throw new ArgumentNullException(nameof(path));
        if (map == null) throw new ArgumentNullException(nameof(map));

        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
        double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
        var copy = new List<CoarsePathPoint>(path.Count);
        for (int index = 0; index < path.Count; index++)
        {
            CoarsePathPoint point = path[index] ?? throw new ArgumentException(
                "The coarse path cannot contain null points.", nameof(path));
            double clearance = double.IsPositiveInfinity(point.BodyClearance)
                ? mapDiagonalMeters
                : point.BodyClearance;
            copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
                point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
                point.IsGearSwitchPoint, point.Source));
        }
        return new ReadOnlyCollection<CoarsePathPoint>(copy);
    }

    private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
    {
        return status == PathSmoothingStatus.Complete ||
            status == PathSmoothingStatus.PartialImprovement ||
            status == PathSmoothingStatus.NotNeeded ||
            status == PathSmoothingStatus.Unchanged;
    }
}

/// <summary>单个观察 tick 的只读结果，供状态文本、报告和可视化共同消费。</summary>
public sealed class TrajectoryObservationObservation
{
    internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
        EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
        TrajectoryExecutionState executorState)
    {
        ObservedAtUtc = observedAtUtc;
        VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
        PublishedTrajectory = publishedTrajectory;
        SelectedPoint = selectedPoint;
        Command = command;
        ExecutorState = executorState;
    }

    public DateTimeOffset ObservedAtUtc { get; }

    public VehicleMotionState VehicleState { get; }

    public EmTrajectory PublishedTrajectory { get; }

    public EmTrajectoryPoint SelectedPoint { get; }

    public TrajectoryControlCommand Command { get; }

    public TrajectoryExecutionState ExecutorState { get; }
}

/// <summary>协调一次冻结会话中的 EM 规划、轨迹采样和诊断；从不将预测命令发送给车辆。</summary>
public sealed class TrajectoryObservationController
{
    private readonly TrajectoryObservationBootstrapResult bootstrap;
    private readonly TrajectoryObservationSettings settings;
    private readonly EmPlannerConfiguration configuration;
    private readonly IEmPlanningService planningService;
    private readonly TrajectoryObservationSegmentTracker segmentTracker;
    private EmPlanningCoordinator coordinator;
    private TrajectoryExecutor executor;
    private EmTrajectory previousTrajectoryForVisualization;
    private EmTrajectory activeFullDirectionTrajectory;
    private EmPlanningCoordinator pendingFullDirectionCoordinator;
    private readonly string sessionId;
    private long cycleId;
    private int plannedSegmentIndex;
    private int pendingFullDirectionSegmentIndex = -1;
    private bool planAttemptedForActiveSegment;
    private bool pendingFullDirectionPlanAttempted;

    public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
    {
        this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
        if (!bootstrap.Succeeded)
            throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (planningService == null) throw new ArgumentNullException(nameof(planningService));
        if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));

        TrajectoryObservationSettings snapshot = settings.CreateValidatedSnapshot();
        this.settings = snapshot;
        configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.ReplanPeriodSeconds = snapshot.ReplanPeriodSeconds;
        configuration.Scheduling.SolverTimeoutSeconds = snapshot.SolverTimeoutSeconds;
        configuration.Solver.MaximumOsqpIterations = snapshot.MaximumOsqpIterations;
        configuration.Scheduling.TimeHorizonSeconds = snapshot.TimeHorizonSeconds;
        configuration.Scheduling.OutputTimeStepSeconds = snapshot.OutputTimeStepSeconds;
        this.planningService = planningService;
        coordinator = new EmPlanningCoordinator(planningService);
        executor = new TrajectoryExecutor(configuration);
        segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, snapshot,
            configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
        this.sessionId = sessionId;
    }

    public EmTrajectory PublishedTrajectory => settings.PlanningScope == EmPlanningScope.FullDirectionSegment
        ? activeFullDirectionTrajectory
        : coordinator.PublishedTrajectory;

    public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex];

    public TrajectoryObservationSegmentState SegmentState => segmentTracker.State;

    public EmTrajectory PreviousTrajectoryForVisualization => previousTrajectoryForVisualization;

    internal EmPlannerConfiguration CreateEffectiveConfigurationSnapshot()
    {
        return configuration.Copy();
    }

    public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
    {
        return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
    }

    public bool ShouldStartCycle(DateTimeOffset now)
    {
        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
        {
            if (plannedSegmentIndex != ActiveSegment.SegmentIndex)
            {
                plannedSegmentIndex = ActiveSegment.SegmentIndex;
                planAttemptedForActiveSegment = false;
            }
            if (TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex))
            {
                if (pendingFullDirectionSegmentIndex != pendingSegmentIndex)
                    return true;
                return !pendingFullDirectionPlanAttempted;
            }
            return !planAttemptedForActiveSegment;
        }
        return coordinator.ShouldStartCycle(now);
    }

    public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
    {
        using var deadline = new TrajectoryObservationPlanningDeadline(
            TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), cancellationToken);
        return await StartCycle(now, state, deadline, cancellationToken).ConfigureAwait(false);
    }

    public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
        TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));
        if (deadline == null) throw new ArgumentNullException(nameof(deadline));
        using var callerCancellation = CancellationTokenSource.CreateLinkedTokenSource(
            deadline.CallerToken, cancellationToken);

        int targetSegmentIndex = ActiveSegment.SegmentIndex;
        int pendingSegmentIndex = -1;
        bool planningPendingFullDirection = settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
            TryGetPendingFullDirectionSegmentIndex(out pendingSegmentIndex);
        EmPlanningCoordinator targetCoordinator;
        EmTrajectory previousTrajectory;
        if (planningPendingFullDirection)
        {
            targetSegmentIndex = pendingSegmentIndex;
            if (pendingFullDirectionSegmentIndex != targetSegmentIndex || pendingFullDirectionCoordinator == null)
            {
                pendingFullDirectionSegmentIndex = targetSegmentIndex;
                pendingFullDirectionPlanAttempted = false;
                pendingFullDirectionCoordinator = new EmPlanningCoordinator(planningService);
            }
            pendingFullDirectionPlanAttempted = true;
            targetCoordinator = pendingFullDirectionCoordinator;
            previousTrajectory = null;
        }
        else
        {
            planAttemptedForActiveSegment = true;
            plannedSegmentIndex = ActiveSegment.SegmentIndex;
            targetCoordinator = coordinator;
            previousTrajectory = coordinator.PublishedTrajectory;
        }

        long currentCycleId = Interlocked.Increment(ref cycleId);
        EmPlannerConfiguration effectiveConfiguration = configuration.Copy();
        TimeSpan cycleRemaining = deadline.Remaining;
        TimeSpan configuredSolverLimit = TimeSpan.FromSeconds(
            configuration.Scheduling.SolverTimeoutSeconds);
        if (cycleRemaining > TimeSpan.Zero &&
            cycleRemaining < configuredSolverLimit - TimeSpan.FromMilliseconds(1d))
        {
            effectiveConfiguration.Scheduling.SolverTimeoutSeconds = cycleRemaining.TotalSeconds;
        }
        var request = new EmPlanningRequest(
            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, effectiveConfiguration,
            targetSegmentIndex, previousTrajectory, now, now,
            sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
            previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
            EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope, cycleRemaining,
            callerCancellation.Token, deadline.DeadlineToken,
            deadline.PublicationAuthorization);
        if (callerCancellation.IsCancellationRequested)
        {
            var cancelledResult = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
                "callerCancellation=true;phase=request");
            return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
                cancelledResult, false, cancelledResult.FailureReason);
        }
        if (deadline.DeadlineToken.IsCancellationRequested)
        {
            var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
                "cycleDeadlineExpired=true;phase=request;remainingMs=" +
                cycleRemaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
            return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
                expiredResult, false, expiredResult.FailureReason);
        }
        Action<EmTrajectory> fullDirectionCommit =
            settings.PlanningScope == EmPlanningScope.FullDirectionSegment && !planningPendingFullDirection
                ? trajectory => activeFullDirectionTrajectory = trajectory
                : null;
        PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(
            new PlanningCycleInput(request, now), callerCancellation.Token,
            fullDirectionCommit).ConfigureAwait(false);
        if (!result.Published && deadline.IsExpired && !callerCancellation.IsCancellationRequested &&
            result.Result.Status == EmPlanningStatus.Cancelled)
        {
            var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
                "cycleDeadlineExpired=true;phase=planning;remainingMs=" +
                deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
            result = new PlanningCycleResult(result.Version, result.Identity, expiredResult, false,
                expiredResult.FailureReason);
        }
        return result;
    }

    public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
            segmentTracker.State.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection &&
            !HasPendingFullDirectionTrajectory())
        {
            return false;
        }

        EmTrajectory activeTrajectory = PublishedTrajectory;
        TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, activeTrajectory);
        if (!update.Advanced)
            return false;

        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
        {
            EmTrajectory pendingTrajectory = pendingFullDirectionCoordinator.PublishedTrajectory;
            previousTrajectoryForVisualization = activeTrajectory;
            activeFullDirectionTrajectory = RebaseEffectiveAt(pendingTrajectory, now);
            coordinator = pendingFullDirectionCoordinator;
            pendingFullDirectionCoordinator = null;
            pendingFullDirectionSegmentIndex = -1;
            pendingFullDirectionPlanAttempted = false;
            executor = new TrajectoryExecutor(configuration);
            plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
            planAttemptedForActiveSegment = true;
            return true;
        }

        previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
        coordinator = new EmPlanningCoordinator(planningService);
        executor = new TrajectoryExecutor(configuration);
        plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
        planAttemptedForActiveSegment = false;
        return true;
    }

    public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

        EmTrajectory trajectory = PublishedTrajectory;
        if (trajectory == null)
            return new TrajectoryObservationObservation(now, state, null, null, null, null);

        TrajectoryObservationSegmentState segmentState = segmentTracker.State;
        TravelDirection currentDirection = ActiveSegment.Direction;
        bool waitingForDirection = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
            segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
        TravelDirection desiredDirection = waitingForDirection && segmentState.ExpectedDirection.HasValue
            ? segmentState.ExpectedDirection.Value
            : currentDirection;
        TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
            desiredDirection, currentDirection, !waitingForDirection);
        TrajectoryExecutionState executorState = executor.State;
        return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
            command, executorState);
    }

    private bool TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex)
    {
        pendingSegmentIndex = -1;
        if (settings.PlanningScope != EmPlanningScope.FullDirectionSegment ||
            segmentTracker.State.Phase != TrajectoryObservationSegmentPhase.WaitingForDirection ||
            segmentTracker.State.ActiveSegmentIndex + 1 >= bootstrap.Segments.Count)
        {
            return false;
        }
        pendingSegmentIndex = segmentTracker.State.ActiveSegmentIndex + 1;
        return true;
    }

    private bool HasPendingFullDirectionTrajectory()
    {
        if (!TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex) ||
            pendingFullDirectionSegmentIndex != pendingSegmentIndex || pendingFullDirectionCoordinator == null)
        {
            return false;
        }

        EmTrajectory pending = pendingFullDirectionCoordinator.PublishedTrajectory;
        DirectionSegmentView segment = bootstrap.Segments[pendingSegmentIndex];
        return pending != null && pending.Metadata.SegmentIndex == segment.SegmentIndex &&
            pending.Metadata.Direction == segment.Direction;
    }

    private static EmTrajectory RebaseEffectiveAt(EmTrajectory source, DateTimeOffset effectiveAtUtc)
    {
        if (source == null) throw new ArgumentNullException(nameof(source));
        EmTrajectoryMetadata metadata = source.Metadata;
        return new EmTrajectory(new EmTrajectoryMetadata(metadata.TrajectoryId, metadata.GeneratedAtUtc, effectiveAtUtc,
            metadata.MapSnapshotId, metadata.ReferencePathId, metadata.VehicleStateSequenceId,
            metadata.PreviousTrajectoryId, metadata.SegmentIndex, metadata.Direction, metadata.TerminalType,
            metadata.LongitudinalMode, metadata.PlanningScope), source.Points);
    }
}

public sealed class TrajectoryObservationLoopTick
{
    internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
        PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight,
        bool planningStarted, bool planningCompleted, bool segmentAdvanced,
        TrajectoryObservationSegmentState segmentState)
    {
        Observation = observation ?? throw new ArgumentNullException(nameof(observation));
        LatestCycle = latestCycle;
        LatestPlanningElapsed = latestPlanningElapsed;
        PlanningInFlight = planningInFlight;
        PlanningStarted = planningStarted;
        PlanningCompleted = planningCompleted;
        SegmentAdvanced = segmentAdvanced;
        SegmentState = segmentState ?? throw new ArgumentNullException(nameof(segmentState));
    }

    public TrajectoryObservationObservation Observation { get; }

    public PlanningCycleResult LatestCycle { get; }

    public TimeSpan LatestPlanningElapsed { get; }

    public bool PlanningInFlight { get; }

    public bool PlanningStarted { get; }

    public bool PlanningCompleted { get; }

    public bool SegmentAdvanced { get; }

    public TrajectoryObservationSegmentState SegmentState { get; }

    public bool ShouldLog => true;
}

/// <summary>按观察周期捕获新鲜只读状态并发布快照的循环；规划耗时不能改变观察安全边界。</summary>
public sealed class TrajectoryObservationLoop
{
    private readonly TrajectoryObservationController controller;
    private Task<PlanningCycleResult> planningTask;
    private DateTimeOffset planningStartedAtUtc;
    private PlanningCycleResult latestCycle;
    private TimeSpan latestPlanningElapsed;

    public TrajectoryObservationLoop(TrajectoryObservationController controller)
    {
        this.controller = controller ?? throw new ArgumentNullException(nameof(controller));
    }

    public TrajectoryObservationLoopTick Tick(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));
        cancellationToken.ThrowIfCancellationRequested();

        bool planningCompleted = ConsumeCompletedPlanning(now);
        bool segmentAdvanced = planningTask == null && controller.TryAdvanceSegment(now, state);
        if (segmentAdvanced)
            latestCycle = null;
        bool planningStarted = false;
        if (planningTask == null && controller.ShouldStartCycle(now))
        {
            planningStarted = true;
            planningStartedAtUtc = now;
            planningTask = controller.StartCycle(now, state, cancellationToken);
            planningCompleted |= ConsumeCompletedPlanning(now);
        }

        TrajectoryObservationObservation observation = controller.Observe(now, state);
        return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
            planningTask != null, planningStarted, planningCompleted, segmentAdvanced, controller.SegmentState);
    }

    private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)
    {
        Task<PlanningCycleResult> completed = planningTask;
        if (completed == null || !completed.IsCompleted) return false;

        latestCycle = completed.GetAwaiter().GetResult();
        TimeSpan elapsed = observedAtUtc - planningStartedAtUtc;
        latestPlanningElapsed = elapsed < TimeSpan.Zero ? TimeSpan.Zero : elapsed;
        planningTask = null;
        return true;
    }
}

public enum TrajectoryObservationSessionEndReason
{
    BootstrapFailure,
    RuntimeFault,
    Cancellation,
}

/// <summary>集中定义观察会话的开始、停止和故障清理语义，避免留下旧会话可视化。</summary>
public static class TrajectoryObservationSessionLifecycle
{
    public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
    {
        switch (reason)
        {
            case TrajectoryObservationSessionEndReason.BootstrapFailure:
                return false;
            case TrajectoryObservationSessionEndReason.RuntimeFault:
            case TrajectoryObservationSessionEndReason.Cancellation:
                return true;
            default:
                throw new ArgumentOutOfRangeException(nameof(reason));
        }
    }
}

public sealed class TrajectoryObservationRuntimeState
{
    public const string GearSwitchWaitingNotice =
        "等待真实档位/方向确认；观察模式不会激活下一方向段";

    private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
    {
        WaitingAtGearSwitch = waitingAtGearSwitch;
        WorldNotice = waitingAtGearSwitch ? GearSwitchWaitingNotice : string.Empty;
    }

    public bool WaitingAtGearSwitch { get; }

    public string WorldNotice { get; }

    public static TrajectoryObservationRuntimeState Create(TrajectoryObservationSegmentState segmentState)
    {
        if (segmentState == null) throw new ArgumentNullException(nameof(segmentState));
        bool waiting = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
            segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
        return new TrajectoryObservationRuntimeState(waiting);
    }

    public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
    {
        if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
            return new TrajectoryObservationRuntimeState(false);

        EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
        bool waiting = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
        return new TrajectoryObservationRuntimeState(waiting);
    }
}

public sealed class TrajectoryObservationLsSample
{
    public TrajectoryObservationLsSample(double pathS, double lateralOffset)
    {
        PathS = pathS;
        LateralOffset = lateralOffset;
    }

    public double PathS { get; }

    public double LateralOffset { get; }
}

public sealed class TrajectoryObservationStSample
{
    public TrajectoryObservationStSample(double timeFromStart, double pathS)
    {
        TimeFromStart = timeFromStart;
        PathS = pathS;
    }

    public double TimeFromStart { get; }

    public double PathS { get; }
}

public sealed class TrajectoryObservationSpeedSample
{
    public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
    {
        TimeFromStart = timeFromStart;
        SignedLongitudinalVelocity = signedLongitudinalVelocity;
    }

    public double TimeFromStart { get; }

    public double SignedLongitudinalVelocity { get; }
}

public sealed class TrajectoryObservationCharts
{
    private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
        IReadOnlyList<TrajectoryObservationStSample> stSamples,
        IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
    {
        LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
            new List<TrajectoryObservationLsSample>(lsSamples));
        StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
            new List<TrajectoryObservationStSample>(stSamples));
        SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
            new List<TrajectoryObservationSpeedSample>(speedSamples));
        FailedProjectionCount = failedProjectionCount;
    }

    public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }

    public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }

    public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }

    public int FailedProjectionCount { get; }

    public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
        double maximumProjectionDistanceMeters)
    {
        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
        if (segment == null) throw new ArgumentNullException(nameof(segment));
        if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
            maximumProjectionDistanceMeters < 0d)
            throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));

        var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
        var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
        var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
        var projector = new FrenetProjector();
        double seedReferenceS = 0d;
        int failedProjectionCount = 0;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            var pose = new Pose2D(point.X, point.Y, point.Yaw);
            if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
                maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
            {
                ls.Add(new TrajectoryObservationLsSample(
                    segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
                seedReferenceS = projection.ReferenceS;
            }
            else
            {
                failedProjectionCount++;
            }

            st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
            speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
        }

        return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
    }
}