using System;

namespace Andicraft.SecondOrderDynamics;

// based on https://www.youtube.com/watch?v=KPoeNZZ6H4s
public abstract class SecondOrderDynamics<T>
{
	public T CurrentValue => _currentValue;
	protected T _currentValue;

	private Vector3 _parameters;
	protected T _previousValue;
	protected T _velocity;

	// yeah idk about these three, complicated and weird math, watch the video
	protected float k1;
	protected float k2;
	protected float k3;

	/// <summary>
	/// Creates a dynamics system with default parameters.
	/// </summary>
	protected SecondOrderDynamics(T startingValue) : this(3f, .5f, 1f, startingValue)
	{
	}

	/// <summary>
	/// Creates a dynamics system with assigned parameters.
	/// </summary>
	protected SecondOrderDynamics( DynamicsParameters p, T startingValue ) : this(p.Frequency, p.Damping, p.Response, startingValue)
	{
	}

	/// <summary>
	/// Creates a dynamics system with assigned parameters.
	/// </summary>
	protected SecondOrderDynamics(Vector3 parameters, T startingValue) : this(parameters.x, parameters.y, parameters.z, startingValue)
	{
	}

	/// <summary>
	/// Creates and initializes the dynamics system.
	/// </summary>
	/// <param name="frequency">Frequency of the system in hertz - the higher this is the faster things move</param>
	/// <param name="damping">Damping. At 0 it will never stop vibrating, at 1 you get critical damping. Between 0 and 1 you get springy motion, above 1 the value approaches very slowly.</param>
	/// <param name="response">Response. Less than one means slow to respond, at exactly one the system will follow the target value immediately. Above 1, the system will overshoot.</param>
	/// <param name="startingValue">Starting value.</param>
	protected SecondOrderDynamics(float frequency, float damping, float response, T startingValue)
	{
		_parameters = new Vector3(frequency, damping, response);

		_previousValue = startingValue;
		_currentValue = startingValue;
		CalculateParameters();
	}

	/// <summary>
	/// Lets you refresh parameters mid-simulation.
	/// </summary>
	/// <param name="v">Parameters in Vector3 format. Implicit conversion allows you to pass in a DynamicsParameters object.</param>
	public void UpdateParameters(Vector3 v)
	{
		_parameters = v;
		CalculateParameters();
	}

	/// <summary>
	/// Lets you refresh parameters mid-simulation.
	/// </summary>
	public void UpdateParameters(float frequency, float damping, float response)
	{
		_parameters.x = frequency;
		_parameters.y = damping;
		_parameters.z = response;
		CalculateParameters();
	}

	/// <summary>
	/// Lets you change the frequency of the system mid-simulation.
	/// </summary>
	public void SetFrequency(float frequency)
	{
		_parameters.x = frequency;
		CalculateParameters();
	}

	/// <summary>
	/// Lets you change the damping of the system mid-simulation.
	/// </summary>
	public void SetDamping(float damping)
	{
		_parameters.y = damping;
		CalculateParameters();
	}

	/// <summary>
	/// Lets you change the response of the system mid-simulation.
	/// </summary>
	public void SetResponse(float response)
	{
		_parameters.z = response;
		CalculateParameters();
	}
	
	private void CalculateParameters()
	{
		var f = _parameters.x;
		var z = _parameters.y;
		var r = _parameters.z;

		k1 = z / (MathF.PI * f);
		k2 = 1 / (2 * MathF.PI * f * (2 * MathF.PI * f));
		k3 = r * z / (2 * MathF.PI * f);
	}

	/// <summary>
	/// Sets system to a specified value, along with setting the velocity to zero.
	/// </summary>
	public abstract void Reset(T value);

	/// <summary>
	/// Update the system and retreive the new value.
	/// </summary>
	/// <param name="deltaTime">Time since last update, in seconds.</param>
	/// <param name="target">The value you want to approach.</param>
	/// <param name="setVelocity">If <c>true</c>, allows you to specify the velocity - useful to kick the system in one direction without changing the target.</param>
	/// <param name="velocity">If <c>setVelocity</c> is <c>true</c>, this is the velocity to set.</param>
	/// <returns></returns>
	public abstract T Update(float deltaTime, T target, bool setVelocity = false, T velocity = default);
	// If we had access to Microsoft.CSharp this would also contain the code to update the systems, using dynamic variables
	// to allow me to do math with generic types - massively reducing code duplication. Oh well!
}

/// <summary>
/// Defines the behavior of a Second Order Dynamics system with simple and easy to control parameters.
/// </summary>
[Serializable]
public class DynamicsParameters
{
	/// <summary>
	/// The frequency of the system in Hz. The higher this is, the faster the system moves.
	/// </summary>
	public float Frequency { get; set; } = 1;
	
	/// <summary>
	/// Damping strength of the system.
	/// <list type="bullet">
	/// <item>At exactly 0, the system will never stop vibrating.</item>
	/// <item>Between 0 and 1, you'll get a slowly settling spring motion.</item>
	/// <item>1 is critical damping (like <c>MathX.SmoothDamp</c>)</item>
	/// <item>Above 1 things start approaching slower and slower.</item>
	/// </list>
	/// </summary>
	public float Damping { get; set; } = 1;
	
	/// <summary>
	/// Determines how the system responds to changing values.
	/// <list type="bullet">
	/// <item>Values &lt; 0 will anticipate (go backwards before going forward - it's weird but cool!)</item>
	/// <item>Values between 0 and 1 will start moving slowly.</item>
	/// <item>If exactly 1, the system will start following the target value immediately.</item>
	/// <item>Values above 1 will cause the system to overshoot - this is where the really cool motion happens!</item>
	/// </list>
	/// </summary>
	public float Response { get; set; } = 1;
	
	public DynamicsParameters(){}

	public DynamicsParameters( float frequency, float response, float damping )
	{
		Frequency = frequency;
		Response = response;
		Damping = damping;
	}

	public static implicit operator Vector3(DynamicsParameters p) =>
		new Vector3(p.Frequency, p.Damping, p.Response);

	public static implicit operator DynamicsParameters(Vector3 p)
	{
		return new DynamicsParameters()
		{
			Frequency = p.x,
			Damping = p.y,
			Response = p.z
		};
	}
}