// Based on: allenchou.net/2015/04/game-math-precise-control-over-numeric-springing/

using Godot;
using System;
using System.Diagnostics;

public class SpringDamper
{
    public float omega = 1;
    public float zeta = 1;

    public SpringDamper(float osc_freq = 1.0f, float osc_red = 0.1f, float osc_red_h = 1.0f)
    {
	    Debug.Assert(osc_red > 0.001 && osc_red < 0.999);
	    omega = osc_freq * 2 * Mathf.Pi;
	    zeta = Mathf.Log(1.0f - osc_red) / (-omega * osc_red_h);
    }

    public (float, float) Calc(float x, float v, float xt, float h)
    {
	    float f = 1 + 2 * h * zeta * omega;
	    float oo = omega * omega;
	    float hoo = oo * h;
	    float hhoo = hoo * h;

	    float det_inv = 1.0f / (f + hhoo);
	    float det_x = f * x + h * v + hhoo * xt;
	    float det_v = v + hoo * (xt - x);
	    
	    return (det_x * det_inv, det_v * det_inv);
    }
    
    public (Vector3, Vector3) Calc(Vector3 x, Vector3 v, Vector3 xt, float h)
    {
	    float f = 1 + 2 * h * zeta * omega;
	    float oo = omega * omega;
	    float hoo = oo * h;
	    float hhoo = hoo * h;

	    float det_inv = 1.0f / (f + hhoo);
	    Vector3 det_x = f * x + h * v + hhoo * xt;
	    Vector3 det_v = v + hoo * (xt - x);
	    
	    return (det_x * det_inv, det_v * det_inv);
    }

    public (Vector3, Vector3) CalcClampedSpeed(Vector3 x, Vector3 v, Vector3 xt, float h, float speedMax)
    {
	    var defaultResult = Calc(x, v, xt, h);

	    Vector3 x_new = defaultResult.Item1;
	    Vector3 vel_new = (x_new - x) / h;
	    float speed_new = vel_new.Length();

	    if (speed_new > speedMax)
	    {
		    vel_new = (vel_new / speed_new) * speedMax;
		    x_new = x + vel_new * h;
	    }

	    return (x_new, vel_new);
    }
}