Untitled

public class PathfindingTester : MonoBehaviour
{
    // The A* manager.
    private AStarManager AStarManager = new AStarManager();
    // List of waypoints.
    List<GameObject> Waypoints = new List<GameObject>();
    // Array of waypoint map connections. Represents a path.
    List<Connection> ConnectionArray = new List<Connection>();
    // The start, pickup, delivery, and return target points.
    public GameObject start;
    public GameObject pickupPoint;
    public GameObject deliveryPoint;
    public GameObject returnPoint;
    public TextMeshProUGUI parcelInfo; // Assign this in the Inspector.
    // The car that needs to follow the path.
    public GameObject car; // Assign this in the Inspector.
    // Camera following the car
    public Camera mainCamera;
    // Camera offset
    public Vector3 cameraOffset = new Vector3(0, 5, -10);
    public float cameraFollowSpeed = 5f;

    // Speed of the car.
    public float moveSpeed = 5f;
    public float turnSpeed = 2f; // Speed of the car's rotation.
    // Current target index in the path.
    private int currentWaypointIndex = 0;
    // Debug line offset.
    Vector3 OffSet = new Vector3(0, 0.3f, 0);

    // Box at pickup and delivery points
    public GameObject box; // Assign this in the Inspector.
    private bool boxCarrying = false;

    private enum State { MovingToPickup, MovingToDelivery, ReturningToStart, Finished }
    private State currentState = State.MovingToPickup;

    void Start()
    {
        if (start == null || pickupPoint == null || deliveryPoint == null || returnPoint == null || box == null)
        {
            Debug.LogError("Missing essential objects. Make sure start, pickupPoint, deliveryPoint, returnPoint, and box are assigned.");
            return;
        }

        // Position the box at the pickup point.
        box.transform.position = pickupPoint.transform.position;
        box.SetActive(true); // Make sure the box is visible at the start.

        // Find all the waypoints in the level.
        GameObject[] GameObjectsWithWaypointTag = GameObject.FindGameObjectsWithTag("Waypoint");
        foreach (GameObject waypoint in GameObjectsWithWaypointTag)
        {
            WaypointCON tmpWaypointCon = waypoint.GetComponent<WaypointCON>();
            if (tmpWaypointCon)
            {
                Waypoints.Add(waypoint);
            }
        }

        // Go through the waypoints and create connections.
        foreach (GameObject waypoint in Waypoints)
        {
            WaypointCON tmpWaypointCon = waypoint.GetComponent<WaypointCON>();
            foreach (GameObject WaypointConNode in tmpWaypointCon.Connections)
            {
                Connection aConnection = new Connection();
                aConnection.SetFromNode(waypoint);
                aConnection.SetToNode(WaypointConNode);
                AStarManager.AddConnection(aConnection);
            }
        }

        // Initially calculate the path to the pickup point.
        SetPathToDestination(start, pickupPoint);
    }

    void Update()
    {
        if (ConnectionArray.Count == 0 || car == null) return;

        // If finished, don't process further.
        if (currentState == State.Finished) return;

        // Move the car and update camera.
        MoveCarAlongPath();
        UpdateCameraPosition();

        // Update the parcel info display
        UpdateParcelInfo();
    }

    void MoveCarAlongPath()
    {
        if (currentWaypointIndex >= ConnectionArray.Count)
        {
            HandleStateTransition();
            return;
        }

        Transform targetWaypoint = ConnectionArray[currentWaypointIndex].GetToNode().transform;
        Vector3 direction = targetWaypoint.position - car.transform.position;
        direction.y = 0; // Keep movement in the horizontal plane.

        // Rotate the car towards the target waypoint.
        if (direction.magnitude > 0.1f)
        {
            Quaternion targetRotation = Quaternion.LookRotation(direction);
            car.transform.rotation = Quaternion.Lerp(car.transform.rotation, targetRotation, Time.deltaTime * turnSpeed);
        }

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

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

    void UpdateCameraPosition()
    {
        if (mainCamera != null)
        {
            // Calculate the desired position of the camera.
            Vector3 desiredPosition = car.transform.position + car.transform.rotation * cameraOffset;
            // Smoothly move the camera to follow the car.
            mainCamera.transform.position = Vector3.Lerp(mainCamera.transform.position, desiredPosition, Time.deltaTime * cameraFollowSpeed);
            // Make the camera look at the car.
            mainCamera.transform.LookAt(car.transform.position);
        }
    }

    void HandleStateTransition()
    {
        if (currentState == State.MovingToPickup)
        {
            StartCoroutine(PickupBox());
        }
        else if (currentState == State.MovingToDelivery)
        {
            StartCoroutine(DeliverBox());
        }
        else if (currentState == State.ReturningToStart)
        {
            currentState = State.Finished;
            StopCar();
        }
    }

    IEnumerator PickupBox()
    {
        Debug.Log("Car arrived at pickup point.");
        yield return new WaitForSeconds(2); // Wait for 2 seconds at the pickup point.

        Debug.Log("Box picked up.");
        box.SetActive(false); // Make the box disappear.
        boxCarrying = true;

        // Update the state and set path to delivery.
        SetPathToDestination(pickupPoint, deliveryPoint);
        currentState = State.MovingToDelivery;
    }

    IEnumerator DeliverBox()
    {
        Debug.Log("Car arrived at delivery point."); // Wait for 2 seconds at the delivery point.

        Debug.Log("Box delivered.");
        box.transform.position = deliveryPoint.transform.position; // Move the box to the delivery point, on the floor.
        box.SetActive(true); // Make the box reappear at the delivery point.
        boxCarrying = false;
        // Add a 2-second delay after the delivery before returning to start.
        yield return new WaitForSeconds(1); // Wait for 2 seconds after delivering the package.
        // Update the state and set path to return.
        SetPathToDestination(deliveryPoint, returnPoint);
        currentState = State.ReturningToStart;
    }

    void StopCar()
    {
        moveSpeed = 0;
        Debug.Log("Journey complete.");
    }

    void SetPathToDestination(GameObject from, GameObject to)
    {
        ConnectionArray.Clear();
        ConnectionArray = AStarManager.PathfindAStar(from, to);
        if (ConnectionArray.Count > 0) currentWaypointIndex = 0;
        else Debug.LogWarning("No valid path found between " + from.name + " and " + to.name);
    }

    void UpdateParcelInfo()
    {
        switch (currentState)
        {
            case State.MovingToPickup:
                parcelInfo.text = "Car is heading towards its pickup point.";
                break;
            case State.MovingToDelivery:
                parcelInfo.text = "Just picked up the package and now heading towards the delivery point.";
                break;
            case State.ReturningToStart:
                parcelInfo.text = "Package has just been delivered.";
                break;
            case State.Finished:
                parcelInfo.text = "Delivery Complete.";
                break;
            default:
                parcelInfo.text = "Completed the job";
                break;
        }
    }

    void OnDrawGizmos()
    {
        Gizmos.color = Color.white;
        foreach (Connection aConnection in ConnectionArray)
        {
            Gizmos.DrawLine(aConnection.GetFromNode().transform.position + OffSet,
                            aConnection.GetToNode().transform.position + OffSet);
        }
    }
}