ID/Assets/Code/Lab3/BetterPathFollower.cs

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

namespace Code.Lab3
{
    public class BetterPathFollower : Seek
    {
        public Path path;
        private List<Vector3> _waypoints;
        private int _currentTargetIndex = -1;
        private List<int> _visitedIndices;
        private bool _reachedLastPoint;

        public override void Awake()
        {
            base.Awake();
            target = new GameObject("BetterPathFollower_Target");
            _visitedIndices = new List<int>();
            InitializeWaypoints();
        }

        private void InitializeWaypoints()
        {
            if (path == null || path.nodes == null || path.nodes.Count == 0) return;

            _waypoints = path.nodes.Select(node => node.transform.position).ToList();
            FindClosestStartPoint();
        }

        private void FindClosestStartPoint()
        {
            var minDistance = float.MaxValue;
            var closestIndex = 0;

            for (var i = 0; i < _waypoints.Count; i++)
            {
                var distance = Vector3.Distance(transform.position, _waypoints[i]);

                if (!(distance < minDistance)) continue;
                
                minDistance = distance;
                closestIndex = i;
            }

            _currentTargetIndex = closestIndex;
            _visitedIndices.Clear();
            _reachedLastPoint = false;
            
            UpdateTargetPosition();
        }

        public override Steering GetSteering()
        {
            if (_waypoints == null || _waypoints.Count == 0 || _reachedLastPoint)
            {
                return new Steering();
            }

            var distanceToTarget = Vector3.Distance(transform.position, _waypoints[_currentTargetIndex]);

            if (distanceToTarget < 1f)
            {
                if (!_visitedIndices.Contains(_currentTargetIndex))
                {
                    _visitedIndices.Add(_currentTargetIndex);
                }

                if (_visitedIndices.Count >= _waypoints.Count)
                {
                    _reachedLastPoint = true;
                    return new Steering();
                }

                FindNextTarget();
            }

            UpdateTargetPosition();
            return base.GetSteering();
        }

        private void FindNextTarget()
        {
            var shortestDistance = float.MaxValue;
            var bestTargetIndex = -1;

            for (var i = 0; i < _waypoints.Count; i++)
            {
                if (_visitedIndices.Contains(i))
                    continue;

                var distance = Vector3.Distance(transform.position, _waypoints[i]);

                if (!(distance < shortestDistance)) continue;
                
                shortestDistance = distance;
                bestTargetIndex = i;
            }

            if (bestTargetIndex != -1)
            {
                _currentTargetIndex = bestTargetIndex;
            }
            else
            {
                _reachedLastPoint = true;
            }
        }

        private void UpdateTargetPosition()
        {
            if (target && _currentTargetIndex >= 0 && _currentTargetIndex < _waypoints.Count)
            {
                target.transform.position = _waypoints[_currentTargetIndex];
            }
        }

        public void ResetPathFollowing()
        {
            _visitedIndices.Clear();
            FindClosestStartPoint();
            _reachedLastPoint = false;
        }

        private void OnDrawGizmosSelected()
        {
            if (_waypoints == null) return;

            for (var i = 0; i < _waypoints.Count; i++)
            {
                if (_reachedLastPoint)
                {
                    Gizmos.color = Color.gray;
                }
                else if (i == _currentTargetIndex)
                {
                    Gizmos.color = Color.green;
                }
                else if (_visitedIndices != null && _visitedIndices.Contains(i))
                {
                    Gizmos.color = Color.blue;
                }
                else
                {
                    Gizmos.color = Color.yellow;
                }

                Gizmos.DrawSphere(_waypoints[i], 0.3f);

#if UNITY_EDITOR
                UnityEditor.Handles.Label(_waypoints[i] + Vector3.up * 0.5f, i.ToString());
                #endif
            }

            if (!_reachedLastPoint && _visitedIndices != null)
            {
                Gizmos.color = Color.cyan;
                for (var i = 0; i < _waypoints.Count; i++)
                {
                    if (!_visitedIndices.Contains(i) && i != _currentTargetIndex)
                    {
                        Gizmos.DrawLine(transform.position, _waypoints[i]);
                    }
                }
            }
        }
    }
}