/*
 * Walking map: a grid over the site in two levels - where one stands (the
 * ground, or a building's floor or step) and, under a floor, the level below
 * it (a basement). A cell of a level is blocked where something fixed stands
 * between knee and head height over it. Ways are found with A* (8
 * directions, no cutting corners, no more up or down from cell to cell than a
 * step or two, from one level to the other where they meet: stairs) and then
 * straightened: only the turns are kept.
 */
#include "world/nav.h"

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

#define NAV_N         ((int)(TERRAIN_SIZE / NAV_CELL))
#define NAV_MARGIN    0.18f  /* clearance from walls (sideways): an 0.8 m landing or doorway still has a row of free cells */
#define STEP_OVER     0.25f  /* lower than this above the ground: stepped over */
#define HEADROOM      1.8f   /* higher than this: walked under */
#define MOST_STEPS    90000  /* cells looked at for one way, at most */
#define FREE_SEARCH   6      /* cells round a blocked start or end to find a free one */
#define WALK_ON_MOST  1.2f   /* building floors and steps (OBJ_WALL) up to this over the natural ground are walked on ... */
#define NAV_RISE      0.38f  /* ... from one cell to the next at most this much up or down (a stair: 1-2 steps) */
#define BELOW_ROOM    2.0f   /* a level this far under the top one (or more): a second one there (a basement) */
#define NO_FLOOR      -1e9f
#define N2            ((size_t)NAV_N * NAV_N) /* cells in one level; node = level * N2 + cell */

static unsigned char *g_blocked; /* [2 * N2] */
static float         *g_floor;   /* [2 * N2] where one stands: the ground, a floor or step of a building; NO_FLOOR: no lower level */
static bool           g_ready;
static double         g_signature = -1.0;

/* Search state (stamped, so nothing needs clearing between searches) */
static float    *g_cost;
static int      *g_from;
static unsigned *g_seen, *g_done;
static unsigned  g_stamp;
typedef struct { float f; int cell; } HeapItem;
static HeapItem *g_heap;
static int       g_heap_n, g_heap_cap;

static int cellOf(float v) { return (int)floorf((v + TERRAIN_SIZE * 0.5f) / NAV_CELL); }
static float centerOf(int i) { return ((float)i + 0.5f) * NAV_CELL - TERRAIN_SIZE * 0.5f; }
static bool inside(int x, int z) { return x >= 0 && z >= 0 && x < NAV_N && z < NAV_N; }
static int  node(int level, int x, int z) { return (int)((size_t)level * N2) + z * NAV_N + x; }
static bool openAt(int level, int x, int z) { return inside(x, z) && !g_blocked[node(level, x, z)]; }
static bool open(int x, int z) { return openAt(0, x, z) || openAt(1, x, z); }

/* Point in the collider, with clearance sideways only (not up and down). */
static bool inCollider(const Object *o, int i, Vec3 p) {
    const Collider *c = &o->colliders[i];
    Vec3 rel = vec3_sub(p, Object_colliderCenter(o, i));
    const float half[3] = { c->half_extents.x, c->half_extents.y, c->half_extents.z };
    for (int k = 0; k < 3; k++) {
        float level = sqrtf(fmaxf(0.0f, 1.0f - c->axes[k].y * c->axes[k].y));
        if (fabsf(vec3_dot(rel, c->axes[k])) > half[k] + NAV_MARGIN * level) return false;
    }
    return true;
}

/* Cells an object's colliders may block (with the clearance), clipped to the map. */
typedef struct { int x0, z0, x1, z1; } CellRect;

