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

public class LimbRenderer : MonoBehaviour
{
    public Transform parent;
    public float segmentLength = 1;

    LineRenderer lr;
    private Vector3[] positions;

    private void Awake() {
        lr = GetComponent<LineRenderer>();
        positions = new Vector3[lr.positionCount];
        lr.GetPositions(positions);
        Update();
    }

    private void Update() {
        positions[0] = parent.position;
        FABRIK.Solve(ref positions, transform.position, segmentLength);

        lr.SetPositions(positions);
    }
}

public static class FABRIK {
    const int maxIterations = 100;
    const float minAcceptableDst = 0.01f;

    public static void Solve(ref Vector3[] points, Vector3 target, float segmentLength = 0) {
        Vector3 origin = points[0];
        float[] segmentLengths = new float[points.Length - 1];
        for (int i = 0; i < points.Length - 1; i++) {
            if (segmentLength > 0) {
                segmentLengths[i] = segmentLength;
            } else {
                segmentLengths[i] = (points[i + 1] - points[i]).magnitude;
            }
        }

        for (int iteration = 0; iteration < maxIterations; iteration++) {
            bool startingFromTarget = iteration % 2 == 0;

            System.Array.Reverse(points);
            System.Array.Reverse(segmentLengths);
            points[0] = (startingFromTarget) ? target : origin;

            // Constrain length
            for (int i = 1; i < points.Length; i++) {
                Vector2 dir = ((Vector2)(points[i] - points[i - 1])).normalized;
                points[i] = points[i - 1] + (Vector3)dir * segmentLengths[i - 1];
            }

            // Terminate if close enough to tqaret
            float dstToTarget = (points[points.Length - 1] - target).magnitude;
            if (!startingFromTarget && dstToTarget <= minAcceptableDst) {
                return;
            }
        }
    }
}


