#nullable enable

public sealed class DampenTest : Component
{
	[Property]
	public GameObject? Target { get; set; }

	[Property]
	public TextRenderer? Text { get; set; }

	[Property]
	public float UpdateRate { get; set; } = 60f;

	[Property]
	public float Variance { get; set; }

	private Vector3.SpringDamped _spring;

	private float _time;
	private float _updateRate;

	protected override void OnStart()
	{
		_spring = new( 0f, Vector3.Up * 100f, 2f, 0.125f );
		_updateRate = UpdateRate;
	}

	public void SetPosition( Vector3 position )
	{
		_spring.Current = position;
	}

	public void SetVelocity( Vector3 velocity )
	{
		_spring.Velocity = velocity;
	}

	protected override void OnUpdate()
	{
		_time += Time.Delta;

		var dt = 1f / _updateRate;

		if ( dt <= 0f ) return;

		if ( _time >= dt )
		{
			while ( _time >= dt )
			{
				_time -= dt;
				_spring.Update( dt );
			}

			_updateRate = Math.Max( 1f, UpdateRate + Random.Shared.Float( -Variance, Variance ) );

			if ( Text is { } text )
			{
				text.Text = $"{_updateRate:F1} Hz";
			}
		}

		if ( Target is { } target )
		{
			target.LocalPosition = _spring.Current;
			target.LocalRotation = Rotation.LookAt( target.LocalPosition, Vector3.Forward );

			DebugOverlay.Sphere( new Sphere( target.WorldPosition + _spring.Velocity, 10f ), Color.Red );
			DebugOverlay.Line( target.WorldPosition, target.WorldPosition + _spring.Velocity, Color.Red );
		}
	}
}

public readonly record struct LegacySpringDamperModel( float Frequency, float Damping )
{
	public (float Position, float Velocity) Simulate( float position, float velocity, float deltaTime )
	{
		if ( deltaTime <= 0.0f ) return (position, velocity);

		// Angular frequency (how fast the spring oscillates)
		var omega = Frequency * MathF.PI * 2.0f;

		// Damping factor to control how much oscillation decays over time
		var dampingFactor = Damping * omega;

		// Compute the velocity using spring physics
		var force = omega * omega * -position - 2.0f * dampingFactor * velocity;
		velocity += force * deltaTime;

		// Update position
		return (position + velocity * deltaTime, velocity);
	}
}

public readonly record struct LegacySmoothDamperModel( float SmoothTime )
{
	public (float Position, float Velocity) Simulate( float position, float velocity, float deltaTime )
	{
		// If smoothing time is zero, directly jump to target (independent of timestep)
		if ( SmoothTime <= 0.0f )
		{
			return (0f, velocity);
		}

		// If timestep is zero, stay at current position
		if ( deltaTime <= 0.0f )
		{
			return (position, velocity);
		}

		// Implicit integration of critically damped spring
		var omega = MathF.PI * 2.0f / SmoothTime;
		velocity = (velocity - (omega * omega) * deltaTime * position) / ((1.0f + omega * deltaTime) * (1.0f + omega * deltaTime));

		return (position + velocity * deltaTime, velocity);
	}
}

public sealed class DampenTestManager : Component
{
	[Property, InputAction]
	public string? KickAction { get; set; }

	[Property, Range( -128f, 128f ), Step( 0.25f )]
	public float InitialPosition { get; set; }

	[Property, Range( -1024f, 1024f ), Step(1 )]
	public float InitialVelocity { get; set; }

	[Property, Range( 0.1f, 4f )]
	public float Frequency { get; set; } = 2f;

	[Property, Range( 0f, 1f )]
	public float Damping { get; set; } = 0.5f;

	[Property, Range( 1f, 120f )]
	public float FrameRate { get; set; } = 60f;

	protected override void OnUpdate()
	{
		if ( Input.Pressed( KickAction ) )
		{
			var position = Random.Shared.VectorInSphere( 128f );
			var velocity = Random.Shared.VectorInSphere( 1024f );

			foreach ( var test in Scene.GetAllComponents<DampenTest>() )
			{
				test.SetPosition( position );
				test.SetVelocity( velocity );
			}
		}
	}

	protected override void DrawGizmos()
	{
		var dt = 1f / FrameRate;
		var offset = new Vector3( 512f, 0f, 0f );

		Gizmo.Draw.Color = Color.White;
		Gizmo.Draw.Line( new Vector3( 0f, 0f, 0f ) + offset, new Vector3( 10f * 64f, 0f, 0f ) + offset );

		Draw( (pos, vel, deltaTime) =>
		{
			Vector3 vel3 = vel;

			pos = Vector3.SpringDamp( pos, 0f, ref vel3, deltaTime, Frequency, Damping ).x;

			return (pos, vel3.x);
		}, offset, 10f, dt, Color.Green );
		Draw( new LegacySpringDamperModel( Frequency, Damping ).Simulate, offset, 10f, dt, Color.Red );
	}

	private void Draw( Func<float, float, float, (float, float)> simulate, Vector3 offset, float duration, float dt, Color color )
	{
		float x = InitialPosition, v = InitialVelocity;

		Gizmo.Draw.Color = color;

		for ( var t = 0f; t <= duration; t += dt )
		{
			var prev = x;

			(x, v) = simulate( x, v, dt );

			if ( Math.Abs( x ) > 1024f )
			{
				x = 0f;
				v = 0f;
			}

			Gizmo.Draw.Line( new Vector3( t * 64f, prev, 0f ) + offset, new Vector3( (t + dt) * 64f, x, 0f ) + offset );
		}
	}
}