using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityUtils;

public class RopeSimulator : MonoBehaviour
{
    [SerializeField]
    private float gravity = 10;

    [SerializeField]
    private float solveIterations = 10;

    [SerializeField]
    private float constrainStickMinLength = 0.1f;

    int[] order;

    public Vector2 testPos;

    Rope rope;

    private void Start()
    {
        rope = new RopeBuilder()
            .AddPoint(new Point(testPos, locked: true))
            .AddPoint(new Point(testPos.Add(x:5f)))
            .AddPoint(new Point(testPos.Add(x: 10f)))
            .AddPoint(new Point(testPos.Add(x: 15f)))
            .AddPoint(new Point(testPos.Add(x: 20f)))
            .ConnectPoints(0, 1)
            .ConnectPoints(1, 2)
            .ConnectPoints(2, 3)
            .ConnectPoints(3, 4)
            .Build();
        CreateOrderArray();
    }

    private void Update()
    {
        Simulate();
    }

    private void OnDrawGizmos()
    {
        if (!Application.isPlaying) return;

        foreach (var point in rope.points)
        {
            Debug.Log($"pos: {point.position}");
            Gizmos.DrawSphere(point.position, 1f);
        }
    }

    void Simulate()
    {
        foreach (Point p in rope.points)
        {
            if (!p.locked)
            {
                Vector2 positionBeforeUpdate = p.position;
                p.position += p.position - p.prevPosition;
                p.position += Vector2.down * gravity * Time.deltaTime * Time.deltaTime;
                p.prevPosition = positionBeforeUpdate;
            }
        }

        for (int i = 0; i < solveIterations; i++)
        {
            for (int s = 0; s < rope.sticks.Count; s++)
            {
                Stick stick = rope.sticks[order[s]];
                if (stick.dead)
                {
                    continue;
                }

                Vector2 stickCentre = (stick.A.position + stick.B.position) / 2;
                Vector2 stickDir = (stick.A.position - stick.B.position).normalized;
                float length = (stick.A.position - stick.B.position).magnitude;

                if (length > stick.length || length > constrainStickMinLength)
                {
                    if (!stick.A.locked)
                    {
                        stick.A.position = stickCentre + stickDir * stick.length / 2;
                    }
                    if (!stick.B.locked)
                    {
                        stick.B.position = stickCentre - stickDir * stick.length / 2;
                    }
                }

            }
        }
    }

    void CreateOrderArray()
    {
        order = new int[rope.sticks.Count];
        for (int i = 0; i < order.Length; i++)
        {
            order[i] = i;
        }
        ShuffleArray(order, new System.Random());
    }

    public static T[] ShuffleArray<T>(T[] array, System.Random prng)
    {

        int elementsRemainingToShuffle = array.Length;
        int randomIndex = 0;

        while (elementsRemainingToShuffle > 1)
        {

            // Choose a random element from array
            randomIndex = prng.Next(0, elementsRemainingToShuffle);
            T chosenElement = array[randomIndex];

            // Swap the randomly chosen element with the last unshuffled element in the array
            elementsRemainingToShuffle--;
            array[randomIndex] = array[elementsRemainingToShuffle];
            array[elementsRemainingToShuffle] = chosenElement;
        }

        return array;
    }
}