using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>下一轮规划起点的来源；只允许安全未来样本或调用方测量状态二选一。</summary>
public enum TrajectoryHandoffSource
{
    MeasuredState,
    PreviousTrajectory,
}

/// <summary>拒绝使用已发布轨迹进行交接的稳定原因；拒绝时必须回退到测量状态。</summary>
public enum TrajectoryHandoffRejectionReason
{
    None,
    MissingTrajectory,
    InvalidConfiguration,
    TrajectoryNotYetEffective,
    TrajectoryTooOld,
    SegmentMismatch,
    DirectionMismatch,
    TrackingErrorExceeded,
    GearBoundary,
    TerminalBoundary,
    HandoffBeyondTrajectory,
    SampleUnavailable,
}

/// <summary>下一次单次规划的不可变交接决策；不会跨方向段、方向或终端边界复用旧轨迹。</summary>
public sealed class TrajectoryHandoffSelection
{
    internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState,
        EmTrajectory previousTrajectory, EmTrajectoryPoint sampledPoint, TrajectoryHandoffRejectionReason rejectionReason)
    {
        Source = source;
        StartState = startState ?? throw new ArgumentNullException(nameof(startState));
        PreviousTrajectory = previousTrajectory;
        SampledPoint = sampledPoint;
        RejectionReason = rejectionReason;
    }

    /// <summary>交接使用未来旧轨迹样本还是调用方测量状态。</summary>
    public TrajectoryHandoffSource Source { get; }

    public VehicleMotionState StartState { get; }

    public EmTrajectory PreviousTrajectory { get; }

    public EmTrajectoryPoint SampledPoint { get; }

    public TrajectoryHandoffRejectionReason RejectionReason { get; }
}

/// <summary>选择安全的同段同方向未来轨迹种子；任一检查失败时保留调用方测量状态。</summary>
public sealed class TrajectoryHandoffSelector
{
    private const double TimeEpsilonSeconds = 1e-9d;
    private readonly TrajectorySampler sampler;

    /// <summary>使用默认同质区间采样器创建交接选择器。</summary>
    public TrajectoryHandoffSelector()
        : this(new TrajectorySampler())
    {
    }

