using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
/// </summary>
public sealed class FrenetProjection
{
    /// <summary>
    /// 从参考点和相对量创建不可变投影结果。
    /// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
    /// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
    /// </summary>
    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
        double squaredDistanceMeters)
    {
        if (referencePoint == null)
            throw new ArgumentNullException(nameof(referencePoint));
        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
            throw new ArgumentOutOfRangeException(nameof(lateralOffset));

        ReferencePoint = referencePoint;
        ReferenceS = referencePoint.ReferenceS;
        LateralOffset = lateralOffset;
        HeadingError = headingError;
        SquaredDistanceMeters = squaredDistanceMeters;
    }

    /// <summary>
    /// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
    /// </summary>
    public FrenetReferencePoint ReferencePoint { get; }

    /// <summary>
    /// 命中点在当前方向段的局部参考弧长 S，单位 m。
    /// </summary>
    public double ReferenceS { get; }

    /// <summary>
    /// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
    /// </summary>
    public double LateralOffset { get; }

    /// <summary>
    /// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
    /// </summary>
    public double HeadingError { get; }

    /// <summary>
    /// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
    /// </summary>
    public double SquaredDistanceMeters { get; }

    /// <summary>
    /// 判定投影标量是否有限。
    /// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}