beep-final/Assets/7_Scripts/Waypoint/WaypointManager.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using Unity.MLAgents;
using UnityEngine;

[ExecuteInEditMode]
public class WaypointManager : MonoBehaviour
{


    [SerializeField]
    public List<Waypoint> m_WaypointList;

    [SerializeField]
    public Waypoint[] m_WaypointArray;

    [SerializeField]
    public Transform m_Target;

    private Vector3 m_StartingPosition;

    [SerializeField]
    public Waypoint m_CurrentWaypoint;

    public Transform m_Agent;

    private bool m_IsMoved;

    public delegate void SwapModelOnReachingWaypoint(string modelName);

    public static event SwapModelOnReachingWaypoint SwapModelOnWaypointReached;

    public delegate void OverrideWaypoint(string modelName);

    public static event OverrideWaypoint OnOverrideWaypoint;

    private void OnEnable()
    {
        m_WaypointList = new List<Waypoint>();
        Waypoint.OnWaypointReached += SetNextWaypoint;
    }

    private void OnDisable()
    {
        Waypoint.OnWaypointReached -= SetNextWaypoint;
    }

    private void Start()
    {
        SortWaypointInChildrenArray();
        m_CurrentWaypoint = m_WaypointList[0];
        MoveTargetToWaypoint();
    }

    private void FixedUpdate()
    {
        if(!m_IsMoved)
        {
            MoveAgentToStartPosition();
            m_IsMoved = true;
        }
        CheckOverride();
        MoveTargetToWaypoint();
    }

    private void MoveTargetToWaypoint()
    {
        Vector3 wpPosition = m_CurrentWaypoint.m_WaypointPosition.position;
        Quaternion wpRotation = m_CurrentWaypoint.m_WaypointPosition.rotation;
        m_Target.position = wpPosition;
        //m_Target.rotation = wpRotation;
    }

    private bool SetNextWaypoint(float index, string modelName)
    {
        if (index == m_CurrentWaypoint.m_Index)
        {
            Debug.Log($"Waypoint reached {index}");
            MoveToNextWaypoint(index);
            SwapModelOnWaypointReached(modelName);
            return true;
        }
        else
        {
            Debug.Log($"Wrong Waypoint reached {index} Current {m_CurrentWaypoint.m_Index}");
            return false;
        }
    }

    private void SetNextWaypointOverride(float index)
    {
        if (index == m_CurrentWaypoint.m_Index)
        {
            Debug.Log($"Waypoint reached {index}");
            MoveToNextWaypoint(index);
        }
        else
        {
            Debug.Log($"Wrong Waypoint reached {index} Current {m_CurrentWaypoint.m_Index}");
        }
    }

    private void MoveToNextWaypoint(float index)
    {
        StartCoroutine(WaypointTimeout(m_CurrentWaypoint.m_WaitTime, index));
    }

    IEnumerator WaypointTimeout(float seconds, float index)
    {
        yield return new WaitForSeconds(seconds);
        if (m_CurrentWaypoint.m_Index < m_WaypointList.Count - 1)
        {
            m_CurrentWaypoint = m_WaypointList[(int)index + 1];
            MoveTargetToWaypoint();
        }
        else
        {
            Debug.Log($"Reached Last Position {m_CurrentWaypoint.m_Index}");
        }
    }

    private void SortWaypointsInChildren()
    {
        foreach (Transform t in transform)
        {
            m_WaypointList.Add(t.GetComponent<Waypoint>());
        }

        m_WaypointList = m_WaypointList.OrderBy(waypoint => waypoint.m_Index).ToList();
    }

    private void SortWaypointInChildrenArray()
    {
        m_WaypointArray = GetComponentsInChildren<Waypoint>();

        foreach (Waypoint wp in m_WaypointArray)
        {
            if ( wp.isActiveAndEnabled )
            {
                m_WaypointList.Add(wp);
                if (wp.m_StartingPosition)
                    m_StartingPosition = wp.transform.position;
            }
        }
        m_WaypointList = m_WaypointList.OrderBy(waypoint => waypoint.m_Index).ToList();
    }

    private void MoveAgentToStartPosition()
    {
        m_Agent.position = m_StartingPosition;
        //Debug.Log($"Moved agent to {m_StartingPosition}");
    }

    private void CheckOverride()
    {
        if(Input.GetKeyUp(KeyCode.F))
        {
            SetNextWaypointOverride(m_CurrentWaypoint.m_Index);
            OnOverrideWaypoint(m_CurrentWaypoint.m_ModelName);
        }
    }

    void OnDrawGizmos()
    {
#if UNITY_EDITOR
        Gizmos.color = Color.red;
        foreach (Waypoint waypoint in m_WaypointList)
        {
            Gizmos.DrawWireSphere(waypoint.m_WaypointPosition.position, 1f);
        }

        Gizmos.color = Color.red;
        for (int i = 0; i < m_WaypointList.Count - 1; i++)
        {
            Gizmos.DrawLine(m_WaypointList[i].m_WaypointPosition.position, m_WaypointList[i + 1].m_WaypointPosition.position);
        }
#endif
    }
}