using System;
using Sandbox;

namespace Andicraft.SecondOrderDynamics;

public class FloatDynamics : SecondOrderDynamics<float>
{
	public float MinWrapValue = 0f;
	public float MaxWrapValue = 0f;

	/// <summary>
	/// Tells you if the system is set to wrap the value.
	/// </summary>
	public bool Wrapping => !MinWrapValue.AlmostEqual( MaxWrapValue, 0.001f );
	private float HalfWrap => MathF.Abs(MaxWrapValue - MinWrapValue) * 0.5f;

	/// <inheritdoc/>
	public FloatDynamics(float startingValue) : base(startingValue)
	{
	}

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

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

	/// <inheritdoc/>
	public override void Reset(float value)
	{
		_previousValue = value;
		_currentValue = value;
		_velocity = 0;
	}

	/// <inheritdoc/>
	public override float Update(float deltaTime, float target, bool setVelocity = false, float velocity = default)
	{
		if (Wrapping)
		{
			target = Utils.Wrap(target, MinWrapValue, MaxWrapValue);

			var dist = target - _currentValue;
			var distOtherDir = target + HalfWrap * -1 * MathF.Sign(dist) - _currentValue;
			if (MathF.Abs(distOtherDir) < MathF.Abs(dist))
			{
				target += HalfWrap * -1 * MathF.Sign(dist);
			}
		}

		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 Wrapping ? Utils.Wrap(_currentValue, MinWrapValue, MaxWrapValue) : _currentValue;
	}
}