#pragma once
#include "geometry/Vec2.h"
#include <cmath>

namespace MC {

// Converts real-world mm coordinates to the integer grid used by plan.xml.
//
// The controller uses a 32px-per-cell abstract grid. Each cell corresponds
// to one "standard straight" unit of track spacing. For Märklin C/K HO,
// we define 1 cell = 32mm (a reasonable approximation that keeps the grid
// manageable; adjust cellSizeMm if your layout uses different spacing).
//
// Usage:
//   GridQuantizer q(32.0);            // 32mm per cell
//   auto [gx, gy] = q.ToGrid({188.3, 0.0});  // -> {6, 0}
//   Vec2 mm = q.ToMm({6, 0});         // -> {192.0, 0.0}  (nearest cell centre)

struct GridCell {
    int x = 0;
    int y = 0;
};

class GridQuantizer {
public:
    explicit GridQuantizer(double cellSizeMm = 32.0)
        : m_cellSize(cellSizeMm) {}

    // mm -> nearest integer grid cell
    GridCell ToGrid(const Vec2& mm) const {
        return {
            static_cast<int>(std::round(mm.x / m_cellSize)),
            static_cast<int>(std::round(mm.y / m_cellSize))
        };
    }

    // integer grid cell -> mm (cell centre)
    Vec2 ToMm(const GridCell& cell) const {
        return { cell.x * m_cellSize, cell.y * m_cellSize };
    }

    // Quantize a heading (degrees) to the nearest compass direction
    // plan.xml uses: east=0°, north=90°, west=180°, south=270°
    static std::string QuantizeOrientation(double headingDeg) {
        // Snap to nearest 90°
        int snapped = static_cast<int>(
            std::round(Transform2D::normalizeAngle(headingDeg) / 90.0)) % 4;
        switch (snapped) {
            case 0: return "east";
            case 1: return "north";
            case 2: return "west";
            case 3: return "south";
            default: return "east";
        }
    }

    double CellSize() const { return m_cellSize; }

private:
    double m_cellSize;
};

} // namespace MC
