using System;

namespace Andicraft.SecondOrderDynamics;

public class Vector2Dynamics : SecondOrderDynamics<Vector2>
{
	/// <inheritdoc/>
	public Vector2Dynamics(Vector2 startingValue) : base(startingValue)
	{
	}

	/// <inheritdoc/>
	public Vector2Dynamics(Vector3 parameters, Vector2 startingValue) : base(parameters, startingValue)
	{
	}

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

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

	/// <inheritdoc/>
	public override Vector2 Update(float deltaTime, Vector2 target, bool setVelocity = false, Vector2 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;
	}
}