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

public class RotationSpring : MonoBehaviour
{
    public Quaternion pin;
    public Transform pinTransform;

    public float strength = 1;
    public float dampener = 0.3f;
    public float threshold = 0.001f;

    Vector3 velocity;



    private void Start()
    {
        pin = Quaternion.identity;

    }
    private void Update()
    {
        Step(Time.deltaTime, 7);
    }

    public void Step(float deltaTime, int subSteps = 1)
    {

        var q = Quaternion.FromToRotation(transform.forward, pin * Vector3.forward);
        float angle = 0;
        Vector3 axis = Vector3.zero;

        q.ToAngleAxis(out angle, out axis);

        Vector3 f = axis * strength - dampener * velocity;

        float zp = pin.eulerAngles.z;
        float zc = transform.rotation.eulerAngles.z;

        f.z = -Mathf.Sign((Mathf.DeltaAngle(zp, zc))) * strength - dampener * velocity.z;

        if (Quaternion.Angle(pin, transform.rotation * Quaternion.Euler(velocity * deltaTime)) > threshold)
        {
            velocity += f * deltaTime;
        }
        else
        {
            velocity = Vector3.MoveTowards(velocity, Vector3.zero, deltaTime * 5);
        }

        transform.Rotate(velocity * deltaTime, Space.World);

        //float angle = Quaternion.Angle(transform.rotation, pin);

        //angle = Vector3.SignedAngle(transform.forward, pin * Vector3.forward, transform.up);

        //float direction = Mathf.Sign(-angle);


        //float f = -direction * strength - dampener * velocity;
        //velocity += f * Time.deltaTime;

        //transform.rotation = Quaternion.SlerpUnclamped(transform.rotation, pin, velocity * deltaTime);
    }
}
