using UnityEngine;

public struct DampedSpring
{
	float pp, pv, vp, vv;

	// Calculates integration coefficients for a spring.
	// 
	// - dt:     time delta
	// - period: time between each oscillation
	// - bounce: 0=critically damped, 1=undamped
	public DampedSpring(float dt, float period, float bounce) {
		// Convert our useful parameters into spring coefficients.
		// See https://medium.com/ios-os-x-development/demystifying-uikit-spring-animations-2bb868446773
		period = Mathf.Max(period, 0.01f);
		float d = Mathf.Clamp(1 - bounce, 0.01f, 0.99f);    // damping ratio
		float w = 2*Mathf.PI / period;                      // angular frequency

		// Spring integration by Ryan Juckett:
		// See http://www.ryanjuckett.com/programming/damped-springs/
		float wd = w * d;
		float a = w * Mathf.Sqrt(1.0f - d*d);
		float e = Mathf.Exp(-wd * dt);
		float c = Mathf.Cos(a * dt);
		float s = Mathf.Sin(a * dt);

		float ia = 1.0f / a;

		float es = e*s;
		float ec = e*c;
		float q = e*wd*s*ia;

		pp = ec + q;
		pv = es*ia;
		vp = -es*a - wd*q;
		vv = ec - q;
	}

	// Applies a set of spring parameters to a value.
	public void Apply(float target, ref float pos, ref float vel) {
		float oldPos = pos - target;
		float oldVel = vel;
		pos = oldPos*pp + oldVel * pv + target;
		vel = oldPos*vp + oldVel * vv;
	}

	// Applies a set of spring parameters to a value.
	public void Apply(Vector3 target, ref Vector3 pos, ref Vector3 vel) {
		Vector3 oldPos = pos - target;
		Vector3 oldVel = vel;
		pos = oldPos*pp + oldVel * pv + target;
		vel = oldPos*vp + oldVel * vv;
	}
}

public struct SprungValue
{
	public float value, velocity;
	DampedSpring springIn, springOut;

	public float goal {
		set {
			springIn.Apply(value, ref this.value, ref this.velocity);
		}
	}

	public float goalOut {
		set {
			springOut.Apply(value, ref this.value, ref this.velocity);
		}
	}

	public void Integrate() {
		value += Time.fixedDeltaTime*velocity;
	}

	public void Configure(float period, float bounce, float p2 = -1, float b2 = -1) {
		if (p2 < 0) p2 = period;
		if (b2 < 0) b2 = bounce;
		springIn = new DampedSpring(Time.fixedDeltaTime, period, bounce);
		springOut = new DampedSpring(Time.fixedDeltaTime, p2, b2);
	}
}
