using System;

namespace Andicraft.SecondOrderDynamics;

public class RotationDynamics : SecondOrderDynamics<Rotation>
{
	/// <inheritdoc/>
	public RotationDynamics(Rotation startingValue) : base(startingValue)
	{
	}

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

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

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

	/// <inheritdoc/>
	public override Rotation Update(float deltaTime, Rotation target, bool setVelocity = false,
		Rotation velocity = default)
	{
		NormalizeSign(_currentValue, ref target);

		if (setVelocity == false)
		{
			velocity.x = (target.x - _previousValue.x) / deltaTime;
			velocity.y = (target.y - _previousValue.y) / deltaTime;
			velocity.z = (target.z - _previousValue.z) / deltaTime;
			velocity.w = (target.w - _previousValue.w) / deltaTime;
			_previousValue = target;
		}

		var k2Stable = MathF.Max(k2, MathF.Max(deltaTime * deltaTime / 2 + deltaTime * k1 / 2, deltaTime * k1));

		_currentValue.x += deltaTime * _velocity.x;
		_currentValue.y += deltaTime * _velocity.y;
		_currentValue.z += deltaTime * _velocity.z;
		_currentValue.w += deltaTime * _velocity.w;

		_velocity.x += deltaTime * (target.x + k3 * velocity.x - _currentValue.x - k1 * _velocity.x) / k2Stable;
		_velocity.y += deltaTime * (target.y + k3 * velocity.y - _currentValue.y - k1 * _velocity.y) / k2Stable;
		_velocity.z += deltaTime * (target.z + k3 * velocity.z - _currentValue.z - k1 * _velocity.z) / k2Stable;
		_velocity.w += deltaTime * (target.w + k3 * velocity.w - _currentValue.w - k1 * _velocity.w) / k2Stable;

		return _currentValue.Normal;
	}

	private static void NormalizeSign(Rotation current, ref Rotation target)
	{
		// if our dot product is positive, we don't need to invert signs.
		if (current.Dot(target) >= 0) return;

		// invert the signs on the components
		target.x *= -1;
		target.y *= -1;
		target.z *= -1;
		target.w *= -1;
	}
}