using Unity.Burst;
using Unity.Collections;
using Unity.Entities;
using Unity.Mathematics;
using Unity.Physics;
using Unity.Transforms;

namespace GarmentButton.PathFinding
{
    [BurstCompile]
    [UpdateAfter(typeof(CalculatePathSystem))]
    [UpdateAfter(typeof(UpdatePathBufferBasePosition))]
    public partial struct PathRigidbodyMovementSystem : ISystem
    {
        [BurstCompile]
        public void OnCreate(ref SystemState state)
        {
            var query = SystemAPI.QueryBuilder()
                .WithAll<PathRigidbodyMovement, PhysicsVelocity, LocalTransform, PathElement>()
                .Build();

            state.RequireForUpdate(query);
            state.RequireForUpdate<Grid>();
        }

        [BurstCompile]
        public void OnUpdate(ref SystemState state)
        {
            var grid = SystemAPI.GetSingleton<Grid>().Value;

            state.Dependency = new MoveAlongPathJob
            {
                Grid = grid,
            }.ScheduleParallel(state.Dependency);
        }

        [BurstCompile]
        public partial struct MoveAlongPathJob : IJobEntity
        {
            [ReadOnly] public BlobAssetReference<PointPathBlobAsset> Grid;

            public void Execute(
                ref PhysicsVelocity velocity,
                in PathRigidbodyMovement movement,
                in LocalTransform transform,
                in DynamicBuffer<PathElement> path)
            {
                var linearVelocity = velocity.Linear;

                if (path.Length == 0 || movement.MoveSpeed <= 0f)
                {
                    linearVelocity.x = 0f;
                    linearVelocity.z = 0f;
                    velocity.Linear = linearVelocity;
                    return;
                }

                var targetPoint = Grid.Value.Array[path[0].IndexGrid];
                var targetPosition = new float3(targetPoint.X, targetPoint.Y, targetPoint.Z) / 10f;
                var planarOffset = (targetPosition - transform.Position).xz;
                var stopDistance = math.max(0.01f, movement.StopDistance);

                if (math.lengthsq(planarOffset) <= stopDistance * stopDistance)
                {
                    linearVelocity.x = 0f;
                    linearVelocity.z = 0f;
                    velocity.Linear = linearVelocity;
                    return;
                }

                var direction = math.normalize(planarOffset);
                linearVelocity.x = direction.x * movement.MoveSpeed;
                linearVelocity.z = direction.y * movement.MoveSpeed;
                velocity.Linear = linearVelocity;
            }
        }
    }
}