using RobotNet10.Shared.Geometry;

namespace RobotNet10.Shared.Localization;

/// <summary>
/// Occupancy grid for representing map as a 2D grid of cells
/// Compatible with ROS navigation_msgs/OccupancyGrid
/// </summary>
public class OccupancyGrid
{
    /// <summary>
    /// Resolution of the grid (meters per pixel)
    /// </summary>
    public double Resolution { get; set; }

    /// <summary>
    /// Width of the grid (number of cells)
    /// </summary>
    public int Width { get; set; }

    /// <summary>
    /// Height of the grid (number of cells)
    /// </summary>
    public int Height { get; set; }

    /// <summary>
    /// Origin of the grid in map frame (bottom-left corner)
    /// </summary>
    public Pose Origin { get; set; }

    /// <summary>
    /// Grid data: -1 = Unknown, 0-100 = Probability of occupancy (0 = free, 100 = occupied)
    /// Stored in row-major order (index = y * width + x)
    /// </summary>
    public sbyte[] Data { get; set; }

    /// <summary>
    /// Creates a new empty occupancy grid
    /// </summary>
    public OccupancyGrid()
    {
        Resolution = 0.05;
        Width = 0;
        Height = 0;
        Origin = new Pose();
        Data = [];
    }

    /// <summary>
    /// Creates a new occupancy grid with specified dimensions
    /// </summary>
    public OccupancyGrid(double resolution, int width, int height, Pose origin)
    {
        Resolution = resolution;
        Width = width;
        Height = height;
        Origin = origin;
        Data = new sbyte[width * height];
        
        // Initialize all cells as unknown (-1)
        Array.Fill(Data, (sbyte)-1);
    }

    /// <summary>
    /// Gets the occupancy value at the specified cell coordinates
    /// </summary>
    /// <param name="x">X coordinate (0 to Width-1)</param>
    /// <param name="y">Y coordinate (0 to Height-1)</param>
    /// <returns>-1 = Unknown, 0-100 = Occupancy probability</returns>
    public sbyte GetCell(int x, int y)
    {
        if (x < 0 || x >= Width || y < 0 || y >= Height)
        {
            return -1; // Out of bounds = unknown
        }
        return Data[y * Width + x];
    }

    /// <summary>
    /// Sets the occupancy value at the specified cell coordinates
    /// </summary>
    /// <param name="x">X coordinate (0 to Width-1)</param>
    /// <param name="y">Y coordinate (0 to Height-1)</param>
    /// <param name="value">-1 = Unknown, 0-100 = Occupancy probability</param>
    public void SetCell(int x, int y, sbyte value)
    {
        if (x < 0 || x >= Width || y < 0 || y >= Height)
        {
            return; // Out of bounds, ignore
        }
        Data[y * Width + x] = value;
    }

    /// <summary>
    /// Converts world coordinates (meters) to grid cell coordinates
    /// </summary>
    /// <param name="worldX">X coordinate in meters</param>
    /// <param name="worldY">Y coordinate in meters</param>
    /// <returns>Cell coordinates (x, y)</returns>
    public (int x, int y) WorldToGrid(double worldX, double worldY)
    {
        // Convert world coordinates relative to origin
        var relativeX = worldX - Origin.Position.X;
        var relativeY = worldY - Origin.Position.Y;

        // Convert to grid coordinates
        var gridX = (int)Math.Floor(relativeX / Resolution);
        var gridY = (int)Math.Floor(relativeY / Resolution);

        return (gridX, gridY);
    }

    /// <summary>
    /// Converts grid cell coordinates to world coordinates (meters)
    /// </summary>
    /// <param name="gridX">X coordinate in grid cells</param>
    /// <param name="gridY">Y coordinate in grid cells</param>
    /// <returns>World coordinates (x, y) in meters</returns>
    public (double x, double y) GridToWorld(int gridX, int gridY)
    {
        var worldX = Origin.Position.X + (gridX * Resolution);
        var worldY = Origin.Position.Y + (gridY * Resolution);
        return (worldX, worldY);
    }

    /// <summary>
    /// Gets the bounds of the grid in world coordinates
    /// </summary>
    public BoundingBox GetBounds()
    {
        var (minX, minY) = GridToWorld(0, 0);
        var (maxX, maxY) = GridToWorld(Width, Height);
        return new BoundingBox
        {
            MinX = Math.Min(minX, maxX),
            MinY = Math.Min(minY, maxY),
            MaxX = Math.Max(minX, maxX),
            MaxY = Math.Max(minY, maxY)
        };
    }
}

/// <summary>
/// Bounding box for representing map bounds
/// </summary>
public class BoundingBox
{
    public double MinX { get; set; }
    public double MinY { get; set; }
    public double MaxX { get; set; }
    public double MaxY { get; set; }
}