using System;
using Godot;

public class NavigationPoint
{
    [Flags]
    public enum NavigationFlags
    {
        Position = 1,
        Orientation = 2
    }

    public readonly NavigationFlags Flags;
    public Quat WorldOrientation = Quat.Identity;
    public float WorldAngle;

    public Vector3 WorldPosition = Vector3.Zero;

    public NavigationPoint(Vector3 worldPosition)
    {
        WorldPosition = worldPosition;
        Flags = NavigationFlags.Position;
    }

    public NavigationPoint(Quat worldOrientation)
    {
        WorldOrientation = worldOrientation;
        Flags = NavigationFlags.Orientation;
    }

    public NavigationPoint(Transform worldTransform)
    {
        WorldPosition = worldTransform.origin;
        WorldOrientation = worldTransform.basis.Quat();
        WorldAngle = Globals.CalcPlaneAngle(worldTransform);
        
        Flags = NavigationFlags.Position | NavigationFlags.Orientation;
    }

    public bool IsReached(Transform worldTransform)
    {
        var goalReached = false;
        var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x,
            WorldPosition.z - worldTransform.origin.z);
        var positionErrorSquared = positionError.LengthSquared();
        worldTransform.basis.Quat();
        var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
        var angleError = Mathf.Abs(Globals.CalcPlaneAngle(worldTransform) - WorldAngle);

        if (Flags.HasFlag(NavigationFlags.Position)
            && Flags.HasFlag(NavigationFlags.Orientation)
            && positionErrorSquared < Globals.EpsPositionSquared
            && angleError < Globals.EpsRadians)
        {
            goalReached = true;
        }
        else if (Flags == NavigationFlags.Position &&
                 positionErrorSquared < Globals.EpsPositionSquared)
        {
            goalReached = true;
        }
        else if (Flags == NavigationFlags.Orientation &&
                 angleError < Globals.EpsRadians)
        {
            goalReached = true;
        }

        return goalReached;
    }
}