using Unity.Burst;
using Unity.Mathematics;

namespace DanieleMarotta.RiversongCodeShowcase
{
    [BurstCompile]
    [Service(typeof(IAgentCommonLogic))]
    [GameSystemGroup(typeof(AgentsSystemGroup))]
    public class AgentCommonLogicSystem : GameSystem, IAgentCommonLogic
    {
        private const float RotationSpeed = 270 * math.TORADIANS;

        [InjectService]
        private GameConfig _config;

        [InjectService]
        private ITileSpace _tileSpace;

        [InjectService]
        private IBuildingSpatialQuery _buildingSpatialQuery;

        [InjectService]
        private IAgentFactory _agentFactory;

        public AgentCommonLogicSystem(IServiceLocator serviceLocator) : base(serviceLocator)
        {
        }

        public bool MoveAgent(Agent agent, float3 targetPosition, float dt)
        {
            var position = agent.Position;
            var heading = agent.Heading;

            var oldPosition = position;
            var targetReached = MoveAgent(targetPosition, dt, _config.Agents.MoveSpeed, ref position, ref heading);

            agent.Position = position;
            agent.Heading = heading;
            agent.Velocity = dt > 0 ? (position - oldPosition) / dt : 0;

            return targetReached;
        }

        [BurstCompile]
        private static bool MoveAgent(in float3 targetPosition, float dt, float speed, ref float3 position, ref float3 heading)
        {
            var positionDelta = targetPosition - position;
            var targetHeading = math.normalizesafe(positionDelta, heading);
            var movementLength = speed * dt;

            const float targetOvershoot = 1.1f;
            var targetReachedThreshold = movementLength * targetOvershoot;
            var snapPosition = math.lengthsq(positionDelta) < targetReachedThreshold * targetReachedThreshold;
            position = math.select(position + targetHeading * movementLength, targetPosition, snapPosition);

            var snapHeading = UpdateHeading(ref heading, targetHeading, RotationSpeed * dt);
            heading = math.select(heading, targetHeading, snapHeading);

            return snapPosition && snapHeading;
        }

        public bool UpdateHeading(Agent agent, in float3 targetHeading, float dt)
        {
            var heading = agent.Heading;
            var headingReached = UpdateHeading(ref heading, targetHeading, RotationSpeed * dt);
            agent.Heading = heading;
            return headingReached;
        }

        [BurstCompile]
        private static bool UpdateHeading(ref float3 heading, in float3 targetHeading, float maxRadiansDelta)
        {
            var q0 = quaternion.LookRotationSafe(heading, math.up());
            var q1 = quaternion.LookRotationSafe(targetHeading, math.up());

            var angle = math.angle(q0, q1);
            if (angle < maxRadiansDelta)
            {
                heading = targetHeading;
                return true;
            }

            var t = math.min(1, maxRadiansDelta / angle);
            var q = math.slerp(q0, q1, t);

            heading = math.rotate(q, math.forward());
            return false;
        }

        public bool TryFetchProductFromStorage(AgentDefinition agentDefinition,
            Building source,
            int productHandle,
            int neededAmount,
            int maxAgentCount = int.MaxValue,
            AgentJob job = AgentJob.None)
        {
            ref var sourceStorage = ref source.GetStorageRW();
            return sourceStorage.CountIncludingReservations(productHandle) < neededAmount &&
                   TryFindFetchStorage(source, productHandle, out var fetchBuilding) &&
                   TryFetchProduct(agentDefinition, source, fetchBuilding, productHandle, 1, maxAgentCount, job);
        }

        public bool TryFetchProductFromProvider(AgentDefinition agentDefinition,
            Building source,
            int productHandle,
            int neededAmount,
            int maxAgentCount = int.MaxValue,
            AgentJob job = AgentJob.None)
        {
            ref var sourceStorage = ref source.GetStorageRW();
            return sourceStorage.CountIncludingReservations(productHandle) < neededAmount &&
                   _buildingSpatialQuery.FindProviderForFetch(source.Id, source.Rect, productHandle, out var fetchBuilding) &&
                   TryFetchProduct(agentDefinition, source, fetchBuilding, productHandle, 1, maxAgentCount, job);
        }

        public bool TryFetchProduct(AgentDefinition agentDefinition,
            Building source,
            Building fetchBuilding,
            int productHandle,
            int amount,
            int maxAgentCount = int.MaxValue,
            AgentJob job = AgentJob.None)
        {
            if (!_agentFactory.CanSpawnAgent(source, maxAgentCount, job)) return false;

            ref var sourceStorage = ref source.GetStorageRW();
            if (sourceStorage.FreeSpace() < amount) return false;

            ref var fetchStorage = ref fetchBuilding.GetStorageRW();
            if (fetchStorage.AvailableNow(productHandle) < amount) return false;

            sourceStorage.ReservePutting(productHandle, amount);
            fetchStorage.ReserveTaking(productHandle, amount);

            var position = _tileSpace.TileToWorld(source.Rect.Center);
            var agent = _agentFactory.CreateVillager(agentDefinition, source, position, job);
            IntentQueue.Fetch(ref agent.GetIntentQueueRW(), source.Rect, source.Id, fetchBuilding.Rect, fetchBuilding.Id, productHandle, amount, PathTraversalRules.TransportAgent);

            return true;
        }

        public bool TryDeliverProduct(AgentDefinition agentDefinition, Building source, int productHandle, int maxAgentCount = int.MaxValue, AgentJob job = AgentJob.None)
        {
            if (!_agentFactory.CanSpawnAgent(source, maxAgentCount, job)) return false;

            ref var sourceStorage = ref source.GetStorageRW();
            if (sourceStorage.AvailableNow(productHandle) <= 0) return false;

            if (!_buildingSpatialQuery.FindStorageForDelivery(source.Id, source.Rect, productHandle, out var deliveryStorage)) return false;

            sourceStorage.ReserveTaking(productHandle, 1);

            ref var destinationStorage = ref deliveryStorage.GetStorageRW();
            destinationStorage.ReservePutting(productHandle, 1);

            var position = _tileSpace.TileToWorld(source.Rect.Center);
            var deliveryAgent = _agentFactory.CreateVillager(agentDefinition, source, position, job);
            IntentQueue.Deliver(
                ref deliveryAgent.GetIntentQueueRW(),
                source.Rect,
                source.Id,
                deliveryStorage.Rect,
                deliveryStorage.Id,
                productHandle,
                1,
                PathTraversalRules.TransportAgent);

            return true;
        }

        private bool TryFindFetchStorage(Building source, int productHandle, out Building fetchBuilding)
        {
            if (source.Definition.ProvidedProducts.Count > 0)
                return _buildingSpatialQuery.FindStorageForFetch(source.Id, source.Rect, source.Definition.Range, productHandle, out fetchBuilding);

            return _buildingSpatialQuery.FindStorageForFetch(source.Id, source.Rect, productHandle, out fetchBuilding);
        }
    }
}