using System;
using System.IO;
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.Physics.Extensions;
using Unity.Transforms;
using Collider = Unity.Physics.Collider;


namespace GarmentButton.VerletIntegration.Interaction
{
    [UpdateInGroup(typeof(VerletIntegrationPhysicsGroupSystems))]
    [UpdateBefore(typeof(SetActiveStateInteractionFlagSystem))]
    [BurstCompile]
    public partial struct SimulateInteractionSystem : ISystem
    {
        [BurstCompile]
        public void OnCreate(ref SystemState state)
        {
            state.RequireForUpdate<Point>();
            state.RequireForUpdate<FlagInteractionEnableTag>();

            state.RequireForUpdate<InteractableWithClothTag>();
            state.RequireForUpdate<LocalTransform>();
        }

        //[BurstCompile]
        public void OnUpdate(ref SystemState state)
        {
            var physicsColliderLookup = SystemAPI.GetComponentLookup<PhysicsCollider>(true);
            var physicsVelocityLookup = SystemAPI.GetComponentLookup<PhysicsVelocity>(true);
            var transformLookUp = SystemAPI.GetComponentLookup<LocalToWorld>(true);
            var pointBuffer = SystemAPI.GetBufferLookup<Point>();

            foreach (var (currentInteractable, pointLenght, entity) in SystemAPI
                         .Query<
                             DynamicBuffer<CurrentInteractable>, RefRO<PointArrayLenght>>()
                         .WithAll<FlagInteractionEnableTag>().WithEntityAccess())
            {
                var interactionCount = currentInteractable.Length;
                if (interactionCount == 0)
                    continue;

                
                NativeParallelMultiHashMap<byte, ColliderCollection> colliders =
                    new NativeParallelMultiHashMap<byte, ColliderCollection>(3, Allocator.TempJob);

                for (int i = 0; i < interactionCount; i++)
                {
                    var collider = physicsColliderLookup.GetRefRO(currentInteractable[i].Entity);
                    unsafe
                    {
                        var colliderPointer = collider.ValueRO.Value.AsPtr();
                        var colliderEntity = currentInteractable[i].Entity;
                        ExtractCollider(colliderPointer, colliders, colliderEntity, float4x4.identity);
                    }
                }

                NativeArray<JobHandle> handles =
                    new NativeArray<JobHandle>(4, Allocator.TempJob);
                handles[0] = state.Dependency;

                var key = (byte)ColliderType.Capsule;
                var count = colliders.CountValuesForKey(key);
                NativeArray<CapsuleInteraction> capsuleCollection =
                    new NativeArray<CapsuleInteraction>(count, Allocator.TempJob);
                if (count > 0)
                {
                    var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
                    int i = 0;
                    foreach (var e in
                             colliders.GetValuesForKey(
                                 key)) 
                        entities[i++] = e;

                    handles[1] = new CollectColliderCapsule()
                    {
                        Collection = capsuleCollection,
                        Colliders = entities,
                        PhysicsVelocityLookup = physicsVelocityLookup,
                        LocalToWorldLookup = transformLookUp,
                        DeltaTime = SystemAPI.Time.fixedDeltaTime,
                    }.Schedule(count, state.Dependency);
                    entities.Dispose(handles[1]);
                }


                key = (byte)ColliderType.Sphere;
                count = colliders.CountValuesForKey(key);
                NativeArray<SphereInteraction> sphereCollection =
                    new NativeArray<SphereInteraction>(count, Allocator.TempJob);
                if (count > 0)
                {
                    var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
                    int i = 0;
                    foreach (var e in
                             colliders.GetValuesForKey(
                                 key)) 
                        entities[i++] = e;

                    handles[2] = new CollectColliderSphere()
                    {
                        Collection = sphereCollection,
                        Colliders = entities,
                        PhysicsVelocityLookup = physicsVelocityLookup,
                        LocalToWorldLookup = transformLookUp,
                        DeltaTime = SystemAPI.Time.fixedDeltaTime,
                    }.Schedule(count, state.Dependency);
                    entities.Dispose(handles[2]);
                }

                key = (byte)ColliderType.Box;
                count = colliders.CountValuesForKey(key);
                NativeArray<BoxInteraction> boxCollection =
                    new NativeArray<BoxInteraction>(count, Allocator.TempJob);
                if (count > 0)
                {
                    var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
                    int i = 0;
                    foreach (var e in
                             colliders.GetValuesForKey(
                                 key)) 
                        entities[i++] = e;

                    handles[3] = new CollectColliderBox()
                    {
                        Collection = boxCollection,
                        Colliders = entities,
                        PhysicsVelocityLookup = physicsVelocityLookup,
                        LocalToWorldLookup = transformLookUp,
                        DeltaTime = SystemAPI.Time.fixedDeltaTime,
                    }.Schedule(count, state.Dependency);
                    entities.Dispose(handles[3]);
                }

                JobHandle combined = JobHandle.CombineDependencies(handles);
                colliders.Dispose(combined);
                state.Dependency = combined;

                state.Dependency = new InteractJob()
                {
                    PointSphereRadius = 0.1f,
                    CapsuleCollection = capsuleCollection.AsReadOnly(),
                    SphereCollection = sphereCollection.AsReadOnly(),
                    BoxCollection = boxCollection.AsReadOnly(),
                    BufferLookup = pointBuffer,
                    Entity = entity
                }.ScheduleParallel(pointLenght.ValueRO.Value, 16, combined);

                capsuleCollection.Dispose(state.Dependency);
                sphereCollection.Dispose(state.Dependency);
                boxCollection.Dispose(state.Dependency);
                handles.Dispose(state.Dependency);
            }
        }