static CellRect boundsOf(const Object *o) {
    CellRect r = { NAV_N, NAV_N, -1, -1 };
    for (int i = 0; i < o->collider_count; i++) {
        const Collider *c = &o->colliders[i];
        Vec3 center = Object_colliderCenter(o, i);
        float ex = 0.0f, ez = 0.0f;
        const float half[3] = { c->half_extents.x, c->half_extents.y, c->half_extents.z };
        for (int k = 0; k < 3; k++) {
            ex += fabsf(c->axes[k].x) * half[k];
            ez += fabsf(c->axes[k].z) * half[k];
        }
        int x0 = cellOf(center.x - ex - NAV_MARGIN), x1 = cellOf(center.x + ex + NAV_MARGIN);
        int z0 = cellOf(center.z - ez - NAV_MARGIN), z1 = cellOf(center.z + ez + NAV_MARGIN);
        if (x0 < r.x0) r.x0 = x0;
        if (z0 < r.z0) r.z0 = z0;
        if (x1 > r.x1) r.x1 = x1;
        if (z1 > r.z1) r.z1 = z1;
    }
    if (r.x0 < 0) r.x0 = 0;
    if (r.z0 < 0) r.z0 = 0;
    if (r.x1 >= NAV_N) r.x1 = NAV_N - 1;
    if (r.z1 >= NAV_N) r.z1 = NAV_N - 1;
    return r;
}

static bool overlaps(CellRect a, CellRect b) { return a.x0 <= b.x1 && b.x0 <= a.x1 && a.z0 <= b.z1 && b.z0 <= a.z1; }

/* A building's floor slab or step (level, its top within reach of the natural ground): stood on. */
static bool walkOn(const Object *o, int i, const Terrain *terrain, float *top) {
    if (o->type != OBJ_WALL || o->variant == 3) return false; /* (building parts; not roofs) */
    const Collider *c = &o->colliders[i];
    if (fabsf(c->axes[1].y) < 0.99f || c->half_extents.x < 0.1f || c->half_extents.z < 0.1f) return false;
    Vec3 center = Object_colliderCenter(o, i);
    *top = center.y + c->half_extents.y;
    return *top <= Terrain_naturalHeight(terrain, center.x, center.z) + WALK_ON_MOST;
}

static void raiseFloor(const Object *o, int i, const Terrain *terrain, CellRect clip, int level) {
    float top;
    if (!walkOn(o, i, terrain, &top)) return;
    const Collider *c = &o->colliders[i];
    Vec3 center = Object_colliderCenter(o, i);
    float ex = fabsf(c->axes[0].x) * c->half_extents.x + fabsf(c->axes[2].x) * c->half_extents.z;
    float ez = fabsf(c->axes[0].z) * c->half_extents.x + fabsf(c->axes[2].z) * c->half_extents.z;
    int x0 = cellOf(center.x - ex), x1 = cellOf(center.x + ex), z0 = cellOf(center.z - ez), z1 = cellOf(center.z + ez);
    if (x0 < clip.x0) x0 = clip.x0;
    if (z0 < clip.z0) z0 = clip.z0;
    if (x1 > clip.x1) x1 = clip.x1;
    if (z1 > clip.z1) z1 = clip.z1;
    for (int z = z0; z <= z1; z++)
        for (int x = x0; x <= x1; x++) {
            Vec3 rel = vec3_sub(vec3(centerOf(x), center.y, centerOf(z)), center);
            if (fabsf(vec3_dot(rel, c->axes[0])) > c->half_extents.x || fabsf(vec3_dot(rel, c->axes[2])) > c->half_extents.z) continue;
            float *f = &g_floor[node(level, x, z)];
            if (level == 1 && top > g_floor[node(0, x, z)] - BELOW_ROOM) continue; /* (the level below: under the top one) */
            if (top > *f) *f = top;
        }
}

