using Godot;

namespace WorldSim;

/// <summary>
/// The world is a torus — a boundless globe with no edges or corners. Walk
/// off the right and you reappear on the left; off the bottom, the top. Every
/// position comparison in the sim must account for the seam: the shortest path
/// between two points may cross an edge. All distance/direction/movement math
/// routes through here so the whole world agrees the map wraps.
///
/// The wrap size is World.GridSize (positions live in world units, same space
/// as node cells and NPC positions).
/// </summary>
public static class Toroidal
{
    public static float Size => World.GridSize;

    /// <summary>Wrap a scalar coordinate into [0, Size).</summary>
    public static float WrapCoord(float v)
    {
        float s = Size;
        v %= s;
        return v < 0f ? v + s : v;
    }

    /// <summary>Wrap a position so it lives inside the world.</summary>
    public static Vector2 Wrap(Vector2 p) => new(WrapCoord(p.X), WrapCoord(p.Y));

    /// <summary>
    /// The shortest displacement FROM a TO b across the torus — each axis takes
    /// whichever way (direct or across the seam) is nearer. Result components
    /// are in [-Size/2, Size/2]. Use this for direction and distance.
    /// </summary>
    public static Vector2 Delta(Vector2 a, Vector2 b)
    {
        float s = Size, half = s * 0.5f;
        float dx = b.X - a.X, dy = b.Y - a.Y;
        if (dx >  half) dx -= s; else if (dx < -half) dx += s;
        if (dy >  half) dy -= s; else if (dy < -half) dy += s;
        return new Vector2(dx, dy);
    }

    /// <summary>Shortest distance between two points on the torus.</summary>
    public static float Distance(Vector2 a, Vector2 b) => Delta(a, b).Length();

    /// <summary>Shortest squared distance — for cheap radius comparisons.</summary>
    public static float DistanceSquared(Vector2 a, Vector2 b) => Delta(a, b).LengthSquared();

    /// <summary>Move `from` toward `target` by `step`, taking the seam-shortest
    /// path and wrapping the result back into the world.</summary>
    public static Vector2 MoveToward(Vector2 from, Vector2 target, float step)
    {
        Vector2 d = Delta(from, target);
        float len = d.Length();
        if (len <= step || len < 0.0001f) return Wrap(target);
        return Wrap(from + d / len * step);
    }
}