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

public class DistanceJoint3D : MonoBehaviour
{
    public Rigidbody ConnectedRigidbody;
    public float distance;
    public float damper = 5f;

	public Vector3 moveDelta;

    private Rigidbody rb;

	public float maxMagnitude = 30;

	[SerializeField]
	private float ps= 10;

    private void Awake()
    {
        rb = GetComponent<Rigidbody>();
    }

    private void Start()
    {
        //distance = Vector3.Distance(rb.position, ConnectedRigidbody.position);
    }

    private void Update()
    {
        if (distance > 0)
        {
			distance  = Mathf.Max(0, distance - ps * Time.deltaTime);
			//distance = Mathf.Lerp(distance, 0, 0.1f);
		}
    }
    private void FixedUpdate()
    {
        
        var connection = rb.position - ConnectedRigidbody.position;
        var distanceDiscrepancy = distance - connection.magnitude;

		//rb.position += distanceDiscrepancy * connection.normalized;
		rb.AddForce(-connection, ForceMode.Acceleration);

		var velocityTarget = connection + rb.velocity;
		var projectOnConnect = Vector3.Project(velocityTarget, connection);
		rb.velocity = (velocityTarget - projectOnConnect + /*distanceDiscrepancy */ -ps * connection.normalized + (projectOnConnect - connection) * 0.1f) / (1 + damper * Time.fixedDeltaTime);
		rb.AddForce(moveDelta);
		if (rb.velocity.magnitude > maxMagnitude)
		{
			rb.velocity = rb.velocity.normalized * maxMagnitude;
		}
		//rb.position += distanceDiscrepancy * connection.normalized;
		/*
		float distanceDelta = Vector3.Dot(rb.velocity, connection.normalized);
		Debug.Log(distanceDelta);
		if (distanceDelta > 0)
		{
			distance += distanceDelta;
			rb.velocity -= connection.normalized;
		}
		else
		{
			var velocityTarget = connection + rb.velocity;
			var projectOnConnect = Vector3.Project(velocityTarget, connection);
			rb.velocity = (velocityTarget - projectOnConnect + distanceDiscrepancy * connection.normalized) / (1 + damper * Time.fixedDeltaTime);
		}
		*/


	}

}
