ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationSegmentTracker.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

/// <summary>观察器对当前方向段和真实换向确认过程的只读阶段。</summary>
public enum TrajectoryObservationSegmentPhase
{
    Planning,
    WaitingForStop,
    WaitingForDirection,
    Completed,
}

public sealed class TrajectoryObservationSegmentState
{
    internal TrajectoryObservationSegmentState(int activeSegmentIndex, TrajectoryObservationSegmentPhase phase,
        TravelDirection? expectedDirection, int confirmedDirectionSamples)
    {
        ActiveSegmentIndex = activeSegmentIndex;
        Phase = phase;
        ExpectedDirection = expectedDirection;
        ConfirmedDirectionSamples = confirmedDirectionSamples;
    }

    public int ActiveSegmentIndex { get; }

    public TrajectoryObservationSegmentPhase Phase { get; }

    public TravelDirection? ExpectedDirection { get; }

    public int ConfirmedDirectionSamples { get; }
}

public sealed class TrajectoryObservationSegmentUpdate
{
    internal TrajectoryObservationSegmentUpdate(bool advanced, bool completed, string diagnostic,
        TrajectoryObservationSegmentState state)
    {
        Advanced = advanced;
        Completed = completed;
        Diagnostic = diagnostic ?? string.Empty;
        State = state ?? throw new ArgumentNullException(nameof(state));
    }

    public bool Advanced { get; }

    public bool Completed { get; }

    public string Diagnostic { get; }

    public TrajectoryObservationSegmentState State { get; }
}

/// <summary>根据真实状态和方向确认推进活动段；不会自行请求或执行车辆换向。</summary>
public sealed class TrajectoryObservationSegmentTracker
{
    private readonly IReadOnlyList<DirectionSegmentView> segments;
    private readonly TrajectoryObservationSettings settings;
    private readonly double stopSpeedTolerance;
    private readonly FrenetProjector projector = new FrenetProjector();
    private long? lastSequenceId;
    private DateTimeOffset? lastUpdateAtUtc;
    private DateTimeOffset? stopHoldStartedAtUtc;
    private int confirmedDirectionSamples;

    public TrajectoryObservationSegmentTracker(IReadOnlyList<DirectionSegmentView> segments,
        TrajectoryObservationSettings settings, double stopSpeedTolerance)
    {
        if (segments == null || segments.Count == 0)
            throw new ArgumentException("At least one direction segment is required.", nameof(segments));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (!IsFinite(stopSpeedTolerance) || stopSpeedTolerance <= 0d)
            throw new ArgumentOutOfRangeException(nameof(stopSpeedTolerance));

        var copy = new List<DirectionSegmentView>(segments.Count);
        for (int index = 0; index < segments.Count; index++)
        {
            DirectionSegmentView segment = segments[index] ?? throw new ArgumentException(
                "Direction segments cannot contain null values.", nameof(segments));
            if (segment.SegmentIndex != index)
                throw new ArgumentException("Direction segments must be ordered with consecutive indexes.", nameof(segments));
            copy.Add(segment);
        }

        this.segments = new ReadOnlyCollection<DirectionSegmentView>(copy);
        this.settings = settings.CreateValidatedSnapshot();
        this.stopSpeedTolerance = stopSpeedTolerance;
        State = CreateState(0, TrajectoryObservationSegmentPhase.Planning, 0);
    }

    public TrajectoryObservationSegmentState State { get; private set; }

    public TrajectoryObservationSegmentUpdate Update(DateTimeOffset now, VehicleMotionState measuredState,
        EmTrajectory trajectory)
    {
        if (measuredState == null) throw new ArgumentNullException(nameof(measuredState));

        if (!HasStrictlyNewEvidence(now, measuredState.SequenceId))
            return ResetConfirmation("换向确认已重置：状态序列或观察时间不连续。");

        lastSequenceId = measuredState.SequenceId;
        lastUpdateAtUtc = now;
        if (State.Phase == TrajectoryObservationSegmentPhase.Completed)
            return CreateUpdate(false, true, "方向段已完成，忽略后续观察样本。");

        DirectionSegmentView current = segments[State.ActiveSegmentIndex];
        if (!IsEligibleTerminal(now, measuredState, trajectory, current))
        {
            if (State.Phase == TrajectoryObservationSegmentPhase.Planning)
                return CreateUpdate(false, false, "等待当前方向段的有效换向终点轨迹。");
            return ResetConfirmation("换向确认已重置：轨迹、终点或连接点投影不再满足条件。");
        }

        if (State.ActiveSegmentIndex + 1 >= segments.Count)
        {
            State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.Completed, 0);
            return CreateUpdate(false, true, "方向段观察完成：已到达最后一个方向段末端。");
        }

        double speed = measuredState.SignedLongitudinalSpeedMetersPerSecond;
        switch (State.Phase)
        {
            case TrajectoryObservationSegmentPhase.Planning:
                if (Math.Abs(speed) > stopSpeedTolerance)
                    return CreateUpdate(false, false, "等待真实速度连续停车后确认换向。");
                stopHoldStartedAtUtc = now;
                confirmedDirectionSamples = 0;
                State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.WaitingForStop, 0);
                return CreateUpdate(false, false, "已检测到停车样本，开始换向停车保持。");

