using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>仅在同一方向段、同一方向和同一边界类型的轨迹区间内插值不可变轨迹。</summary>
public sealed class TrajectorySampler
{
    private const double TimeEpsilonSeconds = 1e-9d;

    /// <summary>按轨迹起点后的时间采样一个同质区间内的轨迹点。</summary>
    /// <param name="trajectory">完整不可变轨迹；其点序列必须按 <c>TimeFromStart</c> 严格递增。</param>
    /// <param name="timeFromStart">相对轨迹起点的采样时刻，单位 s。</param>
    /// <param name="sampledPoint">成功时为原始点或插值新点；失败时为 <see langword="null"/>。</param>
    /// <returns>时刻位于轨迹范围内且不需跨方向段/方向/边界插值时为 <see langword="true"/>；区间外或跨边界时为 <see langword="false"/>。</returns>
    public bool TrySample(EmTrajectory trajectory, double timeFromStart, out EmTrajectoryPoint sampledPoint)
    {
        sampledPoint = null;
        if (trajectory == null || double.IsNaN(timeFromStart) || double.IsInfinity(timeFromStart) ||
            trajectory.Points.Count == 0)
        {
            return false;
        }

        EmTrajectoryPoint first = trajectory.Points[0];
        EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1];
        if (timeFromStart < first.TimeFromStart - TimeEpsilonSeconds ||
            timeFromStart > last.TimeFromStart + TimeEpsilonSeconds)
        {
            return false;
        }

        int lower = 0;
        int upper = trajectory.Points.Count - 1;
        while (upper - lower > 1)
        {
            int middle = lower + (upper - lower) / 2;
            EmTrajectoryPoint point = trajectory.Points[middle];
            if (Math.Abs(point.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
            {
                sampledPoint = point;
                return true;
            }

            if (point.TimeFromStart < timeFromStart)
                lower = middle;
            else
                upper = middle;
        }

        EmTrajectoryPoint left = trajectory.Points[lower];
        EmTrajectoryPoint right = trajectory.Points[upper];
        if (Math.Abs(left.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
        {
            sampledPoint = left;
            return true;
        }
        if (Math.Abs(right.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
        {
            sampledPoint = right;
            return true;
        }
        if (left.SegmentIndex != right.SegmentIndex || left.Direction != right.Direction ||
            left.BoundaryType != right.BoundaryType)
        {
            return false;
        }

        double fraction = (timeFromStart - left.TimeFromStart) / (right.TimeFromStart - left.TimeFromStart);
        sampledPoint = new EmTrajectoryPoint(
            Interpolate(left.X, right.X, fraction),
            Interpolate(left.Y, right.Y, fraction),
            Interpolate(left.Yaw, right.Yaw, fraction),
            Interpolate(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, fraction),
            timeFromStart,
            Interpolate(left.VehicleCurvature, right.VehicleCurvature, fraction),
            left.SegmentIndex,
            Interpolate(left.SegmentLocalS, right.SegmentLocalS, fraction),
            Interpolate(left.PathS, right.PathS, fraction),
            left.Direction,
            left.BoundaryType,
            Interpolate(left.LongitudinalAcceleration, right.LongitudinalAcceleration, fraction),
            Interpolate(left.LongitudinalJerk, right.LongitudinalJerk, fraction));
        return true;
    }

    private static double Interpolate(double left, double right, double fraction)
    {
        return left + (right - left) * fraction;
    }
}