Untitled

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

public class ACOTester : MonoBehaviour
{
    // The ACO Controller.
    ACOCON MyACOCON = new ACOCON();

    // List of all waypoints (using VisGraphWaypointManager).
    List<VisGraphWaypointManager> Waypoints = new List<VisGraphWaypointManager>();

    // Connections between nodes.
    private List<ACOConnection> Connections = new List<ACOConnection>();

    // The route generated by the ACO algorithm.
    private List<ACOConnection> MyRoute = new List<ACOConnection>();

    // Debug line offset.
    private Vector3 OffSet = new Vector3(0, 0.5f, 0);

    // The Start node for any created route.
    public GameObject StartNode;

    // The max length of a path created by the ACO.
    public int MaxPathLength;

    // Movement related variables
    private int currentPathIndex = 0; // Current index in the path.
    private bool isHeadingToEnd = true; // Whether the agent is heading to the end.
    private bool isReturningToStart = false; // Whether the agent is returning to the start.
    private float moveSpeed = 6f;  // Speed of the vehicle.
    private float turnSpeed = 3f;  // Turn speed of the vehicle.
    private Vector3 previousPosition;  // Agent's previous position for distance calculation.

    // Collision related variables
    private float collisionDistance = 10f;  // Minimum distance for collision detection
    private float safeDistance = 11f;  // Distance to maintain to avoid collision
    private List<GameObject> allVehicles = new List<GameObject>();

    // Reference to the vehicle (agent)
    public GameObject Agent;

    // Start is called before the first frame update
    void Start()
    {
        // Ensure the Agent is set, if not, assign the current object as the agent.
        if (Agent == null)
        {
            Agent = gameObject; // Default to the object this script is attached to.
        }

        // Find all the waypoints in the level.
        GameObject[] GameObjectsWithWaypointTag;
        GameObjectsWithWaypointTag = GameObject.FindGameObjectsWithTag("Waypoint");

        foreach (GameObject waypoint in GameObjectsWithWaypointTag)
        {
            // Get the VisGraphWaypointManager component from the waypoint.
            VisGraphWaypointManager tmpWaypointManager = waypoint.GetComponent<VisGraphWaypointManager>();

            if (tmpWaypointManager)
            {
                // Add the waypoint to the list if it is of type Goal.
                if (tmpWaypointManager.WaypointType == VisGraphWaypointManager.waypointPropsList.Goal)
                {
                    Waypoints.Add(tmpWaypointManager);
                }
            }
        }

        // Go through the waypoints and create connections.
        foreach (VisGraphWaypointManager waypointManager in Waypoints)
        {
            // Loop through the connections of each waypoint.
            foreach (VisGraphConnection connection in waypointManager.Connections)
            {
                ACOConnection aConnection = new ACOConnection();

                // Set the connection using the FromNode (current waypoint) and ToNode (connected waypoint).
                aConnection.SetConnection(waypointManager.gameObject, connection.ToNode.gameObject, MyACOCON.GetDefaultPheromone());

                Connections.Add(aConnection);
            }
        }

        // Create an array of GameObjects for the ACO algorithm.
        GameObject[] waypointGameObjects = new GameObject[Waypoints.Count];
        for (int i = 0; i < Waypoints.Count; i++)
        {
            waypointGameObjects[i] = Waypoints[i].gameObject;  // Convert VisGraphWaypointManager to GameObject
        }

        // Run the ACO algorithm to find the best route.
        MyRoute = MyACOCON.ACO(50, 20, waypointGameObjects, Connections, StartNode, MaxPathLength);

        // Set initial position
        previousPosition = Agent.transform.position;

        // Populate all vehicles in the scene (this can be optimized if needed).
        allVehicles.AddRange(GameObject.FindGameObjectsWithTag("Taxi"));
    }

    // Update is called once per frame
    void Update()
    {
        // Move the agent along the computed path.
        if (isHeadingToEnd && currentPathIndex < MyRoute.Count)
        {
            MoveAgentAlongPath(MyRoute);
        }
        else if (isReturningToStart && currentPathIndex >= 0)
        {
            MoveAgentAlongPath(MyRoute);
        }

        // Handle vehicle collision detection
        ManageVehicleCollisions();
    }

