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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
/// </summary>
public static class FrenetTransform
{
    /// <summary>
    /// 将参考点及其横向状态重建为世界车辆中心位姿。
    /// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
    /// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
    /// </summary>
    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
        double minimumFrenetDenominator, out Pose2D pose)
    {
        pose = null;
        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
            return false;

        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
            return false;

        double travelYaw = referencePoint.TravelYaw;
        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
            return false;

        pose = new Pose2D(x, y, vehicleYaw);
        return true;
    }

    /// <summary>
    /// 从车辆车身航向取得对应实际行驶的连续航向。
    /// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
    /// </summary>
    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
    {
        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
    }

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