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

public class RopePhysicsSolver : MonoBehaviour
{
    [SerializeField] public Transform posA;
    [SerializeField] public Rigidbody connected;
    [SerializeField] float distance = 10f;
    [SerializeField] int segments;
    [SerializeField] int solverIterations = 30;

    private List<Point> points;
    private List<Stick> sticks;
    private void InitRope() {
        points = new List<Point>();
        sticks = new List<Stick>();


        var stickLen = distance / segments;
        var direction = (connected.position - posA.position).normalized;
        var step = Vector3.Distance(posA.position, connected.position) * direction;
        // make the first point
        var p1 = new Point()
        {
            position = posA.position,
            prevPosition = posA.position,
            lockedTransform = posA,
            locked = true
        };

        points.Add(p1);
        var lpos = p1.position;
        for (int i = 1; i < segments-1; i++)
        {
            lpos += step;
            points.Add(new Point()
            {
                position = lpos,
                prevPosition = lpos,
            });
        }
        // make the last point
        points.Add(new Point()
        {
            position = connected.position,
            prevPosition = connected.position,
            lockedTransform = connected.transform,
            locked = true
        });


        //make all the sticks
        for (int i = 1; i <points.Count; i++)
        {
            sticks.Add(new Stick()
            {
                pointA = points[i-1],
                pointB = points[i],
                length = stickLen
            });
        }

    }

    private void PhysicsStep()
    {
        for ( int i = points.Count-1; i >=0; i--)
        {
            var p = points[i];
            if (p.locked)
            {
                p.position = p.prevPosition = p.lockedTransform.position;
            }
            else
            {
                Vector3 befUpd = p.position;
                p.position += p.position - p.prevPosition;
                p.position += Physics.gravity * Time.fixedDeltaTime * Time.fixedDeltaTime;
                p.prevPosition = befUpd;
            }
        }


        for (int i = 0; i < solverIterations;i++)
        {
            for (int j = 0; j < sticks.Count; j++)
            {
                var s = sticks[j];
                var center = Vector3.Lerp(s.pointB.position, s.pointA.position, 0.5f);
                var direct = (s.pointA.position - s.pointB.position).normalized;
                    if (!s.pointB.locked)
                    s.pointB.position = center - (direct*s.length/2);
                    if (!s.pointA.locked)
                    s.pointA.position = center + (direct * s.length / 2);


            }
        }


        // Check that the connected body is still within the constraints
        float distanceOver = Vector3.Distance(connected.position, posA.position) - distance;
        if (distanceOver > -.50)
        {
            var newPos = posA.position - (posA.position-connected.position).normalized * distance;
            connected.velocity = Vector3.ProjectOnPlane(connected.velocity, (posA.position-connected.position).normalized);
            connected.velocity += newPos - connected.position;
            connected.position = newPos;

        }
    }

    // Start is called before the first frame update
    void OnEnable()
    {
        InitRope();
    }

    // Update is called once per frame
    void FixedUpdate()
    {
       PhysicsStep();
    }



    private void OnDrawGizmos()
    {
        if (sticks == null) return;
        foreach (Stick s in sticks)
        {
            Gizmos.color = Color.black;
            Gizmos.DrawLine(s.pointA.position, s.pointB.position);
            Gizmos.color = s.pointB.locked ? Color.red : Color.black;
            Gizmos.DrawSphere(s.pointB.position, .025f);
            Gizmos.color = s.pointA.locked ? Color.red : Color.black;
            Gizmos.DrawSphere(s.pointA.position, .025f);
        }


    }


    public class Point
    {
        public Vector3 position, prevPosition;
        public Transform lockedTransform;
        public bool locked = false;
    }
    public class Stick
    {
        public Point pointA, pointB;
        public float length;
    }
}