static void blockCollider(const Object *o, int i, CellRect clip) {
    const Collider *c = &o->colliders[i];
    Vec3 center = Object_colliderCenter(o, i);
    float ex = 0.0f, ey = 0.0f, ez = 0.0f;
    const float half[3] = { c->half_extents.x, c->half_extents.y, c->half_extents.z };
    for (int k = 0; k < 3; k++) {
        ex += fabsf(c->axes[k].x) * half[k];
        ey += fabsf(c->axes[k].y) * half[k];
        ez += fabsf(c->axes[k].z) * half[k];
    }
    int x0 = cellOf(center.x - ex - NAV_MARGIN), x1 = cellOf(center.x + ex + NAV_MARGIN);
    int z0 = cellOf(center.z - ez - NAV_MARGIN), z1 = cellOf(center.z + ez + NAV_MARGIN);
    if (x0 < clip.x0) x0 = clip.x0;
    if (z0 < clip.z0) z0 = clip.z0;
    if (x1 > clip.x1) x1 = clip.x1;
    if (z1 > clip.z1) z1 = clip.z1;
    for (int level = 0; level < 2; level++)
        for (int z = z0; z <= z1; z++)
            for (int x = x0; x <= x1; x++) {
                int n = node(level, x, z);
                if (g_blocked[n]) continue;
                float px = centerOf(x), pz = centerOf(z);
                float ground = g_floor[n];
                if (center.y + ey < ground + STEP_OVER || center.y - ey > ground + HEADROOM) continue;
                for (int k = 0; k < 3; k++)
                    if (inCollider(o, i, vec3(px, ground + 0.4f + 0.6f * (float)k, pz))) {
                        g_blocked[n] = 1;
                        break;
                    }
            }
}

/* What each fixed thing was at the last build (by pointer, open addressing):
   when something changes only the cells round it are done again. */
typedef struct { const Object *o; float x, z, yaw; int colliders; CellRect cells; unsigned seen; } NavEntry;
#define NAV_TABLE 16384
#define NAV_DIRTY 64   /* more changes at once than this: the whole map again */
static NavEntry *g_table, *g_table_new;
static unsigned  g_build;

static NavEntry *slotFor(NavEntry *table, const Object *o, bool *found) {
    size_t h = ((size_t)o >> 4) * 2654435761u;
    for (size_t k = 0; k < NAV_TABLE; k++) {
        NavEntry *e = &table[(h + k) & (NAV_TABLE - 1)];
        if (e->o == o) { *found = true; return e; }
        if (!e->o) { *found = false; return e; }
    }
    return NULL;
}

/* Clears the cells in r and blocks them again from every fixed thing over them. */
static void redo(Object *const *objects, int count, const Terrain *terrain, CellRect r) {
    for (int z = r.z0; z <= r.z1; z++) {
        memset(&g_blocked[node(0, r.x0, z)], 0, (size_t)(r.x1 - r.x0 + 1));
        memset(&g_blocked[node(1, r.x0, z)], 0, (size_t)(r.x1 - r.x0 + 1));
        for (int x = r.x0; x <= r.x1; x++) g_floor[node(0, x, z)] = Terrain_height(terrain, centerOf(x), centerOf(z));
    }
    for (int i = 0; i < count; i++) { /* floors and steps first: what's in the way is from there up */
        const Object *o = objects[i];
        if (!o->is_static || o->type != OBJ_WALL || !overlaps(boundsOf(o), r)) continue;
        for (int c = 0; c < o->collider_count; c++) raiseFloor(o, c, terrain, r, 0);
    }
    /* The level below (a basement under a floor): the ground there, or a step, well under the top one */
    for (int z = r.z0; z <= r.z1; z++)
        for (int x = r.x0; x <= r.x1; x++) {
            float ground = Terrain_height(terrain, centerOf(x), centerOf(z));
            g_floor[node(1, x, z)] = ground <= g_floor[node(0, x, z)] - BELOW_ROOM ? ground : NO_FLOOR;
        }
    for (int i = 0; i < count; i++) {
        const Object *o = objects[i];
        if (!o->is_static || o->type != OBJ_WALL || !overlaps(boundsOf(o), r)) continue;
        for (int c = 0; c < o->collider_count; c++) raiseFloor(o, c, terrain, r, 1);
    }
    for (int z = r.z0; z <= r.z1; z++)
        for (int x = r.x0; x <= r.x1; x++)
            if (g_floor[node(1, x, z)] == NO_FLOOR) g_blocked[node(1, x, z)] = 1;
    for (int i = 0; i < count; i++) {
        const Object *o = objects[i];
        if (!o->is_static || o->collider_count == 0 || !overlaps(boundsOf(o), r)) continue;
        for (int c = 0; c < o->collider_count; c++) blockCollider(o, c, r);
    }
}

