using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
/// </summary>
public sealed class FrenetReferencePoint
{
    /// <summary>
    /// 创建已验证的方向段参考样本。
    /// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
    /// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
    /// </summary>
    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
        double vehicleCurvatureDerivative, double bodyClearance)
    {
        RequireFinite(referenceS, nameof(referenceS));
        RequireFinite(x, nameof(x));
        RequireFinite(y, nameof(y));
        RequireFinite(vehicleYaw, nameof(vehicleYaw));
        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
        RequireFinite(geometricCurvature, nameof(geometricCurvature));
        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
        RequireFinite(bodyClearance, nameof(bodyClearance));

        ReferenceS = referenceS;
        X = x;
        Y = y;
        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
        UnwrappedVehicleYaw = unwrappedVehicleYaw;
        Direction = direction;
        GeometricCurvature = geometricCurvature;
        VehicleCurvature = vehicleCurvature;
        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
        BodyClearance = bodyClearance;
    }

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

    /// <summary>
    /// 参考中心线点的世界 X 坐标，单位 m。
    /// </summary>
    public double X { get; }

    /// <summary>
    /// 参考中心线点的世界 Y 坐标，单位 m。
    /// </summary>
    public double Y { get; }

    /// <summary>
    /// 归一化后的车辆车身航向，单位 rad。
    /// </summary>
    public double VehicleYaw { get; }

    /// <summary>
    /// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
    /// </summary>
    public double UnwrappedVehicleYaw { get; }

    /// <summary>
    /// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
    /// </summary>
    public TravelDirection Direction { get; }

    /// <summary>
    /// 中心线几何曲率，单位 1/m。
    /// </summary>
    public double GeometricCurvature { get; }

    /// <summary>
    /// 满足车辆模型后的车辆曲率，单位 1/m。
    /// </summary>
    public double VehicleCurvature { get; }

    /// <summary>
    /// 车辆曲率相对弧长的导数，单位 1/m²。
    /// </summary>
    public double VehicleCurvatureDerivative { get; }

    /// <summary>
    /// 参考点记录的车体净空或可用裕度，单位 m。
    /// </summary>
    public double BodyClearance { get; }

    /// <summary>
    /// 获取与当前行驶方向一致的连续航向。
    /// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
    /// </summary>
    public double TravelYaw
    {
        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
    }

    /// <summary>
    /// 拒绝不能安全保存为参考几何的数值。
    /// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
    /// </summary>
    private static void RequireFinite(double value, string name)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentOutOfRangeException(name);
    }
}