using System.Runtime.CompilerServices;
using Cysharp.Threading.Tasks;
using Unity.Burst;
using Unity.Mathematics;
using UnityEngine;

namespace DanieleMarotta.RiversongCodeShowcase
{
    [BurstCompile]
    public class LookAtIntentExecutionLogic : IntentExecutionLogic
    {
        private ITileSpace _tileSpace;

        private float _rotationSpeed;

        public override UniTask InitializeAsync(IServiceLocator serviceLocator)
        {
            _tileSpace = serviceLocator.GetService<ITileSpace>();
            _rotationSpeed = serviceLocator.GetService<GameConfig>().Agents.RotationSpeed;

            return UniTask.CompletedTask;
        }

        public override IntentExecutionResult Execute(Agent agent, in AgentIntent intent)
        {
            var lookAt = math.normalizesafe((float3)_tileSpace.TileToWorld(intent.Rect.Center) - agent.Position);
            if (math.all(lookAt == 0)) return IntentExecutionResult.Success;

            var step = math.radians(_rotationSpeed) * Time.deltaTime;
            var heading = agent.Heading;
            LookAt(ref heading, lookAt, step);
            agent.Heading = heading;

            return math.dot(heading, lookAt) > 0.995f ? IntentExecutionResult.Success : IntentExecutionResult.InProgress;
        }

        [BurstCompile]
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void LookAt(ref float3 heading, in float3 lookAt, float maxRadiansDelta)
        {
            var currentRotation = quaternion.LookRotationSafe(heading, math.up());
            var targetRotation = quaternion.LookRotationSafe(lookAt, math.up());

            var dot = math.dot(currentRotation, targetRotation);
            if (dot < 0)
            {
                dot = -dot;
                targetRotation.value = -targetRotation.value;
            }
            dot = math.clamp(dot, -1, 1);

            var angle = math.acos(dot) * 2;
            if (angle <= maxRadiansDelta)
            {
                heading = lookAt;
                return;
            }

            currentRotation = math.slerp(currentRotation, targetRotation, maxRadiansDelta / angle);
            heading = math.mul(currentRotation, math.forward());
        }
    }
}