#pragma once

#include "car_nav_lite/types.hpp"
#include <string>
#include <array>

namespace car_nav_lite {

// ── 8-connected neighbor deltas ──────────────────────────
constexpr std::array<std::pair<int,int>, 8> NEIGHBORS = {{
    {1,0},{-1,0},{0,1},{0,-1},{1,1},{-1,-1},{1,-1},{-1,1}
}};

class GridMap {
public:
    GridMap();

    // ── Load PGM P5 from file ──────────────────────────
    bool loadPGM(const std::string& path);

    // ── Coordinate transforms ──────────────────────────
    bool worldToGrid(double wx, double wy, int& gx, int& gy) const;
    void gridToWorld(int gx, int gy, double& wx, double& wy) const;

    // ── Access ──────────────────────────────────────────
    uint8_t at(int gx, int gy) const;
    void set(int gx, int gy, uint8_t v);
    bool inBounds(int gx, int gy) const;
    bool isFree(int gx, int gy) const;
    bool isFree(int gx, int gy, int inflate_cells) const;  // configurable inflation

    // ── Raycast on static obstacles (for scan-to-map matching) ──
    // Returns range to first obstacle >128 along (wx,wy) → angle.
    double raycast(double wx, double wy, double angle, double max_range = 5.0) const;

    // ── Oriented footprint collision check (EXACT-MPPI style) ──
    // Checks raw occupancy against car's oriented rectangle at (wx,wy,yaw).
    // Handles car width/length correctly regardless of orientation.
    bool isFreeFootprint(double wx, double wy, double yaw) const;

    // ── Obstacle update from scan ──────────────────────
    void updateScan(const LaserScan& scan, const Pose2D& pose);

    // ── Morphological close (remove noise, merge nearby blobs) ──
    void morphologyClose(int kernel_size = 3);

    // ── Reset dynamic obstacles to static PGM layer ──
    void resetDynamic();

    // ── Pre-built obstacle marks ───────────────────────
    void markCircle(double wx, double wy, double radius, uint8_t value = 255);
    void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255);

    // ── Getters ────────────────────────────────────────
    int width()  const { return GRID_SIZE; }
    int height() const { return GRID_SIZE; }
    const OccupancyGrid& data() const { return cells_; }

private:
    OccupancyGrid cells_;
    OccupancyGrid static_cells_;  // snapshot after PGM load
    void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val);
};

}  // namespace car_nav_lite