    // Move the agent along the path
    void MoveAgentAlongPath(List<ACOConnection> path)
    {
        // Determine the current connection and target position.
        ACOConnection currentConnection = path[currentPathIndex];
        Vector3 targetPosition = isReturningToStart
            ? currentConnection.GetFromNode().transform.position
            : currentConnection.GetToNode().transform.position;

        // Rotate the agent towards the target.
        Vector3 direction = (targetPosition - Agent.transform.position).normalized;
        if (direction != Vector3.zero)
        {
            Quaternion targetRotation = Quaternion.LookRotation(direction);
            Agent.transform.rotation = Quaternion.Slerp(Agent.transform.rotation, targetRotation, turnSpeed * Time.deltaTime);
        }

        // Move the agent towards the target position.
        Agent.transform.position = Vector3.MoveTowards(Agent.transform.position, targetPosition, moveSpeed * Time.deltaTime);

        // Check if the agent reached the current target.
        if (Vector3.Distance(Agent.transform.position, targetPosition) < 0.1f)
        {
            if (isHeadingToEnd)
            {
                currentPathIndex++;
                if (currentPathIndex >= path.Count)
                {
                    isHeadingToEnd = false;
                    Debug.Log("Reached the end point.");
                    InitiateReturnPath();
                }
            }
            else if (isReturningToStart)
            {
                currentPathIndex--;
                if (currentPathIndex < 0)
                {
                    Debug.Log("Reached the start point.");
                    isReturningToStart = false;
                }
            }
        }
    }

    // Initiates return path after reaching the end.
    void InitiateReturnPath()
    {
        currentPathIndex = MyRoute.Count - 1;
        isReturningToStart = true;
    }

    // Manage vehicle collisions and adjust vehicle behavior accordingly.
    void ManageVehicleCollisions()
    {
        foreach (GameObject otherVehicle in allVehicles)
        {
            if (otherVehicle != Agent)
            {
                float distance = Vector3.Distance(Agent.transform.position, otherVehicle.transform.position);

                if (distance < collisionDistance)
                {
                    HandleCollisionPriority(otherVehicle);
                }
                else if (distance < safeDistance)
                {
                    ResumeVehicleSpeed();
                }
            }
        }
    }

    // Handle collision priority between vehicles.
    void HandleCollisionPriority(GameObject otherVehicle)
    {
        int priorityA = GetVehiclePriority(Agent);
        int priorityB = GetVehiclePriority(otherVehicle);

        if (priorityA < priorityB)
        {
            StopVehicle(otherVehicle);
        }
        else if (priorityB < priorityA)
        {
            StopVehicle(Agent);
        }
        else
        {
            if (moveSpeed > 0f)
            {
                StopVehicle(otherVehicle);
            }
            else
            {
                StopVehicle(Agent);
            }
        }
    }

    // Stop the vehicle to avoid collision.
    void StopVehicle(GameObject vehicle)
    {
        if (vehicle == Agent)
        {
            moveSpeed = 0f;
            Debug.Log($"{Agent.name} has stopped to avoid collision.");
        }
    }

    // Resume vehicle speed after avoiding a collision.
    void ResumeVehicleSpeed()
    {
        if (moveSpeed == 0f)
        {
            moveSpeed = 6f;
            Debug.Log($"{Agent.name} has resumed movement.");
        }
    }

    // Get the priority of the vehicle based on its name (this can be modified).
    int GetVehiclePriority(GameObject vehicle)
    {
        if (vehicle.name == "Vehicle_1")
        {
            return 1;
        }
        else if (vehicle.name == "Vehicle_2")
        {
            return 2;
        }
        return int.MaxValue;
    }

    // Draws debug objects in the editor and during editor play (if option set).
    void OnDrawGizmos()
    {
        // Draw path.
        if (MyRoute.Count > 0)
        {
            foreach (ACOConnection aConnection in MyRoute)
            {
                Gizmos.color = Color.white;
                Gizmos.DrawLine((aConnection.GetFromNode().transform.position + OffSet),
                                (aConnection.GetToNode().transform.position + OffSet));
            }
        }
    }
}