using System;

namespace Andicraft.SecondOrderDynamics;

public class Vector4Dynamics : SecondOrderDynamics<Vector4>
{
	/// <inheritdoc/>
	public Vector4Dynamics(Vector4 startingValue) : base(startingValue)
	{
	}

	/// <inheritdoc/>
	public Vector4Dynamics(Vector3 parameters, Vector4 startingValue) : base(parameters, startingValue)
	{
	}

	/// <inheritdoc/>
	public Vector4Dynamics(float frequency, float damping, float response, Vector4 startingValue) : base(frequency, damping, response, startingValue)
	{
	}

	/// <inheritdoc/>
	public override void Reset(Vector4 value)
	{
		_previousValue = value;
		_currentValue = value;
		_velocity = Vector4.Zero;
	}

	/// <inheritdoc/>
	public override Vector4 Update(float deltaTime, Vector4 target, bool setVelocity = false, Vector4 velocity = default)
	{
		if (setVelocity == false)
		{
			velocity = (target - _previousValue) / deltaTime;
			_previousValue = target;
		}

		var k2Stable = MathF.Max(k2, MathF.Max(deltaTime * deltaTime / 2 + deltaTime * k1 / 2, deltaTime * k1));
		_currentValue += deltaTime * _velocity;
		_velocity += deltaTime * (target + k3 * velocity - _currentValue - k1 * _velocity) / k2Stable;
		return _currentValue;
	}
}