using Code.Base;
using UnityEngine;

namespace Code.Lab2
{
    public class ArriveAvoid : AgentBehaviour
    {
        public float targetRadius = 5f;
        public float slowRadius = 10f;
        public float timeToTarget = 0.1f;

        public Transform obstacle;
        public float avoidanceRadius = 10f;
        public float maxAvoidForce = 50f;

        public override Steering GetSteering()
        {
            var steering = new Steering();

            var toTarget = target.transform.position - transform.position;
            var distanceToTarget = toTarget.magnitude;

            if (distanceToTarget < targetRadius)
                return steering;

            var targetSpeed = distanceToTarget > slowRadius
                ? Agent.maxSpeed
                : Agent.maxSpeed * (distanceToTarget / slowRadius);

            var desiredVelocity = toTarget.normalized * targetSpeed;
            var arriveForce = (desiredVelocity - Agent.velocity) / timeToTarget;

            var avoidForce = Vector3.zero;

            if (obstacle)
            {
                var toObstacle = obstacle.position - transform.position;
                var distToObstacle = toObstacle.magnitude;

                if (distToObstacle < avoidanceRadius)
                {
                    var forward = Agent.velocity.normalized;
                    if (forward == Vector3.zero) forward = toTarget.normalized;

                    var awayFromObstacle = -toObstacle;

                    var lateral = awayFromObstacle - Vector3.Project(awayFromObstacle, forward);

                    if (lateral.magnitude < 0.1f)
                    {
                        lateral = new Vector3(-forward.z, 0f, forward.x).normalized;
                    }

                    var avoidanceStrength = 1f - (distToObstacle / avoidanceRadius);
                    avoidForce = lateral.normalized * (maxAvoidForce * avoidanceStrength);
                }
            }

            steering.Linear = arriveForce + avoidForce;

            if (steering.Linear.magnitude > Agent.maxAcceleration)
            {
                steering.Linear = steering.Linear.normalized * Agent.maxAcceleration;
            }

            return steering;
        }
    }
}