using Andicraft.SecondOrderDynamics;
using Sandbox;

namespace Andicraft.SecondOrderDynamics.Components;

[Title( "Second Order Dynamics - Follow Target" ), Category("Second Order Dynamics")]
public class FollowTargetComponent : Component
{
	[Property] public GameObject FollowTarget { get; set; }

	/// <summary>
	/// Reset the dynamics if the object moves further than this distance in one frame
	/// </summary>
	[Property] public float TeleportDistanceThreshold { get; set; } = 32 * 10;

	[Property] public DynamicsParameters PositionParameters { get; set; } = new();
	[Property] public DynamicsParameters RotationParameters { get; set; } = new();

	private Vector3Dynamics _positionDynamics;
	private RotationDynamics _rotationDynamics;

	private Vector3 _previousFollowPos = default;
	
	protected override void OnStart()
	{
		_positionDynamics = new Vector3Dynamics( PositionParameters, FollowTarget.WorldPosition );
		_rotationDynamics = new RotationDynamics( RotationParameters, FollowTarget.WorldRotation );

		_previousFollowPos = FollowTarget.WorldPosition;
	}

	protected override void OnUpdate()
	{
		if ( FollowTarget == null ) return;

		if ( FollowTarget.WorldPosition.DistanceSquared( _previousFollowPos ) >
		     TeleportDistanceThreshold * TeleportDistanceThreshold )
		{
			Warp();
			_previousFollowPos = FollowTarget.WorldPosition;
			return;
		}
		
		WorldPosition = _positionDynamics.Update( Time.Delta, FollowTarget.WorldPosition );
		WorldRotation = _rotationDynamics.Update( Time.Delta, FollowTarget.WorldRotation );

		_previousFollowPos = FollowTarget.WorldPosition;
	}

	/// <summary>
	/// Resets the following target, useful for when you teleport or jump somewhere
	/// </summary>
	public void Warp()
	{
		_positionDynamics.Reset( FollowTarget.WorldPosition );
		_rotationDynamics.Reset( FollowTarget.WorldRotation );

		WorldPosition = FollowTarget.WorldPosition;
		WorldRotation = FollowTarget.WorldRotation;
	}

	public void UpdatePositionParameters( DynamicsParameters p )
	{
		PositionParameters = p;
		_positionDynamics.UpdateParameters( p );
	}

	public void UpdateRotationParameters( DynamicsParameters p )
	{
		RotationParameters = p;
		_rotationDynamics.UpdateParameters( p );
	}
}