Untitled

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; // Number of packages carried by this robot.

    public TextMeshProUGUI speedInfo; // Display the current speed.
    public TextMeshProUGUI packageInfo; // Display the number of packages.
    public TextMeshProUGUI parcelInfo; // Display the delivery state.

    public float MoveSpeed
    {
        get => isStopped ? 0 : moveSpeed;
        set
        {
            moveSpeed = value;
            if (value == 0) isStopped = true;
        }
    }

    private bool hasReturned = false;
    public bool HasReturned()
    {
        return hasReturned;
    }

    public void SetReturned(bool value)
    {
        hasReturned = value;
    }
    private AStarManager AStarManager = new AStarManager();
    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; // Robot's game object.
    public Camera mainCamera;
    public Vector3 cameraOffset = new Vector3(0, 5, -10);
    public float cameraFollowSpeed = 5f;
    public float turnSpeed = 2f;

    private int currentWaypointIndex = 0;
    private Vector3 offSet = new Vector3(0, 0.3f, 0);

    public GameObject boxPrefab; // Box prefab.
    private GameObject currentBox; // Box instance for this robot.

    private bool boxCarrying = false;

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

    void Start()
    {
        if (!ValidateSetup()) return;

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

        // Find waypoints and connections.
        FindWaypointsAndConnections();

        // Set initial path to pickup point.
        SetPathToDestination(start, pickupPoint);
        MoveSpeed = baseSpeed;
    }

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

        MoveCarAlongPath();
        UpdateCameraPosition();
        UpdateParcelInfo();
        UpdateSpeedInfo();
        UpdatePackageInfo();
    }
    public void ResumeAgent()
    {
        isStopped = false;
        moveSpeed = baseSpeed - (baseSpeed * numberOfPackages * 0.1f); // Adjust speed based on the number of packages.
        Debug.Log($"{name} resumed movement at speed {moveSpeed}");
    }

    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 true;
    }

    private void FindWaypointsAndConnections()
    {
        GameObject[] waypoints = GameObject.FindGameObjectsWithTag("Waypoint");
        foreach (GameObject waypoint in waypoints)
        {
            WaypointCON waypointCon = waypoint.GetComponent<WaypointCON>();
            if (waypointCon)
            {
                Waypoints.Add(waypoint);
                foreach (GameObject connectionNode in waypointCon.Connections)
                {
                    Connection connection = new Connection();
                    connection.SetFromNode(waypoint);
                    connection.SetToNode(connectionNode);
                    AStarManager.AddConnection(connection);
                }
            }
        }
    }

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

        Transform targetWaypoint = ConnectionArray[currentWaypointIndex].GetToNode().transform;
        Vector3 direction = targetWaypoint.position - car.transform.position;
        direction.y = 0;

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

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

        if (Vector3.Distance(car.transform.position, targetWaypoint.position) < 0.1f)
        {
            currentWaypointIndex++;
        }
    }

    private void UpdateCameraPosition()
    {
        if (mainCamera != null)
        {
            Vector3 desiredPosition = car.transform.position + car.transform.rotation * cameraOffset;
            mainCamera.transform.position = Vector3.Lerp(mainCamera.transform.position, desiredPosition, Time.deltaTime * cameraFollowSpeed);
            mainCamera.transform.LookAt(car.transform.position);
        }
    }

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

    private IEnumerator PickupBox()
    {
        yield return new WaitForSeconds(2);
        currentBox.SetActive(false);
        boxCarrying = true;
        DecreaseSpeedForPackages();
        SetPathToDestination(pickupPoint, deliveryPoint);
        currentState = State.MovingToDelivery;
    }

    private IEnumerator DeliverBox()
    {
        yield return new WaitForSeconds(2);
        currentBox.transform.position = deliveryPoint.transform.position;
        currentBox.SetActive(true);
        boxCarrying = false;
        ResetSpeedAfterDelivery();
        yield return new WaitForSeconds(1);
        SetPathToDestination(deliveryPoint, returnPoint);
        currentState = State.ReturningToStart;
    }

    private void StopCar()
    {
        MoveSpeed = 0;
        Debug.Log($"{car.name} has completed its journey.");
    }

    private void SetPathToDestination(GameObject from, GameObject to)
    {
        ConnectionArray.Clear();
        ConnectionArray = AStarManager.PathfindAStar(from, to);
        currentWaypointIndex = ConnectionArray.Count > 0 ? 0 : currentWaypointIndex;
    }

    private void UpdateParcelInfo()
    {
        if (parcelInfo == null)
        {
            Debug.LogError("ParcelInfo TextMeshProUGUI is not assigned.");
            return;
        }
        parcelInfo.text = currentState switch
        {
            State.MovingToPickup => "Heading to Pickup Point...",
            State.MovingToDelivery => "Carrying Package to Delivery Point...",
            State.ReturningToStart => "Returning to Start Point...",
            State.Finished => "Delivery Complete.",
            _ => "Idle"
        };
    }

    private void UpdateSpeedInfo()
    {
        if (speedInfo != null)
        {
            speedInfo.text = $"Speed: {MoveSpeed:F2} km/h";
        }
    }

    private void UpdatePackageInfo()
    {
        if (packageInfo != null)
        {
            packageInfo.text = $"Packages: {numberOfPackages}";
        }
    }

    private void DecreaseSpeedForPackages()
    {
        MoveSpeed = baseSpeed * Mathf.Pow(0.9f, numberOfPackages);

    }

    private void ResetSpeedAfterDelivery()
    {
        MoveSpeed = baseSpeed;
    }
}