        private static unsafe void ExtractCollider(Collider* colliderPointer,
            NativeParallelMultiHashMap<byte, ColliderCollection> colliders,
            Entity colliderEntity, float4x4 relative)
        {
            var type = colliderPointer->Type;
            switch (type)
            {
                case ColliderType.Convex:
                    return;
                case ColliderType.Triangle:
                    return;
                case ColliderType.Quad:
                    return;
                case ColliderType.Cylinder:
                    return;
                case ColliderType.Mesh:
                    return;
                case ColliderType.Compound:

                    var compoundCollider = (CompoundCollider*)colliderPointer;


                    for (int i = 0; i < compoundCollider->Children.Length; i++)
                    {
                        ref var child = ref compoundCollider->Children[i];

                        var colider = child.Collider;

                        ExtractCollider(colider, colliders, colliderEntity, new float4x4(child.CompoundFromChild));
                    }

                    return;
                case ColliderType.Terrain:
                    return;
            }


            if (colliderPointer->GetCollisionResponse() == CollisionResponsePolicy.RaiseTriggerEvents)
                return;


            colliders.Add((byte)type, new ColliderCollection()
            {
                Collider = colliderPointer,
                Entity = colliderEntity,
                Transform = relative
            });
        }
    }


    [BurstCompile]
    public partial struct InteractJob : IJobFor
    {
        [ReadOnly] public float PointSphereRadius;
        [NativeDisableParallelForRestriction] public BufferLookup<Point> BufferLookup;
        [ReadOnly] public Entity Entity;

        [ReadOnly] public NativeArray<CapsuleInteraction>.ReadOnly CapsuleCollection;
        [ReadOnly] public NativeArray<SphereInteraction>.ReadOnly SphereCollection;
        [ReadOnly] public NativeArray<BoxInteraction>.ReadOnly BoxCollection;

        [BurstCompile]
        public void Execute(int index)
        {
            BufferLookup.TryGetBuffer(Entity, out var points);

            var point = points[index];

            if (point.State != PointState.Free)
                return;

            var shouldItUnlock = false;
            for (int i = 0; i < CapsuleCollection.Length; i++)
            {
                var capsuleData = CapsuleCollection[i];
                var positonA = capsuleData.PositionA;
                var positonB = capsuleData.PositionB;

                for (int j = 0; j < capsuleData.HowMany; j++)
                {
                    var hasInteracted = InteractedCapsule(ref point, positonA, positonB, capsuleData.Radius);
                    if (!hasInteracted)
                        shouldItUnlock = true;

                    positonA += capsuleData.Velocity;
                    positonB += capsuleData.Velocity;
                }
            }

            for (int i = 0; i < SphereCollection.Length; i++)
            {
                var sphereInteraction = SphereCollection[i];
                var center = sphereInteraction.Center;
                for (int j = 0; j < sphereInteraction.HowMany; j++)
                {
                    var hasInteracted = InteractedSphere(ref point, center, sphereInteraction.Radius);
                    if (!hasInteracted)
                        shouldItUnlock = true;

                    center += sphereInteraction.Velocity;
                }
            }

            for (int i = 0; i < BoxCollection.Length; i++)
            {
                var boxInteraction = BoxCollection[i];
                var center = boxInteraction.Center;
                for (int j = 0; j < boxInteraction.HowMany; j++)
                {
                    var hasInteracted = InteractedBoxOBB(ref point, center, boxInteraction.HalfSize,
                        boxInteraction.Rotation);
                    if (!hasInteracted)
                        shouldItUnlock = true;

                    center += boxInteraction.Velocity;
                }
            }


            if (shouldItUnlock)
                point.IsLocked = false;
            points[index] = point;
        }


        bool InteractedCapsule(ref Point point, float3 predicatedPosition, float3 highestPointInCapsule,
            float capsuleRadius)
        {


            float3 closestPoint =
                ClosestPointOnCapsuleAxis(predicatedPosition, highestPointInCapsule, point.Position);

            // Calculate the distance between the closest point and the sphere's center
            float distance = math.distance(closestPoint, point.Position);

            var shouldInteract = distance <= PointSphereRadius + capsuleRadius;

            if (shouldInteract)
            {
                SetNewPosition(ref point, closestPoint, distance, capsuleRadius);
                return true;
            }

            return false;
        }


