using Godot;
using System;

public class Entity : KinematicBody
{
    public Vector3 Velocity { get; set; } = Vector3.Zero;
    public float RotationalVelocity { get; set; } = 0;

    /** Defines the angle in plane coordinates, 0 => pointing to the right/east, pi/2 pointing up/north, range [-pi,pi]. */
    public float PlaneAngle
    {
        get => GlobalTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up);
        set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation);
    }
   
    public float CalcShortestPlaneRotationToTargetDirection(Vector3 globalTargetDirection)
    {
        float angleToTarget = Vector3.Right.SignedAngleTo(globalTargetDirection, Vector3.Up);
        float currentAngle = PlaneAngle;

        float delta = angleToTarget - currentAngle;
        
        delta += (delta > Mathf.Pi) ? -Mathf.Pi * 2 : (delta < -Mathf.Pi) ? Mathf.Pi * 2 : 0;
        return delta;
    }

    public override void _Ready()
    {
    }
}