PathfindingTester

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using TMPro;

public class PathfindingTester : MonoBehaviour
{
    private float moveSpeed;
    public bool isStopped = false;
    public float baseSpeed = 20f;

    public int numberOfPackages = 0;

    public TextMeshProUGUI speedInfo;
    public TextMeshProUGUI packageInfo;
    public TextMeshProUGUI parcelInfo;

    public float MoveSpeed
    {
        get => isStopped ? 0 : moveSpeed;
        set
        {
            moveSpeed = value;
            if (value == 0) isStopped = true; // If speed is set to 0, the robot stops
        }
    }

    private bool hasReturned = false;
    public bool HasReturned() => hasReturned; // Returns if the robot has completed its return trip
    public void SetReturned(bool value) => hasReturned = value; // Sets the return status

    private AStarManager AStarManager = new AStarManager();
    private ACOManager ACOManager = new ACOManager(); // Manager for Ant Colony Optimization (ACO)

    private List<GameObject> Waypoints = new List<GameObject>();
    private List<Connection> ConnectionArray = new List<Connection>();

    public GameObject start;
    public GameObject pickupPoint;
    public GameObject deliveryPoint;
    public GameObject returnPoint;

    public GameObject car;
    public Camera mainCamera;
    public Vector3 cameraOffset = new Vector3(0, 5, -10); // Camera offset for following the car
    public float cameraFollowSpeed = 5f; // Speed at which the camera follows the car
    public float turnSpeed = 2f; // Turn speed of the car when changing direction

    private int currentWaypointIndex = 0; // Index of the current waypoint
    private Vector3 offSet = new Vector3(0, 0.3f, 0); // Offset for positioning the car above the ground

    public GameObject boxPrefab;
    private GameObject currentBox;
    private bool boxCarrying = false; // Whether the robot is currently carrying a box

    private enum State { MovingToPickup, MovingToDelivery, ReturningToStart, Finished }
    private State currentState = State.MovingToPickup; // Current state of the robot

    // Start is called before the first frame update
    void Start()
    {
        if (!ValidateSetup()) return; // Validate if all necessary objects are assigned

        currentBox = Instantiate(boxPrefab, pickupPoint.transform.position, Quaternion.identity); // Instantiate the box at the pickup point

        FindWaypointsAndConnections(); // Find waypoints and setup connections for pathfinding

        SetPathToPickupUsingACO(); // Set path using Ant Colony Optimization (ACO) for pickup
        MoveSpeed = baseSpeed; // Set initial speed
    }

    // Update is called once per frame
    void Update()
    {
        // Exit if there are no connections or car object is missing
        if (ConnectionArray.Count == 0 || car == null) return;

        // Exit if task is finished
        if (currentState == State.Finished) return;

        MoveCarAlongPath(); // Move car along calculated path
        UpdateCameraPosition(); // Update camera's position
        UpdateParcelInfo(); // Update parcel's information on UI
        UpdateSpeedInfo(); // Update speed info on UI
        UpdatePackageInfo(); // Update package info on UI
    }

    // Validates setup by checking if essential game objects are assigned
    private bool ValidateSetup()
    {
        if (start == null || pickupPoint == null || deliveryPoint == null || returnPoint == null || boxPrefab == null)
        {
            Debug.LogError("Missing essential objects. Check start, pickupPoint, deliveryPoint, returnPoint, and boxPrefab assignments.");
            return false; // Return false if any essential object is missing
        }
        return true; // Return true if setup is valid
    }

    // Finds all waypoints in the scene and adds connections between them for pathfinding
    private void FindWaypointsAndConnections()
    {
        GameObject[] waypoints = GameObject.FindGameObjectsWithTag("Waypoint"); // Find all waypoints in the scene
        foreach (GameObject waypoint in waypoints)
        {
            WaypointCON waypointCon = waypoint.GetComponent<WaypointCON>(); // Get WaypointCON component of each waypoint
            if (waypointCon)
            {
                Waypoints.Add(waypoint); // Add waypoint to the list of waypoints
                // Add connections between waypoints
                foreach (GameObject connectionNode in waypointCon.Connections)
                {
                    Connection connection = new Connection();
                    connection.SetFromNode(waypoint); // Set connection source
                    connection.SetToNode(connectionNode); // Set connection destination
                    AStarManager.AddConnection(connection); // Add connection to the A* manager
                }
            }
        }
    }