        private void SetNewPosition(ref Point point, float3 closestPoint, float distance, float capsuleRadius)
        {
            // Calculate the direction to move the sphere to resolve the intersection
            float3 displacementDirection = math.normalizesafe(point.Position - closestPoint, new float3(1, 0, 0));

            float displacementDistance = PointSphereRadius + capsuleRadius - distance;
            var howMuchToMove = displacementDirection * displacementDistance;

            //point.IsLocked = true;
            point.PrevesPosition = point.Position;
            point.Position += howMuchToMove;
        }


        private float3 ClosestPointOnCapsuleAxis(float3 a, float3 b, float3 p)
        {
            float3 ab = b - a;
            float abLenSq = math.lengthsq(ab);
            if (abLenSq < 1e-8f) return a;
            float t = math.clamp(math.dot(p - a, ab) / abLenSq, 0f, 1f);
            return a + t * ab;
        }


        bool InteractedSphere(ref Point point, float3 sphereCenter, float sphereRadius)
        {
            // Closest point on the sphere surface to the particle center
            float3 closestPoint = sphereCenter;

            // Distance from particle center to that closest point
            float distance = math.distance(closestPoint, point.Position); // :contentReference[oaicite:1]{index=1}

            //bool shouldInteract = distance <= PointSphereRadius + sphereRadius;
            bool shouldInteract = distance <= PointSphereRadius + sphereRadius;

            if (shouldInteract)
            {
                SetNewPosition(ref point, closestPoint, distance, sphereRadius);
                return true;
            }

            return false;
        }


        bool InteractedBoxOBB(ref Point point, float3 boxCenter, float3 boxHalfExtents, quaternion boxRotation)
        {
            // Transform point into box local space (OBB -> AABB in local frame)
            float3 pWorld = point.Position;
            float3 pLocal = math.mul(math.inverse(boxRotation), (pWorld - boxCenter));

            // Run the AABB logic in local space
            float3 min = -boxHalfExtents;
            float3 max = boxHalfExtents;

            float3 closestLocal = math.clamp(pLocal, min, max);
            float3 deltaLocal = pLocal - closestLocal;
            float distSq = math.lengthsq(deltaLocal);
            bool inside = distSq < 1e-12f;

            if (!inside)
            {
                float dist = math.sqrt(distSq);
                bool shouldInteract = dist <= PointSphereRadius;
                if (!shouldInteract) return false;

                float3 closestWorld = boxCenter + math.mul(boxRotation, closestLocal);
                ResolveSphereVsBox(ref point, closestWorld, dist, inside: false);
                return true;
            }
            else
            {
                // Inside: nearest face in local space
                float3 absLocal = math.abs(pLocal);
                float3 toFace = boxHalfExtents - absLocal;

                int axis = 0;
                float minToFace = toFace.x;
                if (toFace.y < minToFace)
                {
                    minToFace = toFace.y;
                    axis = 1;
                }

                if (toFace.z < minToFace)
                {
                    minToFace = toFace.z;
                    axis = 2;
                }

                float3 faceLocal = pLocal;
                float sign = (axis == 0 ? (pLocal.x >= 0f ? 1f : -1f)
                    : axis == 1 ? (pLocal.y >= 0f ? 1f : -1f)
                    : (pLocal.z >= 0f ? 1f : -1f));

                if (axis == 0) faceLocal.x = sign * boxHalfExtents.x;
                else if (axis == 1) faceLocal.y = sign * boxHalfExtents.y;
                else faceLocal.z = sign * boxHalfExtents.z;

                float distToFace = math.distance(pLocal, faceLocal);

                float3 faceWorld = boxCenter + math.mul(boxRotation, faceLocal);
                ResolveSphereVsBox(ref point, faceWorld, distToFace, inside: true);
                return true;
            }
        }

        private void ResolveSphereVsBox(ref Point point, float3 surfacePoint, float distanceToSurface, bool inside)
        {
            // If outside: move away from surface by (r - distance)
            // If inside : move outward past surface by (r + distance)
            float3 p = point.Position;

            float3 dir;
            float moveDist;

            if (!inside)
            {
                dir = math.normalizesafe(p - surfacePoint, new float3(1, 0, 0));
                moveDist = PointSphereRadius - distanceToSurface;
            }
            else
            {
                // outward direction is from point -> surface (then continue outward)
                dir = math.normalizesafe(surfacePoint - p, new float3(1, 0, 0));
                moveDist = PointSphereRadius + distanceToSurface;
            }

            float3 move = dir * moveDist;

            point.PrevesPosition = point.Position;
            point.Position += move;
        }
    }
}