void Nav_update(Object *const *objects, int count, const Terrain *terrain) {
    /* What the fixed things are and where: rebuild when that changes */
    double signature = 0.0;
    for (int i = 0; i < count; i++) {
        const Object *o = objects[i];
        if (!o->is_static || o->collider_count == 0) continue;
        signature += 1.0 + (double)o->position.x * 0.731 + (double)o->position.z * 1.379 + (double)o->rotation.y * 0.17 +
                     (double)o->collider_count * 3.1;
    }
    if (g_ready && signature == g_signature) return;
    if (!g_blocked) {
        size_t n = 2 * N2;
        g_blocked = calloc(n, 1);
        g_floor = calloc(n, sizeof *g_floor);
        g_cost = malloc(n * sizeof *g_cost);
        g_from = malloc(n * sizeof *g_from);
        g_seen = calloc(n, sizeof *g_seen);
        g_done = calloc(n, sizeof *g_done);
        g_heap_cap = MOST_STEPS * 8;
        g_heap = malloc((size_t)g_heap_cap * sizeof *g_heap);
        if (!g_blocked || !g_floor || !g_cost || !g_from || !g_seen || !g_done || !g_heap) return;
    }
    if (!g_table) {
        g_table = calloc(NAV_TABLE, sizeof *g_table);
        g_table_new = calloc(NAV_TABLE, sizeof *g_table_new);
        if (!g_table || !g_table_new) return;
    }
    /* What changed since the last build: the cells it was on and is on now */
    CellRect dirty[NAV_DIRTY];
    int n_dirty = 0;
    bool whole = !g_ready;
    g_build++;
    memset(g_table_new, 0, NAV_TABLE * sizeof *g_table_new);
    for (int i = 0; i < count; i++) {
        const Object *o = objects[i];
        if (!o->is_static || o->collider_count == 0) continue;
        bool found = false, was = false;
        NavEntry *e = slotFor(g_table_new, o, &found);
        if (!e) { whole = true; continue; }
        NavEntry *old = slotFor(g_table, o, &was);
        *e = (NavEntry){ o, o->position.x, o->position.z, o->rotation.y, o->collider_count, boundsOf(o), 0 };
        if (was && old->x == e->x && old->z == e->z && old->yaw == e->yaw && old->colliders == e->colliders) { old->seen = g_build; continue; }
        if (was) old->seen = g_build;
        if (n_dirty + 2 > NAV_DIRTY) { whole = true; continue; }
        if (was) dirty[n_dirty++] = old->cells;
        dirty[n_dirty++] = e->cells;
    }
    for (int k = 0; k < NAV_TABLE && !whole; k++) { /* (gone since: its cells) */
        const NavEntry *old = &g_table[k];
        if (!old->o || old->seen == g_build) continue;
        if (n_dirty + 1 > NAV_DIRTY) whole = true;
        else dirty[n_dirty++] = old->cells;
    }
    NavEntry *swap = g_table;
    g_table = g_table_new;
    g_table_new = swap;
    if (whole) {
        CellRect all = { 0, 0, NAV_N - 1, NAV_N - 1 };
        redo(objects, count, terrain, all);
    } else {
        for (int k = 0; k < n_dirty; k++)
            if (dirty[k].x1 >= dirty[k].x0 && dirty[k].z1 >= dirty[k].z0) redo(objects, count, terrain, dirty[k]);
    }
    g_signature = signature;
    g_ready = true;
}

bool Nav_free(Vec3 p) {
    return !g_ready || open(cellOf(p.x), cellOf(p.z));
}

