using System;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
public static class GoalToleranceChecker
{
    /// <summary>
    /// 判断一个位姿是否满足目标容差和进入方向约束。
    /// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
    /// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
    /// </summary>
    public static bool IsSatisfied(
        Pose2D pose,
        Pose2D goal,
        HybridAStarConfiguration configuration,
        TravelDirection direction,
        GoalDirectionConstraint goalDirection)
    {
        if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
            !NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
            !NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
            configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
            !IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
            return false;

        if (!MatchesDirection(direction, goalDirection)) return false;

        double deltaX = pose.X - goal.X;
        double deltaY = pose.Y - goal.Y;
        double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
            return false;

        double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
        return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
    }

    private static bool IsFinitePose(Pose2D pose)
    {
        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
    }

    private static bool IsTravelDirection(TravelDirection direction)
    {
        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
    }

    private static bool IsGoalDirection(GoalDirectionConstraint direction)
    {
        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
    }

    private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
    {
        return constraint == GoalDirectionConstraint.Any ||
            (constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
            (constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
    }
}