/*
 * The cell grid: a counting sort of every thing into the cells its footprint
 * covers (one array of starts, one of items).
 */
#include "world/grid.h"

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "world/terrain.h"

#define GRID_MARGIN 0.6f /* (things move a little within a frame) */

static int cellOf(float v, int n) {
    int c = (int)floorf((v + TERRAIN_SIZE * 0.5f) / NEAR_CELL);
    return c < 0 ? 0 : c >= n ? n - 1 : c;
}

/* The thing's footprint on the ground (x0, z0, x1, z1); false if it has none. */
static bool footprint(const Object *o, float *x0, float *z0, float *x1, float *z1) {
    if (o->collider_count == 0) return false;
    *x0 = *z0 = 1e9f;
    *x1 = *z1 = -1e9f;
    for (int c = 0; c < o->collider_count; c++) {
        const Collider *col = &o->colliders[c];
        Vec3 m = Object_colliderCenter(o, c);
        const float h[3] = { col->half_extents.x, col->half_extents.y, col->half_extents.z };
        float ex = 0.0f, ez = 0.0f;
        for (int k = 0; k < 3; k++) {
            ex += fabsf(col->axes[k].x) * h[k];
            ez += fabsf(col->axes[k].z) * h[k];
        }
        *x0 = fminf(*x0, m.x - ex); *x1 = fmaxf(*x1, m.x + ex);
        *z0 = fminf(*z0, m.z - ez); *z1 = fmaxf(*z1, m.z + ez);
    }
    return true;
}

void Grid_build(Grid *g, Object *const *objects, int count) {
    if (!g->start) {
        g->n = (int)(TERRAIN_SIZE / NEAR_CELL);
        g->start = calloc((size_t)g->n * g->n + 1, sizeof *g->start);
        if (!g->start) return;
    }
    const int n = g->n, cells = n * n;
    memset(g->start, 0, (size_t)(cells + 1) * sizeof *g->start);
    /* count, then place */
    int total = 0;
    for (int pass = 0; pass < 2; pass++) {
        if (pass == 1) {
            if (total > g->items_cap) {
                int *grown = realloc(g->items, (size_t)total * 2 * sizeof *grown);
                if (!grown) { g->built_count = -1; return; }
                g->items = grown;
                g->items_cap = total * 2;
            }
            int run = 0; /* starts from the counts */
            for (int k = 0; k < cells; k++) { int c = g->start[k]; g->start[k] = run; run += c; }
            g->start[cells] = run;
        }
        for (int i = 0; i < count; i++) {
            float x0, z0, x1, z1;
            if (!footprint(objects[i], &x0, &z0, &x1, &z1)) continue;
            int cx0 = cellOf(x0 - GRID_MARGIN, n), cx1 = cellOf(x1 + GRID_MARGIN, n);
            int cz0 = cellOf(z0 - GRID_MARGIN, n), cz1 = cellOf(z1 + GRID_MARGIN, n);
            for (int cz = cz0; cz <= cz1; cz++)
                for (int cx = cx0; cx <= cx1; cx++) {
                    if (pass == 0) { g->start[cz * n + cx]++; total++; }
                    else g->items[g->start[cz * n + cx]++] = i;
                }
        }
    }
    /* (placing moved every start on to the next cell's: back one cell) */
    for (int k = cells; k > 0; k--) g->start[k] = g->start[k - 1];
    g->start[0] = 0;
    if (count > g->seen_cap) {
        unsigned *grown = realloc(g->seen, (size_t)count * 2 * sizeof *grown);
        if (!grown) { g->built_count = -1; return; }
        memset(grown + g->seen_cap, 0, (size_t)(count * 2 - g->seen_cap) * sizeof *grown);
        g->seen = grown;
        g->seen_cap = count * 2;
    }
    g->built_count = count;
}

void Grid_free(Grid *g) {
    free(g->start);
    free(g->items);
    free(g->seen);
    memset(g, 0, sizeof(*g));
}

int Grid_near(Grid *g, int count, float x, float z, float r, int *out, int max) {
    if (!g->start || g->built_count != count) return -1;
    const int n = g->n;
    if (++g->stamp == 0) { memset(g->seen, 0, (size_t)g->seen_cap * sizeof *g->seen); g->stamp = 1; }
    int cx0 = cellOf(x - r, n), cx1 = cellOf(x + r, n), cz0 = cellOf(z - r, n), cz1 = cellOf(z + r, n);
    int got = 0;
    for (int cz = cz0; cz <= cz1; cz++)
        for (int cx = cx0; cx <= cx1; cx++) {
            int k = cz * n + cx;
            for (int j = g->start[k]; j < g->start[k + 1]; j++) {
                int i = g->items[j];
                if (g->seen[i] == g->stamp) continue;
                g->seen[i] = g->stamp;
                if (got >= max) return -1;
                out[got++] = i;
            }
        }
    return got;
}

int Grid_nearObjects(Grid *g, Object *const *objects, int count, float x, float z, float r, Object **out, int max) {
    int idx[1024];
    if (max > 1024) max = 1024;
    int n = Grid_near(g, count, x, z, r, idx, max);
    if (n < 0) return -1;
    for (int i = 1; i < n; i++) { /* (in the world's order) */
        int v = idx[i], j = i - 1;
        while (j >= 0 && idx[j] > v) { idx[j + 1] = idx[j]; j--; }
        idx[j + 1] = v;
    }
    for (int i = 0; i < n; i++) out[i] = objects[idx[i]];
    return n;
}