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

namespace GarmentButton.VerletIntegration
{
    [UpdateInGroup(typeof(VerletIntegrationGroupSystems))]
    public partial struct FollowEntitySystem : ISystem
    {
        [BurstCompile]
        public void OnCreate(ref SystemState state)
        {
            state.RequireForUpdate<Point>();
            state.RequireForUpdate<RelativeLock>();
            state.RequireForUpdate<LocalTransform>();
        }

        [BurstCompile]
        public void OnUpdate(ref SystemState state)
        {
            
            state.Dependency = new FollowEntityJob().Schedule(state.Dependency);
        }

        public partial struct FollowEntityJob : IJobEntity
        {
            public void Execute(ref DynamicBuffer<Point> pointBuffer, ref RecordLastTransform recordLastTransform,
                in DynamicBuffer<RelativeLock> relativeLock, in LocalToWorld transform)
            {
                float3 center = transform.Position;
                quaternion rot = math.normalize(transform.Rotation);

                if (SamePosition(recordLastTransform.Position, center) &&
                    SameRotation(recordLastTransform.Rotation, rot))
                    return;



                for (int i = 0; i < pointBuffer.Length; i++)
                {
                    if (!pointBuffer[i].IsLocked) continue;

                    float3 pointLocal = float3.zero;
                    for (int j = 0; j < relativeLock.Length; j++)
                    {
                        if (relativeLock[j].Index == i)
                        {
                            pointLocal = relativeLock[j].OffSet; // actually local position
                            break;
                        }
                    }


                    var p = pointBuffer[i];
                    p.Position = center + math.rotate(rot, pointLocal);
                    pointBuffer[i] = p;
                }

                recordLastTransform.Position = center;
                recordLastTransform.Rotation = rot;
            }

            private bool SameRotation(quaternion a, quaternion b, float tolerance = 1e-5f)
            {
                float d = math.abs(math.dot(a.value, b.value)); 
                return (1f - d) <= tolerance;
            }

            private bool SamePosition(float3 a, float3 b, float tolerance = 1e-3f)
            {
                float3 d = a - b;
                return math.lengthsq(d) <= tolerance * tolerance; 
            }
        }
    }
}