SHADE_Y3/Assets/Scripts/AIBehaviour/Implemented/LeafNodes/LeafPatrol.cs

/*********************************************************************
 * \file   LeafPatrol.cs
 * \author Ryan Wang Nian Jing
 * \brief  Leaf node implementation for patrolling AI
 *         
 *
 * \copyright Copyright (c) 2022 DigiPen Institute of Technology. Reproduction
   or disclosure of this file or its contents without the prior written
   consent of DigiPen Institute of Technology is prohibited.
 *********************************************************************/

using SHADE;
using SHADE_Scripting.AIBehaviour.BehaviourTree;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

//VARIABLES HERE
public partial class LeafPatrol : BehaviourTreeNode
{
  //Waypoints and movement
  private Transform transform;
  private List<GameObject> waypoints;
  private RigidBody rb;
  private float patrolSpeed;
  private float turningSpeed;
  private float retreatTimer = 0.0f;
  private int currentWaypointIndex = 0;
  private bool retreatState = false;

  //Small delays between waypoints
  private bool isWaiting = false;
  private const float waitDuration = 2.0f;
  private float waitCounter = 0.0f;
}

//FUNCTIONS HERE
public partial class LeafPatrol : BehaviourTreeNode
{
  //Constructor, establish values here
  //Despite inheriting from BehaviourTreeNode, we don't have children to this
  //node, and hence we do not need to inherit its constructors
  public LeafPatrol(string name, Transform t, float patrolSpeed, float turnSpeed, RigidBody rb) : base(name)
  {
    transform = t;
    this.patrolSpeed = patrolSpeed;
    turningSpeed = turnSpeed;
    this.rb = rb;

    currentWaypointIndex = 0;
  }

  //When it comes to evaluating,
  //le this node keep returning RUNNING as it is the last fallback node on tree
  public override BehaviourTreeNodeStatus Evaluate()
  {
    //Debug.LogWarning("LeafPatrol");
    onEnter(BehaviourTreeNodeStatus.RUNNING);
    if(GetNodeData("currentWaypointIndex") == null)
    {
      SetNodeData("currentWaypointIndex", 0);
    }
    if (isWaiting) DelayAtWaypoint();
    else MoveToWaypoint();

    status = BehaviourTreeNodeStatus.RUNNING;
    onExit(BehaviourTreeNodeStatus.RUNNING);
    return status;
  }

  //Move and cycle between waypoints
  private void MoveToWaypoint()
  {
    //Debug.Log("MoveToWaypoint");
    //Waiting, do not move
    if (GetNodeData("isWaiting") != null)
    {
      waitCounter = 0.0f;
      isWaiting = true;
      ClearNodeData("isWaiting");
      return;
    }

    waypoints = (List<GameObject>)GetNodeData("waypoints");
    Vector3 targetPosition = waypoints[currentWaypointIndex].GetComponent<Transform>().GlobalPosition;

    //Reach waypoint by X and Z being near enough
    //Do not consider Y of waypoints yet
    Vector3 remainingDistance = targetPosition - transform.GlobalPosition;
    remainingDistance.y = 0.0f;

    //Reached waypoint, cycle
    if (remainingDistance.GetSqrMagnitude() < 0.1f)
    {
      //Cycle waypoints
      ++currentWaypointIndex;
      if (currentWaypointIndex >= waypoints.Count())
        currentWaypointIndex = 0;

      //Write to blackboard
      SetNodeData("currentWaypointIndex", currentWaypointIndex);

      waitCounter = 0.0f;
      isWaiting = true;
    }
    else if (false /*Physics.OverlapSphere(_selfTransform.position, 0.3f, 1 << 8).Length > 0 && retreatState == false*/)
    {
      //TODO
      //This main segment is to check if the NPC is walking into a solid wall
      //If they are, do a raycast to find the nearest unobstructed waypoint and head there instead
    }
    else //Proceed to waypoint as usual
    {
      //Get the difference vector to the waypoint
      //Debug.Log("Current Waypoint " + waypoints[currentWaypointIndex].x.ToString() + " " + waypoints[currentWaypointIndex].y.ToString() + " " + waypoints[currentWaypointIndex].z.ToString());
      //Debug.Log("AI is at " + transform.GlobalPosition.x.ToString() + " " + transform.GlobalPosition.y.ToString() + " " + transform.GlobalPosition.z.ToString());
      Vector3 normalisedDifference = targetPosition - transform.GlobalPosition;
      normalisedDifference.y = 0.0f; //Do not move vertically
      normalisedDifference /= normalisedDifference.GetMagnitude();
      //Debug.Log("Normalised Difference x " + normalisedDifference.x.ToString() + " z " + normalisedDifference.z.ToString());

      //Look at the correct direction
      Quaternion targetRotation = Quaternion.LookRotation(normalisedDifference, new Vector3(0.0f, 1.0f, 0.0f));
      transform.LocalRotation = Quaternion.Slerp(transform.LocalRotation, targetRotation, turningSpeed * Time.DeltaTimeF);

      //transform.GlobalPosition += normalisedDifference * moveSpeed * (float)Time.DeltaTime;
      //rb.LinearVelocity = normalisedDifference * patrolSpeed;

      //ORIGINAL INTENDED CODE
      /*rb.AddForce(new Vector3(normalisedDifference.x, 0.0f, normalisedDifference.z) * movementForceMultiplier);
      float currentSpeed = MathF.Sqrt(rb.LinearVelocity.x * rb.LinearVelocity.x + rb.LinearVelocity.z * rb.LinearVelocity.z);
      if (currentSpeed > patrolSpeed)
      {
        float adjustmentFactor = patrolSpeed / currentSpeed;
        Vector3 adjustedVelocity = rb.LinearVelocity;
        //adjustedVelocity *= adjustmentFactor;
        adjustedVelocity.x = patrolSpeed;
        adjustedVelocity.z = patrolSpeed;
        rb.LinearVelocity = adjustedVelocity;
      }*/

      //TODO delete this when original intended code above works with velocity being limited correctly
      if (rb != null)
      {
        //Debug.Log("Null check passed?");
        rb.LinearVelocity = normalisedDifference * patrolSpeed;
      }
      if (retreatState)
      {
        if (retreatTimer < 1.0f) retreatTimer += Time.DeltaTimeF;
        else
        {
          retreatState = false;
          retreatTimer = 1.0f;
        }
      }
    }
  }

  private void DelayAtWaypoint()
  {
    //Debug.Log("DelayAtWaypoint");
    waitCounter += Time.DeltaTimeF;
    if (waitCounter >= waitDuration)
      isWaiting = false;
  }
}