            case TrajectoryObservationSegmentPhase.WaitingForStop:
                if (Math.Abs(speed) > stopSpeedTolerance)
                    return ResetConfirmation("换向确认已重置：停车保持期间速度不为零。");
                if (now - stopHoldStartedAtUtc.Value < TimeSpan.FromSeconds(settings.GearSwitchStopHoldSeconds))
                    return CreateUpdate(false, false, "等待连续停车保持完成。");
                State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.WaitingForDirection, 0);
                return CreateUpdate(false, false, "停车保持完成，等待下一方向的连续速度样本。");

            case TrajectoryObservationSegmentPhase.WaitingForDirection:
                TravelDirection expectedDirection = segments[State.ActiveSegmentIndex + 1].Direction;
                if (!MatchesExpectedDirection(speed, expectedDirection))
                    return ResetConfirmation("换向确认已重置：下一方向速度样本符号或幅值无效。");
                confirmedDirectionSamples++;
                if (confirmedDirectionSamples < settings.DirectionConfirmationSamples)
                {
                    State = CreateState(State.ActiveSegmentIndex,
                        TrajectoryObservationSegmentPhase.WaitingForDirection, confirmedDirectionSamples);
                    return CreateUpdate(false, false, "已确认下一方向速度样本 " + confirmedDirectionSamples + "/" +
                        settings.DirectionConfirmationSamples + "。" );
                }
                int nextIndex = State.ActiveSegmentIndex + 1;
                State = CreateState(nextIndex, TrajectoryObservationSegmentPhase.Planning, 0);
                stopHoldStartedAtUtc = null;
                confirmedDirectionSamples = 0;
                return CreateUpdate(true, false, "换向确认完成：方向段 " + (nextIndex - 1) + " 已切换到 " + nextIndex + "。");

            default:
                throw new InvalidOperationException("Unexpected direction-segment phase.");
        }
    }

    private bool HasStrictlyNewEvidence(DateTimeOffset now, long sequenceId)
    {
        return (!lastSequenceId.HasValue || sequenceId > lastSequenceId.Value) &&
            (!lastUpdateAtUtc.HasValue || now >= lastUpdateAtUtc.Value);
    }

    private bool IsEligibleTerminal(DateTimeOffset now, VehicleMotionState measuredState,
        EmTrajectory trajectory, DirectionSegmentView current)
    {
        if (trajectory == null || trajectory.Metadata.SegmentIndex != current.SegmentIndex ||
            trajectory.Metadata.Direction != current.Direction ||
            (trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch &&
                trajectory.Metadata.TerminalType != EmTerminalType.Goal))
            return false;

        EmTrajectoryPoint terminalPoint = trajectory.Points[trajectory.Points.Count - 1];
        if (terminalPoint.SegmentIndex != current.SegmentIndex || terminalPoint.Direction != current.Direction ||
            now < trajectory.Metadata.EffectiveAtUtc + TimeSpan.FromSeconds(terminalPoint.TimeFromStart))
            return false;

        if (!projector.TryProject(measuredState.Pose, current, current.LengthMeters, current.LengthMeters,
                settings.GearSwitchProjectionToleranceMeters, current.LengthMeters, out _))
            return false;
        if (trajectory.Metadata.TerminalType == EmTerminalType.Goal)
            return State.ActiveSegmentIndex + 1 >= segments.Count;
        if (State.ActiveSegmentIndex + 1 >= segments.Count)
            return true;

        DirectionSegmentView next = segments[State.ActiveSegmentIndex + 1];
        return projector.TryProject(measuredState.Pose, next, 0d, 0d,
            settings.GearSwitchProjectionToleranceMeters, 0d, out _);
    }

    private bool MatchesExpectedDirection(double signedSpeed, TravelDirection expectedDirection)
    {
        if (!IsFinite(signedSpeed) || Math.Abs(signedSpeed) < settings.DirectionConfirmationSpeedMetersPerSecond)
            return false;
        return expectedDirection == TravelDirection.Forward ? signedSpeed > 0d : signedSpeed < 0d;
    }

    private TrajectoryObservationSegmentUpdate ResetConfirmation(string diagnostic)
    {
        stopHoldStartedAtUtc = null;
        confirmedDirectionSamples = 0;
        State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.Planning, 0);
        return CreateUpdate(false, false, diagnostic);
    }

    private TrajectoryObservationSegmentState CreateState(int activeSegmentIndex,
        TrajectoryObservationSegmentPhase phase, int samples)
    {
        TravelDirection? expectedDirection = activeSegmentIndex + 1 < segments.Count
            ? segments[activeSegmentIndex + 1].Direction
            : null;
        return new TrajectoryObservationSegmentState(activeSegmentIndex, phase, expectedDirection, samples);
    }

    private TrajectoryObservationSegmentUpdate CreateUpdate(bool advanced, bool completed, string diagnostic)
    {
        return new TrajectoryObservationSegmentUpdate(advanced, completed, diagnostic, State);
    }

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