    // Sets the initial path to the pickup point using Ant Colony Optimization (ACO)
    private void SetPathToPickupUsingACO()
    {
        ConnectionArray.Clear(); // Clear previous connections
        ConnectionArray = ACOManager.FindPath(start, pickupPoint, Waypoints); // Find the path to the pickup point using ACO
        currentWaypointIndex = ConnectionArray.Count > 0 ? 0 : currentWaypointIndex; // Set the waypoint index if path is found
        Debug.Log($"[ACO] Path to pickup point calculated. Path length: {ConnectionArray.Count}");
    }

    // Sets the path to the delivery point using A* algorithm
    private void SetPathToDeliveryUsingAStar()
    {
        ConnectionArray.Clear(); // Clear previous connections
        ConnectionArray = AStarManager.PathfindAStar(pickupPoint, deliveryPoint); // Find the path to the delivery point using A*
        currentWaypointIndex = ConnectionArray.Count > 0 ? 0 : currentWaypointIndex; // Set the waypoint index if path is found
        Debug.Log($"[A*] Path to delivery point calculated. Path length: {ConnectionArray.Count}");
    }

    // Handles transitions between states after certain tasks (pickup, delivery, return, etc.)
    private void HandleStateTransition()
    {
        if (currentState == State.MovingToPickup)
        {
            StartCoroutine(PickupBox()); // Pickup box when moving to pickup point
        }
        else if (currentState == State.MovingToDelivery)
        {
            StartCoroutine(DeliverBox()); // Deliver box when moving to delivery point
        }
        else if (currentState == State.ReturningToStart)
        {
            currentState = State.Finished; // Mark task as finished
            hasReturned = true;
            StopCar(); // Stop the car after returning to the start
        }
        else
        {
            Debug.LogError($"Unexpected state in HandleStateTransition: {currentState}");
        }
    }

    // Coroutine for simulating picking up a package
    private IEnumerator PickupBox()
    {
        yield return new WaitForSeconds(2); // Simulate time taken to pick up the box
        currentBox.SetActive(false); // Hide the box after pickup
        boxCarrying = true; // Mark that the robot is carrying the box
        DecreaseSpeedForPackages(); // Reduce speed when carrying packages
        SetPathToDeliveryUsingAStar(); // Set path to the delivery point
        currentState = State.MovingToDelivery; // Transition to moving to delivery state
    }

    // Sets the state to a new value and logs the transition
    private void SetState(State newState)
    {
        Debug.Log($"State changing from {currentState} to {newState}"); // Log state transition
        currentState = newState; // Update the current state
    }

    // Coroutine for simulating delivering a package
    private IEnumerator DeliverBox()
    {
        yield return new WaitForSeconds(2); // Simulate time taken to deliver the box
        currentBox.transform.position = deliveryPoint.transform.position; // Set box's position at the delivery point
        currentBox.SetActive(true); // Show the delivered box
        boxCarrying = false; // Mark that the robot is no longer carrying the box
        ResetSpeedAfterDelivery(); // Reset the speed after delivery
        yield return new WaitForSeconds(1);

        // Transition to returning to start
        currentState = State.ReturningToStart;
        SetPathToDestination(deliveryPoint, returnPoint); // Set the path to return to the start
    }

    // Sets the path for the robot depending on its current state
    private void SetPathToDestination(GameObject from, GameObject to)
    {
        if (currentState == State.MovingToPickup)
        {
            ConnectionArray = ACOManager.FindPath(from, to, Waypoints); // Use ACO for pickup
            Debug.Log($"[ACO] Path from {from.name} to {to.name} calculated.");
        }
        else if (currentState == State.MovingToDelivery)
        {
            ConnectionArray = AStarManager.PathfindAStar(from, to); // Use A* for delivery
            Debug.Log($"[A*] Path from {from.name} to {to.name} calculated.");
        }
        else if (currentState == State.ReturningToStart)
        {
            ConnectionArray = AStarManager.PathfindAStar(from, to); // Use A* for returning to start
            Debug.Log($"[A*] Path from {from.name} to {to.name} calculated (returning to start).");
        }
        else if (currentState == State.Finished)
        {
            Debug.LogWarning("Pathfinding not required. State is 'Finished'.");
        }
        else
        {
            Debug.LogError($"Unexpected state in SetPathToDestination: {currentState}");
        }

        currentWaypointIndex = 0; // Reset index to start from the beginning of the new path
    }

    // Stops the car by setting its speed to zero
    private void StopCar()
    {
        MoveSpeed = 0; // Stop the car
        Debug.Log($"{car.name} has completed its journey.");
    }

