using System;
using Unity.Collections;
using Unity.Mathematics;

namespace RebootReality.jelycho.Ropes {
    public struct RopeConfig {
        public float segmentLength;
        public float gravity;
        public float dampingFactor;
        public int numberOfConstrainIterations;
        
        public static readonly RopeConfig Default = new RopeConfig {
            segmentLength = 0.5f,
            gravity = -9.81f,
            dampingFactor = 0.95f,
            numberOfConstrainIterations = 50
        };
    }

    public struct Rope : IDisposable {
        internal struct Segment {
            public float3 position;
            public float3 previousPosition;
            public bool isLocked;
        }

        internal NativeArray<Segment> Segments;
        readonly RopeConfig m_Config;

        public Rope(NativeArray<float3> positions, RopeConfig config) {
            if (positions.Length < 2) {
                throw new ArgumentException("Rope must have at least two segments.");
            }
            
            Segments = new NativeArray<Segment>(positions.Length, Allocator.Persistent);
            m_Config = config;

            for (int i = 0; i < positions.Length; ++i) {
                Segment segment = new Segment {
                    position = positions[i],
                    previousPosition = positions[i],
                    isLocked = false
                };
                Segments[i] = segment;
            }
        }

        public void Dispose() {
            Segments.Dispose();
        }

        public bool FirstSegmentIsLocked {
            get {
                return Segments.Length > 0 && Segments[0].isLocked;
            }

            set {
                if (Segments.Length > 0) {
                    Segment firstSegment = Segments[0];
                    firstSegment.isLocked = value;
                    Segments[0] = firstSegment;
                }
            }
        }
        
        public bool LastSegmentIsLocked {
            get {
                return Segments.Length > 0 && Segments[Segments.Length - 1].isLocked;
            }

            set {
                if (Segments.Length > 0) {
                    Segment lastSegment = Segments[Segments.Length - 1];
                    lastSegment.isLocked = value;
                    Segments[Segments.Length - 1] = lastSegment;
                }
            }
        }

        public void Simulate(float deltaTime) {
            if (Segments.Length < 2) {
                return;
            }
            
            for (int i = 0; i < Segments.Length; ++i) {
                Segment segment = Segments[i];
                if (segment.isLocked) {
                    continue;
                }

                float3 segmentPositionBeforeUpdate = segment.position;
                segment.position += (segment.position - segment.previousPosition) * m_Config.dampingFactor;
                segment.position.y += m_Config.gravity * deltaTime;
                segment.previousPosition = segmentPositionBeforeUpdate;
                
                Segments[i] = segment;
            }

            for (int iteration = 0; iteration < m_Config.numberOfConstrainIterations; ++iteration) {
                for (int i = 0; i < Segments.Length - 1; ++i) {
                    Segment segment = Segments[i];
                    Segment nextSegment = Segments[i + 1];
                
                    float currentDistance = math.distance(segment.position, nextSegment.position);
                    float difference = currentDistance - m_Config.segmentLength;
                    float3 direction = math.normalize(segment.position - nextSegment.position);
                    float3 change = direction * (difference * 0.5f);

                    if (!segment.isLocked) {
                        segment.position -= change;
                        Segments[i] = segment;
                    }

                    if (!nextSegment.isLocked) {
                        nextSegment.position += change;
                        Segments[i + 1] = nextSegment;
                    }
                }               
            }
        }
    }
}