using System;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>
/// Hybrid A* 运行期节点。
/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
/// </summary>
public sealed class HybridAStarNode
{
    /// <summary>
    /// 创建一个搜索节点。
    /// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
    /// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
    /// </summary>
    public HybridAStarNode(
        int nodeIndex,
        HybridAStarNodeKey key,
        Pose2D pose,
        int parentNodeIndex,
        MotionPrimitive incomingPrimitive,
        double curvaturePerMeter,
        double gCostMeters,
        double hCostMeters)
        : this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
    {
    }

    /// <summary>
    /// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
    /// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
    /// </summary>
    public HybridAStarNode(
        int nodeIndex,
        HybridAStarNodeKey key,
        Pose2D pose,
        int parentNodeIndex,
        MotionPrimitive incomingPrimitive,
        double curvaturePerMeter,
        double gCostMeters,
        double hCostMeters,
        bool isGoalCandidate)
    {
        if (key == null) throw new ArgumentNullException(nameof(key));
        if (pose == null) throw new ArgumentNullException(nameof(pose));

        NodeIndex = nodeIndex;
        Key = key;
        Pose = pose;
        ParentNodeIndex = parentNodeIndex;
        IncomingPrimitive = incomingPrimitive;
        CurvaturePerMeter = curvaturePerMeter;
        GCostMeters = gCostMeters;
        HCostMeters = hCostMeters;
        IsGoalCandidate = isGoalCandidate;
    }

    /// <summary>本次搜索节点数组中的稳定索引。</summary>
    public int NodeIndex { get; }

    /// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
    public HybridAStarNodeKey Key { get; }

    /// <summary>未量化的连续车辆中心位姿。</summary>
    public Pose2D Pose { get; }

    /// <summary>父节点稳定索引；根节点为 -1。</summary>
    public int ParentNodeIndex { get; }

    /// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
    public MotionPrimitive IncomingPrimitive { get; }

    /// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
    public MotionPrimitive Primitive { get { return IncomingPrimitive; } }

    /// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
    public double CurvaturePerMeter { get; }

    /// <summary>当前节点的累计等效米代价。</summary>
    public double GCostMeters { get; }

    /// <summary>当前节点的二维启发式等效米代价。</summary>
    public double HCostMeters { get; }

    /// <summary>
    /// 本节点是否由原语内部首次满足终点条件而生成。
    /// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
    /// </summary>
    public bool IsGoalCandidate { get; }

    /// <summary>Open List 排序使用的总等效米代价。</summary>
    public double FCostMeters { get { return GCostMeters + HCostMeters; } }

    /// <summary>本节点末段的行驶方向。</summary>
    public TravelDirection Direction { get { return Key.Direction; } }

    /// <summary>本节点末段的曲率等级索引。</summary>
    public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
}