    // Moves the car along the calculated path
    private void MoveCarAlongPath()
    {
        if (currentWaypointIndex >= ConnectionArray.Count) // If all waypoints have been traversed
        {
            HandleStateTransition(); // Transition to the next state
            return;
        }

        Transform targetWaypoint = ConnectionArray[currentWaypointIndex].GetToNode().transform; // Get the next waypoint
        Vector3 direction = targetWaypoint.position - car.transform.position; // Calculate direction to the target waypoint
        direction.y = 0; // Keep movement on the XZ plane

        if (direction.magnitude > 0.1f) // If the car is far from the target
        {
            Quaternion targetRotation = Quaternion.LookRotation(direction); // Rotate the car to face the target
            car.transform.rotation = Quaternion.Lerp(car.transform.rotation, targetRotation, Time.deltaTime * turnSpeed); // Smooth rotation
        }

        car.transform.position = Vector3.MoveTowards(car.transform.position, targetWaypoint.position, moveSpeed * Time.deltaTime); // Move the car

        if (Vector3.Distance(car.transform.position, targetWaypoint.position) < 0.1f) // Check if the car has reached the target waypoint
        {
            currentWaypointIndex++; // Move to the next waypoint
        }
    }

    // Updates the camera's position to follow the car
    private void UpdateCameraPosition()
    {
        if (mainCamera != null) // Check if camera is assigned
        {
            Vector3 desiredPosition = car.transform.position + car.transform.rotation * cameraOffset; // Calculate desired camera position
            mainCamera.transform.position = Vector3.Lerp(mainCamera.transform.position, desiredPosition, Time.deltaTime * cameraFollowSpeed); // Smoothly move camera
            mainCamera.transform.LookAt(car.transform.position); // Keep camera focused on the car
        }
    }

    // Updates the text that shows the current parcel's status
    private void UpdateParcelInfo()
    {
        if (parcelInfo == null)
        {
            Debug.LogError("ParcelInfo TextMeshProUGUI is not assigned."); // Check if parcelInfo is assigned
            return;
        }
        parcelInfo.text = currentState switch
        {
            State.MovingToPickup => "Heading to Pickup Point...", // Show message for moving to pickup
            State.MovingToDelivery => "Carrying Package to Delivery Point...", // Show message for delivering package
            State.ReturningToStart => "Returning to Start Point...", // Show message for returning to start
            State.Finished => "Delivery Complete.", // Show message for completed delivery
            _ => "Idle" // Default message when idle
        };
    }

    // Resumes the robot's movement if it has been stopped
    public void ResumeAgent()
    {
        if (isStopped) // If the agent is stopped
        {
            isStopped = false; // Unstop the agent
            MoveSpeed = baseSpeed; // Restore the base speed
            Debug.Log("Agent resumed moving."); // Log that the agent has resumed
        }
    }

    // Gizmos to draw connections in the editor for visualization
    private void OnDrawGizmos()
    {
        if (ConnectionArray == null || ConnectionArray.Count == 0) return; // Exit if there are no connections

        Gizmos.color = currentState == State.MovingToPickup ? Color.red : Color.green; // Set Gizmos color based on state

        foreach (var connection in ConnectionArray) // Iterate through each connection
        {
            Vector3 from = connection.GetFromNode().transform.position; // Get starting position of the connection
            Vector3 to = connection.GetToNode().transform.position; // Get ending position of the connection
            Gizmos.DrawLine(from, to); // Draw line between waypoints in the editor
        }
    }

    // Updates the speed info displayed on the UI
    private void UpdateSpeedInfo()
    {
        if (speedInfo != null) // Check if speedInfo UI element is assigned
        {
            speedInfo.text = $"Speed: {MoveSpeed:F2} km/h"; // Display the speed on the UI
        }
    }

    // Updates the package info displayed on the UI
    private void UpdatePackageInfo()
    {
        if (packageInfo != null) // Check if packageInfo UI element is assigned
        {
            packageInfo.text = $"Packages: {numberOfPackages}"; // Display the number of packages
        }
    }

    // Decreases the speed based on the number of packages being carried
    private void DecreaseSpeedForPackages()
    {
        MoveSpeed = baseSpeed * Mathf.Pow(0.9f, numberOfPackages); // Reduce speed based on the number of packages
    }

    // Resets the speed after a delivery is completed
    private void ResetSpeedAfterDelivery()
    {
        MoveSpeed = baseSpeed; // Reset speed to base speed
    }
}