using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Entities;
using Unity.Jobs;
using Unity.Mathematics;
using Unity.Physics;
using Unity.Transforms;
using BoxCollider = Unity.Physics.BoxCollider;
using CapsuleCollider = Unity.Physics.CapsuleCollider;
using Collider = Unity.Physics.Collider;
using SphereCollider = Unity.Physics.SphereCollider;

namespace GarmentButton.VerletIntegration.Interaction
{
    [BurstCompile]
    public partial struct CollectColliderSphere : IJobFor
    {
        [ReadOnly] public ComponentLookup<PhysicsVelocity> PhysicsVelocityLookup;
        [ReadOnly] public ComponentLookup<LocalToWorld> LocalToWorldLookup;
        [ReadOnly] public NativeArray<ColliderCollection> Colliders;
        [ReadOnly] public float DeltaTime;
        public NativeArray<SphereInteraction> Collection;

        public void Execute(int index)
        {
            var entity = Colliders[index].Entity;

            SphereCollider collider;
            unsafe
            {
                collider = UnsafeUtility.AsRef<SphereCollider>(Colliders[index].Collider);
            }


            var localToWorld = LocalToWorldLookup.GetRefRO(entity).ValueRO.Value;
            if (!float4x4.identity.Equals(Colliders[index].Transform))
                localToWorld = math.mul(localToWorld, Colliders[index].Transform);

            var velocity = float3.zero;
            if (PhysicsVelocityLookup.TryGetRefRO(entity, out var physicsVelocity))
                velocity = physicsVelocity.ValueRO.Linear * (DeltaTime * 1);


            var collusion = ExtractSphereInteraction(collider, localToWorld, velocity);

            Collection[index] = collusion;
        }

        private static SphereInteraction ExtractSphereInteraction(SphereCollider collider, float4x4 localToWorld,
            float3 velocity)
        {
            var radius = collider.Radius;
            var worldCenter = math.transform(localToWorld, collider.Center);
            var lenght = math.length(velocity);
            var howManyTime = lenght / (radius / 2);
            var howManyTimeInside = (int)math.floor(howManyTime);
            if (howManyTimeInside == 0)
                howManyTimeInside = 1;
            var direction = math.normalizesafe(velocity) * radius;

            float3 sx = new float3(math.length(localToWorld.c0.xyz), math.length(localToWorld.c1.xyz),
                math.length(localToWorld.c2.xyz));
            float maxScale = math.cmax(sx);


            var collusion = new SphereInteraction()
            {
                Radius = radius * maxScale,
                Center = worldCenter,
                Velocity = direction,
                HowMany = howManyTimeInside,
            };
            return collusion;
        }
    }

    [BurstCompile]
    public partial struct CollectColliderCapsule : IJobFor
    {
        [ReadOnly] public ComponentLookup<PhysicsVelocity> PhysicsVelocityLookup;
        [ReadOnly] public ComponentLookup<LocalToWorld> LocalToWorldLookup;
        [ReadOnly] public NativeArray<ColliderCollection> Colliders;

        [ReadOnly] public float DeltaTime;

        //public CommandBuilder Builder;
        public NativeArray<CapsuleInteraction> Collection;

        public void Execute(int index)
        {
            var entity = Colliders[index].Entity;
            
            CapsuleCollider collider;
            unsafe
            {
                var capsule = (CapsuleCollider*)Colliders[index].Collider;
                collider =  UnsafeUtility.AsRef<CapsuleCollider>(capsule);
            }
            var localToWorld = LocalToWorldLookup.GetRefRO(entity).ValueRO.Value;

            if (!float4x4.identity.Equals(Colliders[index].Transform))
                localToWorld = math.mul(localToWorld, Colliders[index].Transform);


            var velocity = float3.zero;
            if (PhysicsVelocityLookup.TryGetRefRO(entity, out var physicsVelocity))
                velocity = physicsVelocity.ValueRO.Linear * (DeltaTime * 1);


            var interaction = ExtractCapsuleInteraction(collider, localToWorld, velocity);
            Collection[index] = interaction;
        }

        private static CapsuleInteraction ExtractCapsuleInteraction(CapsuleCollider collider, float4x4 localToWorld,
            float3 velocity)
        {
            float capsuleRadius = collider.Radius;
            var capsulePointA = math.transform(localToWorld, collider.Vertex0);
            var capsulePointB = math.transform(localToWorld, collider.Vertex1);
            var lenght = math.length(velocity);
            var howManyTime = lenght / (capsuleRadius / 2 / 2);
            var howManyTimeInside = (int)math.floor(howManyTime);
            if (howManyTimeInside == 0)
                howManyTimeInside = 1;
            var direction = math.normalizesafe(velocity) * capsuleRadius;

            var interaction = new CapsuleInteraction()
            {
                PositionA = capsulePointA,
                PositionB = capsulePointB,
                Velocity = direction,
                HowMany = howManyTimeInside,
                Radius = capsuleRadius,
            };
            return interaction;
        }
    }

    [BurstCompile]
    public partial struct CollectColliderBox : IJobFor
    {
        [ReadOnly] public ComponentLookup<PhysicsVelocity> PhysicsVelocityLookup;
        [ReadOnly] public ComponentLookup<LocalToWorld> LocalToWorldLookup;
        [ReadOnly] public NativeArray<ColliderCollection> Colliders;

        [ReadOnly] public float DeltaTime;

        public NativeArray<BoxInteraction> Collection;

        public void Execute(int index)
        {
            var entity = Colliders[index].Entity;
            BoxCollider collider;
            unsafe
            {
                collider = UnsafeUtility.AsRef<BoxCollider>(Colliders[index].Collider);
            }
            var localToWorld = LocalToWorldLookup.GetRefRO(entity).ValueRO.Value;
            if (!float4x4.identity.Equals(Colliders[index].Transform))
                localToWorld = math.mul(localToWorld, Colliders[index].Transform);

            var velocity = float3.zero;
            if (PhysicsVelocityLookup.TryGetRefRO(entity, out var physicsVelocity))
                velocity = physicsVelocity.ValueRO.Linear * (DeltaTime * 1);

            var interaction = ExtractBoxInteraction(collider, localToWorld, velocity);
            Collection[index] = interaction;
        }

        private static BoxInteraction ExtractBoxInteraction(BoxCollider collider, float4x4 localToWorld,
            float3 velocity)
        {
            var center = collider.Center;
            var halfExtent = collider.Size;
            var rotation = collider.Orientation;


            float3 sx = new float3(math.length(localToWorld.c0.xyz), math.length(localToWorld.c1.xyz),
                math.length(localToWorld.c2.xyz));
            halfExtent *= sx;
            center = math.transform(localToWorld, center);
            rotation = math.mul(localToWorld.Rotation(), rotation);

            var lenght = math.length(velocity);
            var size = math.length(halfExtent);
            var howManyTime = lenght / (size / 2);
            var howManyTimeInside = (int)math.floor(howManyTime);
            if (howManyTimeInside == 0)
                howManyTimeInside = 1;
            var direction = math.normalizesafe(velocity) * halfExtent;

            var interaction = new BoxInteraction()
            {
                Center = center,
                HalfSize = (halfExtent / 2) * 1.05f,
                Rotation = rotation,
                Velocity = direction,
                HowMany = howManyTimeInside,
            };
            return interaction;
        }
    }

    public unsafe struct ColliderCollection
    {
        public Collider* Collider;
        public float4x4 Transform;
        public Entity Entity;
    }
}