#include "graph_spatial_hash.h"

#include <cmath>
#include <cstdlib>
#include <cstring>

// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------

static inline int floor_div(float v, float cell) {
    return static_cast<int>(std::floor(v / cell));
}

static inline float sq(float x) { return x * x; }

// ---------------------------------------------------------------------------
// Snapshot de posiciones (almacenado internamente en build)
// La interfaz del header no pasa xs/ys a los metodos de query, por lo que
// SpatialHash conserva copias internas para calcular distancias.
// ---------------------------------------------------------------------------

struct SpatialHashSnap {
    float* xs    = nullptr;
    float* ys    = nullptr;
    float* sizes = nullptr;
    int    count = 0;
};

// Almacenamos el snapshot en memoria contigua al final del bloque de entries,
// para no añadir campos al header publico. Usamos un puntero opaco en un
// campo reservado. Como el header ya esta fijado, guardamos el snapshot como
// una variable estatica por instancia — aceptable para uso single-threaded
// tipico de ImGui. Para multi-instancia se puede usar un map.
//
// En la practica, la convencion del registry es que SpatialHash se crea una
// vez por frame de ImGui y se usa en el mismo hilo de render.

static SpatialHashSnap g_snap;   // snapshot mas reciente

// ---------------------------------------------------------------------------
// SpatialHash
// ---------------------------------------------------------------------------

int SpatialHash::cell_hash(int cx, int cy) const {
    unsigned int h = static_cast<unsigned int>(cx) * 73856093u
                   ^ static_cast<unsigned int>(cy) * 19349663u;
    return static_cast<int>(h % static_cast<unsigned int>(table_size));
}

SpatialHash::SpatialHash(float cell_size_, int table_size_)
    : cell_size(cell_size_)
    , table_size(table_size_)
    , entry_count(0)
    , entry_capacity(256)
{
    buckets = static_cast<int*>(std::malloc(
        static_cast<std::size_t>(table_size) * sizeof(int)));

    entries = static_cast<int*>(std::malloc(
        static_cast<std::size_t>(entry_capacity) * 2 * sizeof(int)));

    std::memset(buckets, -1,
        static_cast<std::size_t>(table_size) * sizeof(int));
}

SpatialHash::~SpatialHash() {
    std::free(buckets);
    std::free(entries);

    std::free(g_snap.xs);
    std::free(g_snap.ys);
    std::free(g_snap.sizes);
    g_snap = {};
}

void SpatialHash::build(const float* xs, const float* ys, const float* sizes, int count) {
    // --- Limpiar tabla ---
    std::memset(buckets, -1,
        static_cast<std::size_t>(table_size) * sizeof(int));
    entry_count = 0;

    // --- Snapshot de posiciones para queries ---
    if (g_snap.count < count) {
        std::free(g_snap.xs);
        std::free(g_snap.ys);
        std::free(g_snap.sizes);
        g_snap.xs    = static_cast<float*>(std::malloc(static_cast<std::size_t>(count) * sizeof(float)));
        g_snap.ys    = static_cast<float*>(std::malloc(static_cast<std::size_t>(count) * sizeof(float)));
        g_snap.sizes = static_cast<float*>(std::malloc(static_cast<std::size_t>(count) * sizeof(float)));
    }
    std::memcpy(g_snap.xs,    xs,    static_cast<std::size_t>(count) * sizeof(float));
    std::memcpy(g_snap.ys,    ys,    static_cast<std::size_t>(count) * sizeof(float));
    std::memcpy(g_snap.sizes, sizes, static_cast<std::size_t>(count) * sizeof(float));
    g_snap.count = count;

    // --- Insertar nodos en la tabla hash ---
    for (int i = 0; i < count; ++i) {
        int cx     = floor_div(xs[i], cell_size);
        int cy     = floor_div(ys[i], cell_size);
        int bucket = cell_hash(cx, cy);

        // Crecer entries si necesario
        if (entry_count >= entry_capacity) {
            entry_capacity *= 2;
            entries = static_cast<int*>(std::realloc(
                entries,
                static_cast<std::size_t>(entry_capacity) * 2 * sizeof(int)));
        }

        // Insertar al frente de la cadena encadenada
        int slot = entry_count++;
        entries[slot * 2 + 0] = i;               // node_index
        entries[slot * 2 + 1] = buckets[bucket];  // next_in_chain
        buckets[bucket] = slot;
    }
}

int SpatialHash::query_nearest(float qx, float qy, float radius, float* out_dist) const {
    int   best_idx = -1;
    float best_d   = radius;  // umbral: solo aceptamos dentro del radio

    int qcx = floor_div(qx, cell_size);
    int qcy = floor_div(qy, cell_size);

    // Escanear vecindad 3x3 de celdas
    for (int dy = -1; dy <= 1; ++dy) {
        for (int dx = -1; dx <= 1; ++dx) {
            int bucket = cell_hash(qcx + dx, qcy + dy);
            int slot   = buckets[bucket];
            while (slot != -1) {
                int ni      = entries[slot * 2 + 0];
                float ex    = g_snap.xs[ni] - qx;
                float ey    = g_snap.ys[ni] - qy;
                float dist  = std::sqrt(sq(ex) + sq(ey)) - g_snap.sizes[ni];
                if (dist < best_d) {
                    best_d   = dist;
                    best_idx = ni;
                }
                slot = entries[slot * 2 + 1];
            }
        }
    }

    if (out_dist && best_idx != -1)
        *out_dist = best_d;

    return best_idx;
}

int SpatialHash::query_radius(float qx, float qy, float radius, int* out, int max_out) const {
    int found = 0;

    int qcx = floor_div(qx, cell_size);
    int qcy = floor_div(qy, cell_size);

    for (int dy = -1; dy <= 1; ++dy) {
        for (int dx = -1; dx <= 1; ++dx) {
            int bucket = cell_hash(qcx + dx, qcy + dy);
            int slot   = buckets[bucket];
            while (slot != -1 && found < max_out) {
                int ni     = entries[slot * 2 + 0];
                float ex   = g_snap.xs[ni] - qx;
                float ey   = g_snap.ys[ni] - qy;
                float dist = std::sqrt(sq(ex) + sq(ey)) - g_snap.sizes[ni];
                if (dist <= radius)
                    out[found++] = ni;
                slot = entries[slot * 2 + 1];
            }
        }
    }

    return found;
}
