/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using CartographerSharp.Common.Math;
using CartographerSharp.Models.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D2D;

/// <summary>
/// Defines the limits of a grid map.
/// </summary>
public class MapLimits
{
    private readonly double _resolution;
    private readonly Vector2 _max;
    private readonly CellLimits _cellLimits;

    public MapLimits(double resolution, Vector2 max, CellLimits cellLimits)
    {
        if (resolution <= 0.0)
        {
            throw new ArgumentException("Resolution must be positive", nameof(resolution));
        }
        if (cellLimits.NumXCells <= 0 || cellLimits.NumYCells <= 0)
        {
            throw new ArgumentException("Cell limits must be positive", nameof(cellLimits));
        }

        _resolution = resolution;
        _max = max;
        _cellLimits = cellLimits;
    }

    /// <summary>
    /// Creates from proto representation.
    /// Match C++: explicit MapLimits(const proto::MapLimits& map_limits)
    /// </summary>
    public MapLimits(Models.Mapping.MapLimits proto)
    {
        _resolution = proto.Resolution;
        _max = new Vector2(proto.Max.X, proto.Max.Y);
        // Match C++: cell_limits_(map_limits.cell_limits())
        _cellLimits = new CellLimits(proto.CellLimits);
    }

    /// <summary>
    /// Returns the cell size in meters. All cells are square and the resolution is
    /// the length of one side.
    /// </summary>
    public double Resolution => _resolution;

    /// <summary>
    /// Returns the corner of the limits, i.e., all pixels have positions with
    /// smaller coordinates.
    /// </summary>
    public Vector2 Max => _max;

    /// <summary>
    /// Returns the limits of the grid in number of cells.
    /// </summary>
    public CellLimits CellLimits => _cellLimits;

    /// <summary>
    /// Returns the index of the cell containing the 'point' which may be outside
    /// the map, i.e., negative or too large indices that will return false for
    /// Contains().
    /// </summary>
    public Array2i GetCellIndex(Vector2 point)
    {
        // Index values are row major and the top left has (0, 0)
        // and contains (centered_max_x, centered_max_y). We need to flip and rotate.
        // Match C++: common::RoundToInt(x) = std::lround(x) - rounds to nearest long, then casts to int
        // std::lround rounds to nearest integer using current rounding mode (default: round half away from zero)
        // In C#, Math.Round with MidpointRounding.AwayFromZero matches std::lround behavior
        // CRITICAL: Use std::lround equivalent - Math.Round with MidpointRounding.AwayFromZero
        // But std::lround actually uses "round half to even" (banker's rounding) by default in C++11+
        // However, C++ code uses common::RoundToInt which is std::lround, and std::lround uses current rounding mode
        // In practice, std::lround with default rounding mode rounds half away from zero
        // So Math.Round with MidpointRounding.AwayFromZero should match
        var x = (int)Math.Round((_max.Y - point.Y) / _resolution - 0.5, MidpointRounding.AwayFromZero);
        var y = (int)Math.Round((_max.X - point.X) / _resolution - 0.5, MidpointRounding.AwayFromZero);
        return new Array2i(x, y);
    }

    /// <summary>
    /// Returns the center of the cell at 'cell_index'.
    /// </summary>
    public Vector2 GetCellCenter(Array2i cellIndex)
    {
        return new Vector2(
            (_max.X - Resolution * (cellIndex.Y + 0.5)),
            (_max.Y - Resolution * (cellIndex.X + 0.5))
        );
    }

    /// <summary>
    /// Returns true if the grid contains 'cell_index'.
    /// </summary>
    public bool Contains(Array2i cellIndex)
    {
        return cellIndex.X >= 0 && cellIndex.Y >= 0 &&
               cellIndex.X < _cellLimits.NumXCells &&
               cellIndex.Y < _cellLimits.NumYCells;
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public Models.Mapping.MapLimits ToProto()
    {
        var cellLimitsProto = _cellLimits.ToProto();
        return new Models.Mapping.MapLimits(
            _resolution,
            new Vector2d(_max.X, _max.Y),
            new Models.Mapping.CellLimits(cellLimitsProto.NumXCells, cellLimitsProto.NumYCells)
        );
    }
}