RobustToolbox/Robust.Shared/Map/EntityCoordinates.cs

using System;
using System.Numerics;
using JetBrains.Annotations;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map.Components;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;

namespace Robust.Shared.Map
{
    /// <summary>
    ///     A set of coordinates relative to another entity.
    /// </summary>
    [PublicAPI]
    public readonly struct EntityCoordinates : IEquatable<EntityCoordinates>, ISpanFormattable
    {
        public static readonly EntityCoordinates Invalid = new(EntityUid.Invalid, Vector2.Zero);

        /// <summary>
        ///     ID of the entity that this position is relative to.
        /// </summary>
        public readonly EntityUid EntityId;

        /// <summary>
        ///     Position in the entity's local space.
        /// </summary>
        public readonly Vector2 Position;

        /// <summary>
        ///     Location of the X axis local to the entity.
        /// </summary>
        public float X => Position.X;

        /// <summary>
        ///     Location of the Y axis local to the entity.
        /// </summary>
        public float Y => Position.Y;

        public EntityCoordinates()
        {
            EntityId = EntityUid.Invalid;
            Position = Vector2.Zero;
        }

        /// <summary>
        ///     Constructs a new instance of <see cref="EntityCoordinates"/>.
        /// </summary>
        /// <param name="entityId">ID of the entity that this position is relative to.</param>
        /// <param name="position">Position in the entity's local space.</param>
        public EntityCoordinates(EntityUid entityId, Vector2 position)
        {
            EntityId = entityId;
            Position = position;
        }

        public EntityCoordinates(EntityUid entityId, float x, float y)
        {
            EntityId = entityId;
            Position = new Vector2(x, y);
        }

        /// <summary>
        ///     Verifies that this set of coordinates can be currently resolved to a location.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <returns><see langword="true" /> if this set of coordinates can be currently resolved to a location, otherwise <see langword="false" />.</returns>
        public bool IsValid(IEntityManager entityManager)
        {
            if (!EntityId.IsValid() || !entityManager.EntityExists(EntityId))
                return false;

            if (!float.IsFinite(Position.X) || !float.IsFinite(Position.Y))
                return false;

            return true;
        }

        /// <summary>
        ///     Transforms this set of coordinates from the entity's local space to the map space.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <returns></returns>
        [Obsolete("Use ToMap() with TransformSystem overload")]
        public MapCoordinates ToMap(IEntityManager entityManager)
        {
            return ToMap(entityManager, entityManager.System<SharedTransformSystem>());
        }

        /// <summary>
        ///     Transforms this set of coordinates from the entity's local space to the map space.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <param name="transformSystem">Shared transform system for doing calculations.</param>
        public MapCoordinates ToMap(IEntityManager entityManager, SharedTransformSystem transformSystem)
        {
            if(!IsValid(entityManager))
                return MapCoordinates.Nullspace;

            var transform = entityManager.GetComponent<TransformComponent>(EntityId);
            var worldPos = transformSystem.GetWorldMatrix(transform).Transform(Position);
            return new MapCoordinates(worldPos, transform.MapID);
        }

        /// <summary>
        ///    Transform this set of coordinates from the entity's local space to the map space.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <returns></returns>
        [Obsolete("Use ToMapPos() with TransformSystem overload")]
        public Vector2 ToMapPos(IEntityManager entityManager)
        {
            return ToMap(entityManager, entityManager.System<SharedTransformSystem>()).Position;
        }

        /// <summary>
        ///    Transform this set of coordinates from the entity's local space to the map space.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <param name="transformSystem">Shared transform system for doing calculations.</param>
        public Vector2 ToMapPos(IEntityManager entityManager, SharedTransformSystem transformSystem)
        {
            return ToMap(entityManager, transformSystem).Position;
        }

        /// <summary>
        ///    Creates EntityCoordinates given an entity and some MapCoordinates.
        /// </summary>
        /// <exception cref="InvalidOperationException">If <see cref="entity"/> is not on the same map as the <see cref="coordinates"/>.</exception>
        [Obsolete("Use FromMap() with TransformSystem overload")]
        public static EntityCoordinates FromMap(EntityUid entity, MapCoordinates coordinates, IEntityManager? entMan = null)
        {
            IoCManager.Resolve(ref entMan);
            return FromMap(entity, coordinates, entMan.System<SharedTransformSystem>(), entMan);
        }

        /// <summary>
        ///    Creates EntityCoordinates given an entity and some MapCoordinates.
        /// </summary>
        /// <exception cref="InvalidOperationException">If <see cref="entity"/> is not on the same map as the <see cref="coordinates"/>.</exception>
        public static EntityCoordinates FromMap(EntityUid entity, MapCoordinates coordinates, SharedTransformSystem transformSystem, IEntityManager? entMan = null)
        {
            IoCManager.Resolve(ref entMan);
            var transform = entMan.GetComponent<TransformComponent>(entity);
            if(transform.MapID != coordinates.MapId)
                throw new InvalidOperationException("Entity is not on the same map!");

            var localPos = transformSystem.GetInvWorldMatrix(transform).Transform(coordinates.Position);
            return new EntityCoordinates(entity, localPos);
        }

        /// <summary>
        ///    Creates EntityCoordinates given an entity Uid and some MapCoordinates.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the entity Id.</param>
        /// <param name="entityUid"></param>
        /// <param name="coordinates"></param>
        /// <returns></returns>
        /// <exception cref="InvalidOperationException">If <see cref="entityUid"/> is not on the same map as the <see cref="coordinates"/>.</exception>
        [Obsolete("Use overload with other parameter order.")]
        public static EntityCoordinates FromMap(IEntityManager entityManager, EntityUid entityUid, MapCoordinates coordinates)
        {
            return FromMap(entityUid, coordinates, entityManager.System<SharedTransformSystem>(), entityManager);
        }

        /// <summary>
        ///    Creates a set of EntityCoordinates given some MapCoordinates.
        /// </summary>
        /// <param name="mapManager"></param>
        /// <param name="coordinates"></param>
        public static EntityCoordinates FromMap(IMapManager mapManager, MapCoordinates coordinates)
        {
            var mapId = coordinates.MapId;
            var mapEntity = mapManager.GetMapEntityId(mapId);

            return new EntityCoordinates(mapEntity, coordinates.Position);
        }

        /// <summary>
        ///     Converts this set of coordinates to Vector2i.
        /// </summary>
        [Obsolete("Use overload with TransformSystem")]
        public Vector2i ToVector2i(IEntityManager entityManager, IMapManager mapManager)
        {
            return ToVector2i(entityManager, mapManager, entityManager.System<SharedTransformSystem>());
        }

        /// <summary>
        ///     Converts this set of coordinates to Vector2i.
        /// </summary>
        public Vector2i ToVector2i(
            IEntityManager entityManager,
            IMapManager mapManager,
            SharedTransformSystem transformSystem)
        {
            if(!IsValid(entityManager))
                return new Vector2i();

            var mapSystem = entityManager.System<SharedMapSystem>();
            var gridIdOpt = GetGridUid(entityManager);
            if (gridIdOpt is { } gridId && gridId.IsValid())
            {
                var grid = entityManager.GetComponent<MapGridComponent>(gridId);
                return mapSystem.GetTileRef(gridId, grid, this).GridIndices;
            }

            var vec = ToMapPos(entityManager, transformSystem);

            return new Vector2i((int)MathF.Floor(vec.X), (int)MathF.Floor(vec.Y));
        }

        /// <summary>
        ///     Returns an new set of EntityCoordinates with the same <see cref="EntityId"/>
        ///     but on a different position.
        /// </summary>
        /// <param name="newPosition">The position the new EntityCoordinates will be in</param>
        /// <returns>A new set of EntityCoordinates with the specified position and same <see cref="EntityId"/> as this one.</returns>
        public EntityCoordinates WithPosition(Vector2 newPosition)
        {
            return new(EntityId, newPosition);
        }

        /// <summary>
        ///     Returns a new set of EntityCoordinates local to a new entity.
        /// </summary>
        /// <param name="entityManager">The Entity Manager holding this entity</param>
        /// <param name="entityId">The entity that the new coordinates will be local to</param>
        /// <returns>A new set of EntityCoordinates local to a new entity.</returns>
        public EntityCoordinates WithEntityId(IEntityManager entityManager, EntityUid entityId)
        {
            if(!entityManager.EntityExists(entityId))
                return new EntityCoordinates(entityId, Vector2.Zero);

            return WithEntityId(entityId);
        }

        /// <summary>
        ///     Returns a new set of EntityCoordinates local to a new entity.
        /// </summary>
        /// <param name="entity">The entity that the new coordinates will be local to</param>
        /// <returns>A new set of EntityCoordinates local to a new entity.</returns>
        public EntityCoordinates WithEntityId(EntityUid entity, IEntityManager? entMan = null)
        {
            IoCManager.Resolve(ref entMan);
            return WithEntityId(entity, entMan.System<SharedTransformSystem>(), entMan);
        }

        /// <summary>
        ///     Returns a new set of EntityCoordinates local to a new entity.
        /// </summary>
        /// <param name="entity">The entity that the new coordinates will be local to</param>
        /// <returns>A new set of EntityCoordinates local to a new entity.</returns>
        public EntityCoordinates WithEntityId(
            EntityUid entity,
            SharedTransformSystem transformSystem,
            IEntityManager? entMan = null)
        {
            IoCManager.Resolve(ref entMan);
            var mapPos = ToMap(entMan, transformSystem);

            if(!IsValid(entMan) || entMan.GetComponent<TransformComponent>(entity).MapID != mapPos.MapId)
                return new EntityCoordinates(entity, Vector2.Zero);

            var localPos = transformSystem.GetInvWorldMatrix(entity).Transform(mapPos.Position);
            return new EntityCoordinates(entity, localPos);
        }

        /// <summary>
        ///     Returns the Grid EntityUid these coordinates are on.
        ///     If none of the ancestors are a grid, returns null instead.
        /// </summary>
        /// <param name="entityManager"></param>
        /// <returns>Grid EntityUid this entity is on or null</returns>
        public EntityUid? GetGridUid(IEntityManager entityManager)
        {
            return !IsValid(entityManager) ? null : entityManager.GetComponent<TransformComponent>(EntityId).GridUid;
        }

        /// <summary>
        ///     Returns the Map Id these coordinates are on.
        ///     If the relative entity is not valid, returns <see cref="MapId.Nullspace"/> instead.
        /// </summary>
        /// <param name="entityManager"></param>
        /// <returns>Map Id these coordinates are on or <see cref="MapId.Nullspace"/></returns>
        public MapId GetMapId(IEntityManager entityManager)
        {
            return !IsValid(entityManager) ? MapId.Nullspace : entityManager.GetComponent<TransformComponent>(EntityId).MapID;
        }

        /// <summary>
        ///     Returns the Map Id these coordinates are on.
        ///     If the relative entity is not valid, returns null instead.
        /// </summary>
        /// <param name="entityManager"></param>
        /// <returns>Map Id these coordinates are on or null</returns>
        public EntityUid? GetMapUid(IEntityManager entityManager)
        {
            return !IsValid(entityManager) ? null : entityManager.GetComponent<TransformComponent>(EntityId).MapUid;
        }

        /// <summary>
        /// Offsets the position by a given vector. This happens in local space.
        /// </summary>
        /// <param name="position">The vector to offset by local to the entity.</param>
        /// <returns>Newly offset coordinates.</returns>
        public EntityCoordinates Offset(Vector2 position)
        {
            return new(EntityId, Position + position);
        }

        /// <summary>
        ///     Compares two sets of coordinates to see if they are in range of each other.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the two entity Ids.</param>
        /// <param name="otherCoordinates">Other set of coordinates to use.</param>
        /// <param name="range">maximum distance between the two sets of coordinates.</param>
        /// <returns>True if the two points are within a given range.</returns>
        [Obsolete("Use overload with TransformSystem")]
        public bool InRange(IEntityManager entityManager, EntityCoordinates otherCoordinates, float range)
        {
            return InRange(entityManager, entityManager.System<SharedTransformSystem>(), otherCoordinates, range);
        }

        /// <summary>
        ///     Compares two sets of coordinates to see if they are in range of each other.
        /// </summary>
        /// <param name="entityManager">Entity Manager containing the two entity Ids.</param>
        /// <param name="otherCoordinates">Other set of coordinates to use.</param>
        /// <param name="range">maximum distance between the two sets of coordinates.</param>
        /// <returns>True if the two points are within a given range.</returns>
        public bool InRange(
            IEntityManager entityManager,
            SharedTransformSystem transformSystem,
            EntityCoordinates otherCoordinates,
            float range)
        {
            if (!IsValid(entityManager) || !otherCoordinates.IsValid(entityManager))
                return false;

            if (EntityId == otherCoordinates.EntityId)
                return (otherCoordinates.Position - Position).LengthSquared() < range * range;

            var mapCoordinates = ToMap(entityManager, transformSystem);
            var otherMapCoordinates = otherCoordinates.ToMap(entityManager, transformSystem);

            if (mapCoordinates.MapId != otherMapCoordinates.MapId)
                return false;

            return mapCoordinates.InRange(otherMapCoordinates, range);
        }

        /// <summary>
        ///     Tries to calculate the distance between two sets of coordinates.
        /// </summary>
        /// <param name="entityManager"></param>
        /// <param name="otherCoordinates"></param>
        /// <param name="distance"></param>
        /// <returns>True if it was possible to calculate the distance</returns>
        public bool TryDistance(IEntityManager entityManager, EntityCoordinates otherCoordinates, out float distance)
        {
            return TryDistance(
                entityManager,
                entityManager.System<SharedTransformSystem>(),
                otherCoordinates,
                out distance);
        }

        /// <summary>
        ///     Tries to calculate the distance between two sets of coordinates.
        /// </summary>
        /// <returns>True if it was possible to calculate the distance</returns>
        public bool TryDistance(
            IEntityManager entityManager,
            SharedTransformSystem transformSystem,
            EntityCoordinates otherCoordinates,
            out float distance)
        {
            if (TryDelta(entityManager, transformSystem, otherCoordinates, out var delta))
            {
                distance = delta.Length();
                return true;
            }

            distance = 0f;
            return false;
        }

        /// <summary>
        ///     Tries to calculate the distance vector between two sets of coordinates.
        /// </summary>
        /// <returns>True if it was possible to calculate the distance</returns>
        public bool TryDelta(
            IEntityManager entityManager,
            SharedTransformSystem transformSystem,
            EntityCoordinates otherCoordinates,
            out Vector2 delta)
        {
            delta = Vector2.Zero;

            if (!IsValid(entityManager) || !otherCoordinates.IsValid(entityManager))
                return false;

            if (EntityId == otherCoordinates.EntityId)
            {
                delta = Position - otherCoordinates.Position;
                return true;
            }

            var mapCoordinates = ToMap(entityManager, transformSystem);
            var otherMapCoordinates = otherCoordinates.ToMap(entityManager, transformSystem);

            if (mapCoordinates.MapId != otherMapCoordinates.MapId)
                return false;

            delta = mapCoordinates.Position - otherMapCoordinates.Position;
            return true;
        }

        #region IEquatable

        /// <inheritdoc />
        public bool Equals(EntityCoordinates other)
        {
            return EntityId.Equals(other.EntityId) && Position.Equals(other.Position);
        }

        /// <inheritdoc />
        public override bool Equals(object? obj)
        {
            return obj is EntityCoordinates other && Equals(other);
        }

        /// <inheritdoc />
        public override int GetHashCode()
        {
            return HashCode.Combine(EntityId, Position);
        }

        /// <summary>
        ///     Check for equality by value between two objects.
        /// </summary>
        public static bool operator ==(EntityCoordinates left, EntityCoordinates right)
        {
            return left.Equals(right);
        }

        /// <summary>
        ///     Check for inequality by value between two objects.
        /// </summary>
        public static bool operator !=(EntityCoordinates left, EntityCoordinates right)
        {
            return !left.Equals(right);
        }

        #endregion

        #region Operators

        /// <summary>
        ///     Returns the sum for both coordinates but only if they have the same relative entity.
        /// </summary>
        /// <exception cref="ArgumentException">Thrown when the relative entities aren't the same</exception>
        public static EntityCoordinates operator +(EntityCoordinates left, EntityCoordinates right)
        {
            if(left.EntityId != right.EntityId)
                throw new ArgumentException("Can't sum EntityCoordinates with different relative entities.");

            return new EntityCoordinates(left.EntityId, left.Position + right.Position);
        }

        /// <summary>
        ///     Returns the difference for both coordinates but only if they have the same relative entity.
        /// </summary>
        /// <exception cref="ArgumentException">Thrown when the relative entities aren't the same</exception>
        public static EntityCoordinates operator -(EntityCoordinates left, EntityCoordinates right)
        {
            if(left.EntityId != right.EntityId)
                throw new ArgumentException("Can't subtract EntityCoordinates with different relative entities.");

            return new EntityCoordinates(left.EntityId, left.Position - right.Position);
        }

        /// <summary>
        ///     Returns the multiplication of both coordinates but only if they have the same relative entity.
        /// </summary>
        /// <exception cref="ArgumentException">When the relative entities aren't the same</exception>
        public static EntityCoordinates operator *(EntityCoordinates left, EntityCoordinates right)
        {
            if(left.EntityId != right.EntityId)
                throw new ArgumentException("Can't multiply EntityCoordinates with different relative entities.");

            return new EntityCoordinates(left.EntityId, left.Position * right.Position);
        }

        /// <summary>
        ///     Scales the coordinates by a given factor.
        /// </summary>
        /// <exception cref="ArgumentException">When the relative entities aren't the same</exception>
        public static EntityCoordinates operator *(EntityCoordinates left, float right)
        {
            return new(left.EntityId, left.Position * right);
        }

        /// <summary>
        ///     Scales the coordinates by a given factor.
        /// </summary>
        /// <exception cref="ArgumentException">When the relative entities aren't the same</exception>
        public static EntityCoordinates operator *(EntityCoordinates left, int right)
        {
            return new(left.EntityId, left.Position * right);
        }

        #endregion

        /// <summary>
        /// Deconstructs the object into it's fields.
        /// </summary>
        /// <param name="entId">ID of the entity that this position is relative to.</param>
        /// <param name="localPos">Position in the entity's local space.</param>
        public void Deconstruct(out EntityUid entId, out Vector2 localPos)
        {
            entId = EntityId;
            localPos = Position;
        }

        /// <inheritdoc />
        public override string ToString()
        {
            return $"EntId={EntityId}, X={Position.X:N2}, Y={Position.Y:N2}";
        }

        public string ToString(string? format, IFormatProvider? formatProvider) => ToString();

        public bool TryFormat(
            Span<char> destination,
            out int charsWritten,
            ReadOnlySpan<char> format,
            IFormatProvider? provider)
        {
            return FormatHelpers.TryFormatInto(
            destination,
            out charsWritten,
            $"EntId={EntityId}, X={Position.X:N2}, Y={Position.Y:N2}");
        }
    }
}