/* The level at (x, z) nearest the height y (the lower one only if it's there and nearer). */
static int levelFor(int x, int z, float y) {
    if (!inside(x, z)) return 0;
    float f0 = g_floor[node(0, x, z)], f1 = g_floor[node(1, x, z)];
    return f1 != NO_FLOOR && fabsf(y - f1) < fabsf(y - f0) ? 1 : 0;
}

/* Nearest open cell of that level to (x, z) within a few cells; false if none. */
static bool nearestOpen(int level, int *x, int *z) {
    if (openAt(level, *x, *z)) return true;
    for (int r = 1; r <= FREE_SEARCH; r++) {
        int best = -1;
        float best_d = 1e9f;
        for (int dz = -r; dz <= r; dz++)
            for (int dx = -r; dx <= r; dx++) {
                if (abs(dx) != r && abs(dz) != r) continue;
                float d = (float)(dx * dx + dz * dz);
                if (openAt(level, *x + dx, *z + dz) && d < best_d) {
                    best_d = d;
                    best = (*z + dz) * NAV_N + (*x + dx);
                }
            }
        if (best >= 0) {
            *x = best % NAV_N;
            *z = best / NAV_N;
            return true;
        }
    }
    return false;
}

static void heapPush(float f, int cell) {
    if (g_heap_n >= g_heap_cap) return;
    int i = g_heap_n++;
    while (i > 0) {
        int up = (i - 1) / 2;
        if (g_heap[up].f <= f) break;
        g_heap[i] = g_heap[up];
        i = up;
    }
    g_heap[i] = (HeapItem){ f, cell };
}

static HeapItem heapPop(void) {
    HeapItem top = g_heap[0], last = g_heap[--g_heap_n];
    int i = 0;
    for (;;) {
        int c = 2 * i + 1;
        if (c >= g_heap_n) break;
        if (c + 1 < g_heap_n && g_heap[c + 1].f < g_heap[c].f) c++;
        if (g_heap[c].f >= last.f) break;
        g_heap[i] = g_heap[c];
        i = c;
    }
    if (g_heap_n > 0) g_heap[i] = last;
    return top;
}

static float guess(int x, int z, int gx, int gz) {
    float dx = fabsf((float)(x - gx)), dz = fabsf((float)(z - gz));
    return (dx + dz) + (1.41421356f - 2.0f) * fminf(dx, dz);
}

/* The open node at (x, z) a step from height `from` (either level), or -1. */
static int stepTo(int x, int z, float from, bool diagonal) {
    for (int level = 0; level < 2; level++) {
        if (!openAt(level, x, z)) continue;
        int n = node(level, x, z);
        if (fabsf(g_floor[n] - from) <= (diagonal ? NAV_RISE * 1.42f : NAV_RISE)) return n;
    }
    return -1;
}

/* Straight line between two nodes over open cells, a step at a time up or down. */
static bool clearLine(int a, int b) {
    int ac = (int)((size_t)a % N2), bc = (int)((size_t)b % N2);
    int ax = ac % NAV_N, az = ac / NAV_N, bx = bc % NAV_N, bz = bc / NAV_N;
    float dx = (float)(bx - ax), dz = (float)(bz - az);
    int steps = (int)(fmaxf(fabsf(dx), fabsf(dz)) * 3.0f) + 1, last = ac;
    float floor_y = g_floor[a];
    for (int s = 1; s <= steps; s++) {
        float t = (float)s / (float)steps;
        float fx = (float)ax + 0.5f + dx * t, fz = (float)az + 0.5f + dz * t;
        int cx = (int)floorf(fx), cz = (int)floorf(fz), here = cz * NAV_N + cx;
        if (here != last) {
            int n = stepTo(cx, cz, floor_y, true); /* (not off the edge of a floor) */
            if (n < 0) return false;
            floor_y = g_floor[n];
            last = here;
        }
        /* (a little to each side too: no grazing corners) */
        if (stepTo((int)floorf(fx + 0.3f), (int)floorf(fz + 0.3f), floor_y, true) < 0 ||
            stepTo((int)floorf(fx - 0.3f), (int)floorf(fz - 0.3f), floor_y, true) < 0)
            return false;
    }
    return fabsf(floor_y - g_floor[b]) <= NAV_RISE * 1.42f;
}

