ID/Assets/Code/Lab6/VertexPathFollower.cs

using Code.Base;
using System.Collections.Generic;
using Code.Lab1;
using UnityEngine;

namespace Code.Lab6
{
    public class VertexPathFollower : Seek
    {
        private List<Vertex> _path;
        private int _currentTargetIndex;
        private bool _isFollowing;
        
        public float waypointReachedDistance = 0.5f;
        public bool loopPath;
        public bool drawGizmos = true;
        
        public float agentHeight = 1f;

        public bool fixYPosition = true;

        private float _startY;

        public override void Awake()
        {
            base.Awake();
            if (target == null)
            {
                target = new GameObject("PathFollower_Target");
            }
            _path = new List<Vertex>();
            _isFollowing = false;

            _startY = transform.position.y;
        }

        public void SetPath(List<Vertex> newPath)
        {
            if (newPath == null || newPath.Count == 0)
            {
                Debug.LogWarning("Path is empty!");
                StopFollowing();
                return;
            }

            _path = newPath;
            _currentTargetIndex = 0;
            _isFollowing = true;
            
            Debug.Log($"Path set! Starting movement to {_path.Count} waypoints");

            UpdateTargetPosition();
        }

        public void StopFollowing()
        {
            _isFollowing = false;
            _path?.Clear();
        }

        private void UpdateTargetPosition()
        {
            if (!target || _path == null || _path.Count == 0)
                return;
                
            if (_currentTargetIndex < _path.Count && _path[_currentTargetIndex] != null)
            {
                var targetPos = _path[_currentTargetIndex].transform.position;
                targetPos.y = agentHeight;
                target.transform.position = targetPos;
            }
        }

        public override Steering GetSteering()
        {
            if (!_isFollowing || _path == null || _path.Count == 0 || _currentTargetIndex >= _path.Count)
            {
                return new Steering();
            }

            if (fixYPosition)
            {
                var currentPos = transform.position;
                currentPos.y = agentHeight;
                transform.position = currentPos;
            }

            var distanceToTarget = Vector3.Distance(
                new Vector3(transform.position.x, 0, transform.position.z), 
                new Vector3(target.transform.position.x, 0, target.transform.position.z)
            );

            if (distanceToTarget < waypointReachedDistance)
            {
                _currentTargetIndex++;

                if (_currentTargetIndex >= _path.Count)
                {
                    if (loopPath)
                    {
                        _currentTargetIndex = 0;
                        Debug.Log("Path completed! Looping...");
                    }
                    else
                    {
                        Debug.Log("Path completed! Stopping.");
                        StopFollowing();
                        return new Steering();
                    }
                }

                UpdateTargetPosition();
            }

            var steering = base.GetSteering();

            steering.Linear = new Vector3(steering.Linear.x, 0, steering.Linear.z);
            
            return steering;
        }

        private void FixedUpdate()
        {
            if (fixYPosition)
            {
                var pos = transform.position;
                pos.y = agentHeight;
                transform.position = pos;
            }
        }

        public bool IsFollowing()
        {
            return _isFollowing && _path != null && _path.Count > 0 && _currentTargetIndex < _path.Count;
        }

        public Vertex GetCurrentTarget()
        {
            if (!_isFollowing || _path == null || _path.Count == 0 || _currentTargetIndex >= _path.Count)
                return null;
                
            return _path[_currentTargetIndex];
        }

        public int GetRemainingWaypoints()
        {
            if (!_isFollowing || _path == null)
                return 0;
                
            return Mathf.Max(0, _path.Count - _currentTargetIndex);
        }

        private void OnDrawGizmos()
        {
            if (!drawGizmos || _path == null) return;

            Gizmos.color = Color.green;
            for (var i = 0; i < _path.Count - 1; i++)
            {
                if (_path[i] != null && _path[i + 1] != null)
                {
                    var start = _path[i].transform.position;
                    var end = _path[i + 1].transform.position;
                    start.y = agentHeight;
                    end.y = agentHeight;
                    Gizmos.DrawLine(start, end);
                }
            }

            if (_isFollowing && _currentTargetIndex < _path.Count && _path[_currentTargetIndex] != null)
            {
                var targetPos = _path[_currentTargetIndex].transform.position;
                targetPos.y = agentHeight;
                
                Gizmos.color = Color.red;
                Gizmos.DrawSphere(targetPos, 0.3f);

                Gizmos.color = Color.yellow;
                var agentPos = transform.position;
                agentPos.y = agentHeight;
                Gizmos.DrawLine(agentPos, targetPos);
            }

#if UNITY_EDITOR
            for (var i = 0; i < _path.Count; i++)
            {
                if (_path[i] == null) continue;
                
                var labelPos = _path[i].transform.position;
                labelPos.y = agentHeight + 0.5f;
                
                UnityEditor.Handles.color = Color.white;
                UnityEditor.Handles.Label(labelPos, $"#{i}");
            }
#endif
        }
    }
}