using UnityEngine;
using UnityEngine.AI;

public class HasReachedDestination : Condition
{
    private readonly Vector3 targetPosition;
    private readonly float arrivalThreshold;

    public HasReachedDestination(Vector3 position, string name = null, float arrivalThreshold = 0.7f) : base(name)
    {
        targetPosition = position;
        this.arrivalThreshold = Mathf.Max(0.1f, arrivalThreshold); // Ensure minimum threshold
    }

    public override bool IsConditionMet(HTNPlanner planner)
    {
        NavMeshAgent agent = planner.Agent.NavMeshAgent;

        // Check if agent has a path first
        if (!agent.hasPath || agent.pathPending)
            return false;

        // Check if we're close enough to the target position
        float distanceToTarget = Vector3.Distance(agent.transform.position, targetPosition);

        // Check if we've reached the end of our path
        bool reachedPathEnd = agent.remainingDistance <= arrivalThreshold &&
                            !agent.pathPending &&
                            agent.velocity.sqrMagnitude == 0f;

        #if UNITY_EDITOR
                // Visual debugging
                Debug.DrawLine(planner.Agent.transform.position, targetPosition,
                              reachedPathEnd ? Color.green : Color.yellow);
        #endif

        return reachedPathEnd || distanceToTarget <= arrivalThreshold;
    }
}