int Nav_findPath(Vec3 from, Vec3 to, Vec3 *out, int max) {
    if (!g_ready || max < 1) return 0;
    int sx = cellOf(from.x), sz = cellOf(from.z), gx = cellOf(to.x), gz = cellOf(to.z);
    int s_level = levelFor(sx, sz, from.y), g_level = levelFor(gx, gz, to.y);
    bool goal_open = openAt(g_level, gx, gz);
    if (!nearestOpen(s_level, &sx, &sz) || !nearestOpen(g_level, &gx, &gz)) return 0;
    int start = node(s_level, sx, sz), goal = node(g_level, gx, gz);

    if (++g_stamp == 0) { /* wrapped: clear the stamps once */
        memset(g_seen, 0, 2 * N2 * sizeof *g_seen);
        memset(g_done, 0, 2 * N2 * sizeof *g_done);
        g_stamp = 1;
    }
    g_heap_n = 0;
    g_cost[start] = 0.0f;
    g_from[start] = -1;
    g_seen[start] = g_stamp;
    heapPush(guess(sx, sz, gx, gz), start);
    static const int DX[8] = { 1, -1, 0, 0, 1, 1, -1, -1 }, DZ[8] = { 0, 0, 1, -1, 1, -1, 1, -1 };
    bool found = false;
    for (int steps = 0; g_heap_n > 0 && steps < MOST_STEPS; steps++) {
        HeapItem it = heapPop();
        int cur = it.cell;
        if (g_done[cur] == g_stamp) continue;
        g_done[cur] = g_stamp;
        if (cur == goal) { found = true; break; }
        int cell = (int)((size_t)cur % N2), x = cell % NAV_N, z = cell / NAV_N;
        float here = g_floor[cur];
        for (int k = 0; k < 8; k++) {
            int nx = x + DX[k], nz = z + DZ[k];
            int n = stepTo(nx, nz, here, k >= 4); /* (up or down a floor's edge: no) */
            if (n < 0) continue;
            if (k >= 4 && stepTo(x + DX[k], z, here, false) < 0 && stepTo(x, z + DZ[k], here, false) < 0) continue; /* no squeezing between two corners (past one: the margin's room enough - a turned stair runs diagonally) */
            if (g_done[n] == g_stamp) continue;
            float cost = g_cost[cur] + (k < 4 ? 1.0f : 1.41421356f);
            if (g_seen[n] == g_stamp && cost >= g_cost[n]) continue;
            g_seen[n] = g_stamp;
            g_cost[n] = cost;
            g_from[n] = cur;
            heapPush(cost + guess(nx, nz, gx, gz), n);
        }
    }
    if (!found) return 0;

    /* The nodes back from the goal, then only the turns that matter */
    int length = 0;
    for (int c = goal; c >= 0; c = g_from[c]) length++;
    int *chain = malloc((size_t)length * sizeof *chain);
    if (!chain) return 0;
    int k = length;
    for (int c = goal; c >= 0; c = g_from[c]) chain[--k] = c;
    int n = 0, at = 0;
    while (at < length - 1 && n < max) {
        int next = at + 1; /* as far along as can be seen straight */
        while (next + 1 < length && clearLine(chain[at], chain[next + 1])) next++;
        at = next;
        int cell = (int)((size_t)chain[at] % N2);
        out[n++] = vec3(centerOf(cell % NAV_N), g_floor[chain[at]], centerOf(cell / NAV_N));
    }
    free(chain);
    if (n == 0) out[n++] = vec3(centerOf(gx), g_floor[goal], centerOf(gz));
    if (goal_open && at == length - 1) out[n - 1] = vec3(to.x, g_floor[goal], to.z); /* right to it */
    return n;
}