using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Entities;
using Unity.Jobs;
using Unity.Mathematics;
using UnityEngine;
using Unity.Transforms;

namespace GarmentButton.VerletIntegration
{
    [UpdateInGroup(typeof(VerletIntegrationPhysicsGroupSystems))]
    public partial struct SimulateSystem : ISystem
    {

        public void OnCreate(ref SystemState state)
        {
            state.RequireForUpdate<FlagData>();
        }

        [BurstCompile]
        public void OnUpdate(ref SystemState state)
        {

            foreach (var (flag, pointBuffer, sticksBuffer,boundBox,transform) in SystemAPI.Query<RefRW<FlagData>, DynamicBuffer<Point>, DynamicBuffer<Stick>, RefRO<BoundBox>,RefRO<LocalToWorld>>())
            {

                #region Debug
#if false
                DebugPoints(pointBuffer);
                foreach (var stick in stickesBuffer)
                {
                    UnityEngine.Debug.DrawLine(pointBuffer[stick.PointA].Position, pointBuffer[stick.PointB].Position, stick.Active ?Color.blue : Color.red);
                }
#endif
                #endregion
                
                var sticksTemp = sticksBuffer.AsNativeArray().AsReadOnly();
                var pointArrayTemp = pointBuffer.AsNativeArray();
                state.Dependency = FirstJob(state.Dependency, ref state, pointArrayTemp, flag, boundBox,transform.ValueRO.Position);
                var numberIterations = flag.ValueRO.NumberIterations > 0 ? flag.ValueRO.NumberIterations : 1;

                for (var i = 1; i < numberIterations; i++)
                    state.Dependency = SecondJob(state.Dependency, ref state, pointArrayTemp, sticksTemp);
            }

        }
        
        
        private JobHandle FirstJob(JobHandle dependency, ref SystemState state, NativeArray<Point> pointBuffer, RefRW<FlagData> flag, RefRO<BoundBox> boundBox, float3 entityPosition)
        {
            var job = new SimulatePhysicsForEachPointJob
            {
                DeltaTime = Time.fixedUnscaledDeltaTime,
                Points = pointBuffer,
                Gravity = flag.ValueRO.GravityForce,
                Damping = flag.ValueRO.Damping,
                MinBoundBox = boundBox.ValueRO.Min + entityPosition,
                MaxBoundBox = boundBox.ValueRO.Max + entityPosition,
            };
            return job.ScheduleParallel(pointBuffer.Length, 16, dependency);
        }

        private JobHandle SecondJob(JobHandle dependency, ref SystemState state, NativeArray<Point> pointBuffer, NativeArray<Stick>.ReadOnly sticksBuffer)
        {
            var job2 = new SimulateConstrainsJob
            {
                Points = pointBuffer,
                Sticks = sticksBuffer,
                Thinness = 0.9f,
            };
            return job2.Schedule(sticksBuffer.Length, dependency);
        }



        [BurstCompile]
        public struct SimulatePhysicsForEachPointJob : IJobFor
        {
            [NativeDisableContainerSafetyRestriction]
            public NativeArray<Point> Points;
            public float DeltaTime;
            public float Gravity;
            public float Damping;
            public float3 MinBoundBox;
            public float3 MaxBoundBox;

            [BurstCompile]
            public void Execute(int index)
            {
                if (Points[index].IsLocked)
                    return;
                
                var point = Points[index];
                float3 positionBeforeUpdate = point.Position;
                var velocity = (point.Position - point.PrevesPosition) * Damping;
                var gravity = math.down() * Gravity * DeltaTime;
                var finaPosition = positionBeforeUpdate + velocity + gravity;
                finaPosition = LimitedByBound(finaPosition, MinBoundBox, MaxBoundBox);
                point.Position = finaPosition;
                point.PrevesPosition = positionBeforeUpdate;
                Points[index] = point;
            }
            private float3 LimitedByBound(float3 point, float3 miniBound, float3 maxBound)
            {
                return new float3(math.clamp(point.x, miniBound.x, maxBound.x),
                    math.clamp(point.y, miniBound.y, maxBound.y),
                    math.clamp(point.z, miniBound.z, maxBound.z));
            }
        }

        [BurstCompile]
        public struct SimulateConstrainsJob : IJobFor
        {
            [ReadOnly] public NativeArray<Stick>.ReadOnly Sticks;
            [NativeDisableContainerSafetyRestriction]
            public NativeArray<Point> Points;
            public float Thinness;

            [BurstCompile]
            public void Execute(int index)
            {
                if (!Sticks[index].Active)
                    return;
                Constrain(index);
            }
            [BurstCompile]
            private void Constrain(int index)
            {
                var pointA = Points[Sticks[index].PointA];
                var pointB = Points[Sticks[index].PointB];

                
                var bothPointsLocked = pointA.IsLocked && pointB.IsLocked;
                if (bothPointsLocked) return;

                var delta = pointA.Position - pointB.Position;
                float distances = math.length(delta);
                

                var offset = (float)(distances - Sticks[index].Lenght * Stick.CONVERTION_RATE) / distances;

                if (!pointA.IsLocked && !pointB.IsLocked)
                {

                    var correction = 0.5f * offset * Thinness * delta;
                    pointA.Position -= correction;
                    pointB.Position += correction;
                    Points[Sticks[index].PointA] = pointA;
                }
                else if(!pointA.IsLocked)
                {
                    var correction = offset * Thinness * delta;
                    pointA.Position -= correction;
                }
                else
                {
                    var correction = offset * Thinness * delta;
                    pointB.Position += correction;
                }
                
                Points[Sticks[index].PointA] = pointA;
                Points[Sticks[index].PointB] = pointB;
            }
        }
    }
}