using System;

namespace Andicraft.SecondOrderDynamics;

public class Vector3Dynamics : SecondOrderDynamics<Vector3>
{
	/// <inheritdoc/>
	public Vector3Dynamics(Vector3 startingValue) : base(startingValue)
	{
	}

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

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

	/// <inheritdoc/>
	public override void Reset(Vector3 value)
	{
		_previousValue = value;
		_currentValue = value;
		_velocity = Vector3.Zero;
	}
	
	/// <inheritdoc/>
	public override Vector3 Update(float deltaTime, Vector3 target, bool setVelocity = false, Vector3 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;
	}
}