using Godot;
using System;

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

    public float targetAngle = 0;
    public float currentAngle = 0;
    public float currentAngularVelocity = 0;

    [Export] public float maxSpeed = 3;

    private SpringDamper _positionSpringDamper;
    private SpringDamper _angleSpringDamper;

    [Signal]
    delegate void PositionUpdated(Vector3 newPosition);

    [Signal]
    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 targetDir = Vector3.Zero;
        Vector3 targetError = targetPosition - currentPosition;

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

            if (currentAngle < Mathf.Pi && targetAngle > Mathf.Pi)
            {
            }

            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)
        {
            var springDamperResult =
                _angleSpringDamper.Calc(currentAngle, currentAngularVelocity, targetAngle, delta);

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

            EmitSignal("OrientationUpdated", this.currentAngle);
        }

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

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

            EmitSignal("PositionUpdated", currentPosition);
        }
    }
}