using HC3;

public sealed class ReturnToGridAction : AgentAction
{
	private bool _navigating;

	public override float Score()
	{
		if ( Agent.Building.IsValid() || Agent.IsMoving ) return 0;

		var cell = GridManager.Instance.GetCell( GridManager.WorldToGridPosition( WorldPosition ) );
		int level = GridManager.FloorWorldHeightToGrid( WorldPosition.z );
		return cell?.IsWalkable( level ) ?? false ? 0f : 1010f;
	}

	public override void StartAction()
	{
		base.StartAction();
		_navigating = false;

		if ( TryFindRecoveryTarget( out var recoveryPoint ) )
		{
			Controller.Navigate( new List<Vector3> { recoveryPoint } );
			_navigating = true;
		}
	}

	public override Status TickAction()
	{
		if ( !_navigating )
			return Status.Failure;

		if ( Controller.IsNavigating )
			return Status.Running;

		var grid3d = GridManager.WorldToGridPosition3D( WorldPosition );

		var cell = GridManager.Instance.GetCell( GridManager.WorldToGridPosition( WorldPosition ) );
		int level = GridManager.FloorWorldHeightToGrid( WorldPosition.z );
		return cell?.IsWalkable( level ) ?? false ? Status.Success : Status.Failure;
	}

	public override ActionDisplayInfo? GetDisplay()
	{
		return new ActionDisplayInfo( "error", "Returning to grid...", Controller.GetPathProgress() );
	}

	private bool TryFindRecoveryTarget( out Vector3 point )
	{
		point = default;

		// Find closest walkable path tile without LINQ allocation
		float closestDist = float.MaxValue;
		Vector3? closest = null;

		foreach ( var x in Scene.GetAll<Path>() )
		{
			if ( x.RegionId != 0 || x.IsElevated )
				continue;
			if ( !GridNavigation.Instance.CanEnter( new Vector2Int( x.TilePosition ), NavFlags.Default ) )
				continue;

			float dist = x.WorldPosition.DistanceSquared( WorldPosition );
			if ( dist < closestDist )
			{
				closestDist = dist;
				closest = x.WorldPosition;
			}
		}

		if ( closest is null )
			return false;

		point = closest.Value;
		return true;
	}
}