using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;

/// <summary>平滑空间路径上的不可变采样点；位置和长度单位为 m，航向为 rad，曲率为 1/m。</summary>
public sealed class SmoothedPathPoint
{
    /// <summary>
    /// 创建不显式提供曲率导数的不可变平滑路径点；曲率对弧长导数按 0 处理。
    /// </summary>
    /// <param name="xMeters">世界 X 坐标，单位 m。</param>
    /// <param name="yMeters">世界 Y 坐标，单位 m。</param>
    /// <param name="headingRadians">规范化车辆航向，单位 rad。</param>
    /// <param name="unwrappedHeadingRadians">跨越 ±π 后仍连续的车辆航向，单位 rad。</param>
    /// <param name="arcLengthMeters">从完整路径起点累计的弧长，单位 m。</param>
    /// <param name="direction">该点所属方向段的实际行驶方向。</param>
    /// <param name="geometricCurvaturePerMeter">几何曲线曲率，单位 1/m。</param>
    /// <param name="vehicleCurvaturePerMeter">车辆模型使用的有符号曲率，单位 1/m。</param>
    /// <param name="bodyClearanceMeters">扩大车体后的保守净空下界，单位 m。</param>
    /// <param name="isGearSwitchPoint">为 <see langword="true"/> 时表示新方向段开始的精确换向点。</param>
    /// <param name="source">该点在平滑流程中的来源枚举。</param>
    public SmoothedPathPoint(
        double xMeters,
        double yMeters,
        double headingRadians,
        double unwrappedHeadingRadians,
        double arcLengthMeters,
        TravelDirection direction,
        double geometricCurvaturePerMeter,
        double vehicleCurvaturePerMeter,
        double bodyClearanceMeters,
        bool isGearSwitchPoint,
        SmoothedPathPointSource source)
        : this(
            xMeters,
            yMeters,
            headingRadians,
            unwrappedHeadingRadians,
            arcLengthMeters,
            direction,
            geometricCurvaturePerMeter,
            vehicleCurvaturePerMeter,
            0d,
            bodyClearanceMeters,
            isGearSwitchPoint,
            source)
    {
    }

    /// <summary>创建带有车辆曲率对弧长导数的不可变平滑路径点。</summary>
    /// <param name="xMeters">世界 X 坐标，单位 m。</param>
    /// <param name="yMeters">世界 Y 坐标，单位 m。</param>
    /// <param name="headingRadians">规范化车辆航向，单位 rad。</param>
    /// <param name="unwrappedHeadingRadians">连续展开的车辆航向，单位 rad。</param>
    /// <param name="arcLengthMeters">从完整路径起点累计的弧长，单位 m。</param>
    /// <param name="direction">该点所属的前进或倒车方向。</param>
    /// <param name="geometricCurvaturePerMeter">几何曲率，单位 1/m。</param>
    /// <param name="vehicleCurvaturePerMeter">车辆曲率，单位 1/m。</param>
    /// <param name="vehicleCurvatureDerivativePerSquareMeter">车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</param>
    /// <param name="bodyClearanceMeters">扩大车体后的保守净空下界，单位 m。</param>
    /// <param name="isGearSwitchPoint">是否为新方向段开始的换向点。</param>
    /// <param name="source">该点的平滑来源。</param>
    public SmoothedPathPoint(
        double xMeters,
        double yMeters,
        double headingRadians,
        double unwrappedHeadingRadians,
        double arcLengthMeters,
        TravelDirection direction,
        double geometricCurvaturePerMeter,
        double vehicleCurvaturePerMeter,
        double vehicleCurvatureDerivativePerSquareMeter,
        double bodyClearanceMeters,
        bool isGearSwitchPoint,
        SmoothedPathPointSource source)
    {
        X = xMeters;
        Y = yMeters;
        Heading = headingRadians;
        UnwrappedHeading = unwrappedHeadingRadians;
        ArcLength = arcLengthMeters;
        Direction = direction;
        GeometricCurvature = geometricCurvaturePerMeter;
        VehicleCurvature = vehicleCurvaturePerMeter;
        VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter;
        BodyClearance = bodyClearanceMeters;
        IsGearSwitchPoint = isGearSwitchPoint;
        Source = source;
    }

    /// <summary>世界 X 坐标，单位 m。</summary>
    public double X { get; }

    /// <summary>世界 Y 坐标，单位 m。</summary>
    public double Y { get; }

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

    /// <summary>连续展开的车辆航向，单位 rad。</summary>
    public double UnwrappedHeading { get; }

    /// <summary>从完整路径起点累计的弧长，单位 m。</summary>
    public double ArcLength { get; }

    /// <summary>该点所在连续方向段的行驶方向。</summary>
    public TravelDirection Direction { get; }

    /// <summary>按几何弧长计算的有符号路径曲率，单位 1/m。</summary>
    public double GeometricCurvature { get; }

    /// <summary>车辆模型使用的有符号曲率，单位 1/m。</summary>
    public double VehicleCurvature { get; }

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

    /// <summary>扩大车体后的保守净空下界，单位 m。</summary>
    public double BodyClearance { get; }

    /// <summary>该点是否为新方向段开始的换向点。</summary>
    public bool IsGearSwitchPoint { get; }

    /// <summary>该点在平滑流程中的来源。</summary>
    public SmoothedPathPointSource Source { get; }
}