    internal TrajectoryHandoffSelector(TrajectorySampler sampler)
    {
        this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler));
    }

    /// <summary>从已发布轨迹选择下一轮规划的未来交接状态。</summary>
    /// <param name="previousTrajectory">最后一条完整已发布轨迹；为 <see langword="null"/> 时直接使用测量状态。</param>
    /// <param name="measuredState">调用方当前车辆状态快照；位置 m、航向 rad、带符号速度 m/s。</param>
    /// <param name="expectedSegmentIndex">下一轮请求期望的非负方向段索引。</param>
    /// <param name="expectedDirection">下一轮请求期望的实际行驶方向。</param>
    /// <param name="now">调用方当前时刻，用于检查旧轨迹年龄和 lookahead。</param>
    /// <param name="configuration">包含最大状态年龄、lookahead 和追踪容差的配置快照。</param>
    /// <returns>只有年龄、追踪、同段同方向、末点和边界检查均通过时返回未来轨迹样本；否则返回测量状态及拒绝原因。</returns>
    public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState,
        int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now,
        EmPlannerConfiguration configuration)
    {
        if (measuredState == null)
            throw new ArgumentNullException(nameof(measuredState));
        if (expectedSegmentIndex < 0 || !Enum.IsDefined(typeof(TravelDirection), expectedDirection))
            throw new ArgumentOutOfRangeException(nameof(expectedSegmentIndex));
        if (previousTrajectory == null)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.MissingTrajectory);
        if (!TryReadConfiguration(configuration, out double maximumAgeSeconds, out double lookaheadSeconds,
                out double spatialToleranceMeters, out double kinematicTolerance))
        {
            return Measured(measuredState, TrajectoryHandoffRejectionReason.InvalidConfiguration);
        }
        if (previousTrajectory.Metadata.SegmentIndex != expectedSegmentIndex)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SegmentMismatch);
        if (previousTrajectory.Metadata.Direction != expectedDirection)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.DirectionMismatch);

        double trajectoryAgeSeconds = (now - previousTrajectory.Metadata.EffectiveAtUtc).TotalSeconds;
        if (trajectoryAgeSeconds < -TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryNotYetEffective);
        if (trajectoryAgeSeconds > maximumAgeSeconds + TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryTooOld);
        if (!sampler.TrySample(previousTrajectory, trajectoryAgeSeconds, out EmTrajectoryPoint currentPoint))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
        if (!Tracks(currentPoint, measuredState, spatialToleranceMeters, kinematicTolerance))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrackingErrorExceeded);

        double handoffTime = trajectoryAgeSeconds + lookaheadSeconds;
        double terminalTime = previousTrajectory.Points[previousTrajectory.Points.Count - 1].TimeFromStart;
        if (handoffTime > terminalTime + TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory);
        TrajectoryHandoffRejectionReason boundaryReason = BoundaryInInterval(previousTrajectory, trajectoryAgeSeconds,
            handoffTime);
        if (boundaryReason != TrajectoryHandoffRejectionReason.None)
            return Measured(measuredState, boundaryReason);
        if (!sampler.TrySample(previousTrajectory, handoffTime, out EmTrajectoryPoint handoffPoint))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);

        var startState = new VehicleMotionState(new Pose2D(handoffPoint.X, handoffPoint.Y, handoffPoint.Yaw),
            handoffPoint.SignedLongitudinalVelocity, handoffPoint.LongitudinalAcceleration,
            previousTrajectory.Metadata.EffectiveAtUtc.AddSeconds(handoffTime), measuredState.SequenceId);
        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.PreviousTrajectory, startState,
            previousTrajectory, handoffPoint, TrajectoryHandoffRejectionReason.None);
    }

    private static TrajectoryHandoffSelection Measured(VehicleMotionState measuredState,
        TrajectoryHandoffRejectionReason rejectionReason)
    {
        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.MeasuredState, measuredState, null, null,
            rejectionReason);
    }

    private static bool TryReadConfiguration(EmPlannerConfiguration configuration, out double maximumAgeSeconds,
        out double lookaheadSeconds, out double spatialToleranceMeters, out double kinematicTolerance)
    {
        maximumAgeSeconds = configuration?.Scheduling?.MaximumVehicleStateAgeSeconds ?? double.NaN;
        lookaheadSeconds = configuration?.Scheduling?.HandoffLookaheadSeconds ?? double.NaN;
        spatialToleranceMeters = configuration?.Validation?.SpatialToleranceMeters ?? double.NaN;
        kinematicTolerance = configuration?.Validation?.KinematicTolerance ?? double.NaN;
        return IsNonNegativeFinite(maximumAgeSeconds) && IsNonNegativeFinite(lookaheadSeconds) &&
            IsNonNegativeFinite(spatialToleranceMeters) && IsNonNegativeFinite(kinematicTolerance);
    }

    private static bool Tracks(EmTrajectoryPoint trajectoryPoint, VehicleMotionState measuredState,
        double spatialToleranceMeters, double kinematicTolerance)
    {
        double dx = trajectoryPoint.X - measuredState.Pose.X;
        double dy = trajectoryPoint.Y - measuredState.Pose.Y;
        if (Math.Sqrt(dx * dx + dy * dy) > spatialToleranceMeters)
            return false;
        double yawError = Math.Atan2(Math.Sin(trajectoryPoint.Yaw - measuredState.Pose.Heading),
            Math.Cos(trajectoryPoint.Yaw - measuredState.Pose.Heading));
        return Math.Abs(yawError) <= kinematicTolerance &&
            Math.Abs(trajectoryPoint.SignedLongitudinalVelocity -
                measuredState.SignedLongitudinalSpeedMetersPerSecond) <= kinematicTolerance;
    }

    private static TrajectoryHandoffRejectionReason BoundaryInInterval(EmTrajectory trajectory, double startTime,
        double endTime)
    {
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point.TimeFromStart <= startTime + TimeEpsilonSeconds || point.TimeFromStart > endTime + TimeEpsilonSeconds)
                continue;
            if (point.BoundaryType == EmBoundaryType.GearSwitchApproach ||
                point.BoundaryType == EmBoundaryType.GearSwitchDeparture)
            {
                return TrajectoryHandoffRejectionReason.GearBoundary;
            }
            if (point.BoundaryType == EmBoundaryType.RollingSafetyStop || point.BoundaryType == EmBoundaryType.Goal)
                return TrajectoryHandoffRejectionReason.TerminalBoundary;
        }
        return TrajectoryHandoffRejectionReason.None;
    }

    private static bool IsNonNegativeFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
    }
}