namespace MultiWheelC.TrajectoryPlanning.CoarsePath;

/// <summary>最终粗路径上的一个稠密连续点；长度、净空和位置使用 m，航向使用 rad，曲率使用 1/m。</summary>
public sealed class CoarsePathPoint
{
    /// <summary>
    /// 创建粗路径点。
    /// 参数：xMeters、yMeters 为世界坐标 m；headingRadians 与 unwrappedHeadingRadians 为航向 rad；arcLengthMeters 和 bodyClearanceMeters 为 m；vehicleCurvaturePerMeter 为 1/m。
    /// </summary>
    public CoarsePathPoint(
        double xMeters,
        double yMeters,
        double headingRadians,
        double unwrappedHeadingRadians,
        double arcLengthMeters,
        TravelDirection direction,
        double vehicleCurvaturePerMeter,
        double bodyClearanceMeters,
        bool isGearSwitchPoint,
        CoarsePathPointSource source)
    {
        X = xMeters;
        Y = yMeters;
        Heading = headingRadians;
        UnwrappedHeading = unwrappedHeadingRadians;
        ArcLength = arcLengthMeters;
        Direction = direction;
        VehicleCurvature = vehicleCurvaturePerMeter;
        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 VehicleCurvature { get; }

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

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

    /// <summary>此点由起点、普通原语或终点截断产生的来源。</summary>
    public CoarsePathPointSource Source { get; }
}