using Godot;

public class MovableComponent : Component {
    public Vector3 targetPosition = Vector3.Zero;
    public Vector3 currentPosition = Vector3.Zero;
    public Vector3 currentVelocity = Vector3.Zero;

    public float targetAngle;
    public float currentAngle;
    public float currentAngularVelocity;

    [Export] public float maxSpeed = 3;

    private SpringDamper _positionSpringDamper;
    private SpringDamper _angleSpringDamper;

    [Signal]
    private delegate void PositionUpdated(Vector3 newPosition);

    [Signal]
    private delegate void OrientationUpdated(float newAngle);

    // Called when the node enters the scene tree for the first time.
    public override void _Ready() {
        _positionSpringDamper = new SpringDamper(4, 0.99f, 0.5f);
        _angleSpringDamper = new SpringDamper(4, 0.99f, 0.5f);
    }

    // Called every frame. 'delta' is the elapsed time since the previous frame.
    public override void _Process(float delta) {
        Vector3 targetError = targetPosition - currentPosition;

        if (targetError.LengthSquared() > 0.01) {
            Vector3 targetDir = targetError.Normalized();
            targetAngle = new Vector3(0, 0, 1).SignedAngleTo(targetDir, Vector3.Up);

            if (targetAngle - currentAngle > Mathf.Pi) {
                currentAngle += 2 * Mathf.Pi;
            } else if (targetAngle - currentAngle < -Mathf.Pi) {
                currentAngle -= 2 * Mathf.Pi;
            }
        }

        if (Mathf.Abs(currentAngularVelocity) > 0.1 || Mathf.Abs(targetAngle - currentAngle) > 0.01) {
            (float, float) springDamperResult =
                _angleSpringDamper.Calc(currentAngle, currentAngularVelocity, targetAngle, delta);

            currentAngle = springDamperResult.Item1;
            currentAngularVelocity = springDamperResult.Item2;

            EmitSignal("OrientationUpdated", currentAngle);
        }

        if (Mathf.Abs(currentAngularVelocity) < 5 && Mathf.Abs(targetAngle - currentAngle) < 0.3
                                                  && (targetPosition - currentPosition).LengthSquared() > 0.01) {
            (Vector3, Vector3) springDamperResult =
                _positionSpringDamper.CalcClampedSpeedXZ(currentPosition, currentVelocity, targetPosition, delta,
                    maxSpeed);

            currentPosition = springDamperResult.Item1;
            currentVelocity = springDamperResult.Item2;
        }

        currentVelocity.y = currentVelocity.y - 9.81f * delta;
        currentPosition.y = currentPosition.y + currentVelocity.y * delta;

        EmitSignal("PositionUpdated", currentPosition);
    }
}