ID/Assets/Code/Lab4/BetterBlendPathFollower.cs

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

namespace Code.Lab4
{
    public class BetterBlendPathFollower : Seek
    {
        public Path path;
        public float pathOffset = 1.0f;
        
        [Header("Targets")]
        public GameObject pursuer;

        [Header("Weights")]
        [Range(0, 1)]
        public float pathFollowingWeight = 0.5f;
        [Range(0, 1)]
        public float optimalPathWeight = 0.3f;
        [Range(0, 1)]
        public float evadeWeight = 0.2f;

        [Header("Evade Settings")]
        public float maxPrediction = 2f;

        public float evadeDistance = 5f;
        public float minEvadeWeight = 0.1f;

        private float _currentParam;
        private List<Vector3> _waypoints;
        private int _currentTargetIndex = -1;
        private List<int> _visitedIndices;
        private bool _reachedLastPoint;
        
        private GameObject _evadeTarget;
        private Agent _pursuerAgent;
        private Agent _selfAgent;
        
        public override void Awake()
        {
            base.Awake();
            target = new GameObject("BetterBlendPathFollower_Target");
            _evadeTarget = new GameObject("Evade_Target");
            _visitedIndices = new List<int>();
            
            _selfAgent = GetComponent<Agent>();
            
            InitializeWaypoints();
        }
        
        private void Start()
        {
            if (pursuer != null)
            {
                _pursuerAgent = pursuer.GetComponent<Agent>();
            }
        }
        
        private void InitializeWaypoints()
        {
            if (path == null || path.nodes == null || path.nodes.Count == 0) return;
            
            _waypoints = new List<Vector3>();
            
            foreach (var node in path.nodes.Where(node => node != null))
            {
                _waypoints.Add(node.transform.position);
            }
            
            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)
                {
                    minDistance = distance;
                    closestIndex = i;
                }
            }
            
            _currentTargetIndex = closestIndex;
            _currentParam = 0;
            _visitedIndices.Clear();
            _reachedLastPoint = false;
        }
        
        private void OnDestroy()
        {
            if (_evadeTarget != null)
                Destroy(_evadeTarget);
        }
        
        public override Steering GetSteering()
        {
            if (_waypoints == null || _waypoints.Count == 0 || _reachedLastPoint)
            {
                return new Steering();
            }

            var pathSteering = GetPathFollowingSteering();
            var optimalSteering = GetOptimalPathSteering();
            var evadeSteering = GetEvadeSteering();

            var dynamicEvadeWeight = CalculateDynamicEvadeWeight();

            var totalWeight = pathFollowingWeight + optimalPathWeight + dynamicEvadeWeight;
            var normPathWeight = pathFollowingWeight / totalWeight;
            var normOptimalWeight = optimalPathWeight / totalWeight;
            var normEvadeWeight = dynamicEvadeWeight / totalWeight;

            var blendedSteering = new Steering
            {
                Linear = normPathWeight * pathSteering.Linear + 
                        normOptimalWeight * optimalSteering.Linear + 
                        normEvadeWeight * evadeSteering.Linear,
                Angular = normPathWeight * pathSteering.Angular + 
                         normOptimalWeight * optimalSteering.Angular + 
                         normEvadeWeight * evadeSteering.Angular
            };

            if (blendedSteering.Linear.magnitude > Agent.maxAcceleration)
            {
                blendedSteering.Linear = blendedSteering.Linear.normalized * Agent.maxAcceleration;
            }
            
            return blendedSteering;
        }
        
        private float CalculateDynamicEvadeWeight()
        {
            if (pursuer == null) return 0f;
            
            var distanceToPursuer = Vector3.Distance(transform.position, pursuer.transform.position);
            
            if (distanceToPursuer <= evadeDistance)
            {
                var t = 1f - distanceToPursuer / evadeDistance;
                return Mathf.Lerp(minEvadeWeight, evadeWeight, t);
            }
            
            return minEvadeWeight;
        }
        
        private Steering GetPathFollowingSteering()
        {
            if (!path) return new Steering();
            
            _currentParam = path.GetParam(transform.position, _currentParam);
            var targetParam = _currentParam + pathOffset;
            target.transform.position = path.GetPosition(targetParam);
            
            return base.GetSteering();
        }
        
        private Steering GetOptimalPathSteering()
        {
            if (_waypoints == null || _waypoints.Count == 0) 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();
            }

            var tempTarget = target.transform.position;
            target.transform.position = _waypoints[_currentTargetIndex];
            var steering = base.GetSteering();
            target.transform.position = tempTarget;

            return steering;
        }
        
        private Steering GetEvadeSteering()
        {
            if (pursuer == null || _pursuerAgent == null) return new Steering();

            var direction = pursuer.transform.position - transform.position;
            var distance = direction.magnitude;
            var speed = _selfAgent.velocity.magnitude;
            
            float prediction;
            if (speed <= distance / maxPrediction)
                prediction = maxPrediction;
            else
                prediction = distance / speed;

            _evadeTarget.transform.position = pursuer.transform.position;
            _evadeTarget.transform.position += _pursuerAgent.velocity * prediction;

            var directionToEvade = _evadeTarget.transform.position - transform.position;
            var fleeSteering = new Steering
            {
                Linear = -directionToEvade.normalized * Agent.maxAcceleration,
                Angular = 0
            };
            
            return fleeSteering;
        }
        
        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)
                {
                    shortestDistance = distance;
                    bestTargetIndex = i;
                }
            }
            
            if (bestTargetIndex != -1)
            {
                _currentTargetIndex = bestTargetIndex;
            }
            else
            {
                _reachedLastPoint = true;
            }
        }
        
        public void ResetPathFollowing()
        {
            _visitedIndices.Clear();
            FindClosestStartPoint();
            _reachedLastPoint = false;
        }
        
        public void SetWeights(float pathWeight, float optimalWeight, float evadeWeightValue)
        {
            pathFollowingWeight = Mathf.Clamp01(pathWeight);
            optimalPathWeight = Mathf.Clamp01(optimalWeight);
            evadeWeight = Mathf.Clamp01(evadeWeightValue);

            var total = pathFollowingWeight + optimalPathWeight + evadeWeight;
            if (total > 1f)
            {
                pathFollowingWeight /= total;
                optimalPathWeight /= total;
                evadeWeight /= total;
            }
        }
        
        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 (path != null)
            {
                Gizmos.color = Color.magenta;
                var segments = path.GetSegments();
                foreach (var segment in segments)
                {
                    Gizmos.DrawLine(segment.A, segment.B);
                }
            }

            if (pursuer != null)
            {
                Gizmos.color = Color.red;
                Gizmos.DrawWireSphere(transform.position, evadeDistance);

                Gizmos.color = Color.yellow;
                Gizmos.DrawLine(transform.position, pursuer.transform.position);
            }
        }
    }
}