/**
 * @file collision_grid.c
 * @brief The incremental uniform grid, and why it is the default.
 *
 * A dense array of cell heads over the world, each holding a chain of the
 * proxies that overlap it. Insert and remove happen when a proxy crosses a cell
 * boundary and at no other time, so an actor walking across a tile touches this
 * structure on one frame in sixteen and a static proxy touches it once, ever.
 *
 * That property is the whole argument for it over a tree, and it is measured
 * rather than asserted: `PERFORMANCE.md` records Construct getting a 96% check
 * reduction from viewport-sized cells and rejecting quadtrees as more expensive
 * to maintain, and Phaser's rebuild-the-tree-every-frame approach capping out
 * around five thousand bodies. A tree partitioner ships alongside this one so
 * the two can be compared rather than argued about, but this is the default.
 *
 * Everything here is an `int16_t` index rather than a pointer, which keeps the
 * structure relocatable and makes clearing it a `memset`.
 */

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

#include <akerror.h>
#include <akstdlib.h>

#include <akgl/collision.h>
#include <akgl/error.h>
#include <akgl/heap.h>

/** @brief Columns of cells. Fixed, so the head array is a compile-time size. */
#ifndef AKGL_COLLISION_GRID_COLS
#define AKGL_COLLISION_GRID_COLS        128
#endif
/** @brief Rows of cells. */
#ifndef AKGL_COLLISION_GRID_ROWS
#define AKGL_COLLISION_GRID_ROWS        128
#endif

/**
 * @brief Most cells one proxy may occupy before it goes on the oversized chain.
 *
 * A proxy spanning half the map would otherwise claim thousands of cell entries
 * and drain the pool on its own. Anything larger than this is held in one list
 * that every query walks, which is the standard escape hatch -- and it is what
 * makes the cell pool's ceiling provable rather than hopeful: at most
 * `proxies * AKGL_COLLISION_GRID_MAX_SPAN` entries can ever be live.
 */
#define AKGL_COLLISION_GRID_MAX_SPAN    16

/** @brief Cell heads, one per cell, each an index into the cell-entry pool or -1. */
static int16_t grid_heads[AKGL_COLLISION_GRID_ROWS * AKGL_COLLISION_GRID_COLS];
/** @brief Head of the chain of proxies too large to cell, or -1. */
static int16_t grid_oversized;
/** @brief Cell size and origin, copied from the world by `reset`. */
static float32_t grid_cellw;
static float32_t grid_cellh;
static float32_t grid_originx;
static float32_t grid_originy;
/** @brief Bumped by every query, and stamped onto proxies to suppress duplicates. */
static uint32_t grid_sweep;

/** @brief Index of a proxy in the pool, or -1 if it is not one of ours. */
static int16_t grid_proxy_index(akgl_CollisionProxy *proxy)
{
    ptrdiff_t index = 0;

    index = proxy - &akgl_heap_collision_proxies[0];
    if ( (index < 0) || (index >= AKGL_MAX_HEAP_COLLISION_PROXY) ) {
	return -1;
    }
    return (int16_t)index;
}

/** @brief Index of a cell entry in the pool. */
static int16_t grid_cell_index(akgl_CollisionCell *cell)
{
    return (int16_t)(cell - &akgl_heap_collision_cells[0]);
}

/**
 * @brief The inclusive rectangle of cells a proxy's bounds cover.
 *
 * `floorf` and not a cast to `int`, because truncation rounds toward zero and
 * puts -0.5 and +0.5 in the same cell. **Today that difference is unobservable**
 * -- everything below zero is clamped to cell 0 either way, and swapping in a
 * truncating cast passes the whole suite. It is written correctly anyway,
 * because the clamp is the only thing hiding it: give this world a negative
 * origin, or relax the clamp so an off-map proxy keeps its own cell, and
 * truncation becomes a real defect with no test standing in front of it.
 */
static void grid_cellrect(SDL_FRect *bounds, int32_t *cx0, int32_t *cy0, int32_t *cx1, int32_t *cy1)
{
    *cx0 = (int32_t)floorf((bounds->x - grid_originx) / grid_cellw);
    *cy0 = (int32_t)floorf((bounds->y - grid_originy) / grid_cellh);
    *cx1 = (int32_t)floorf(((bounds->x + bounds->w) - grid_originx) / grid_cellw);
    *cy1 = (int32_t)floorf(((bounds->y + bounds->h) - grid_originy) / grid_cellh);

    /*
     * Clamp rather than drop. A proxy outside the grid is still a collider --
     * an actor that has walked off the map is exactly the one that needs a wall
     * to walk back into -- so it is folded onto the edge cells instead of
     * vanishing from the broad phase.
     */
    if ( *cx0 < 0 ) { *cx0 = 0; }
    if ( *cy0 < 0 ) { *cy0 = 0; }
    if ( *cx1 > (AKGL_COLLISION_GRID_COLS - 1) ) { *cx1 = AKGL_COLLISION_GRID_COLS - 1; }
    if ( *cy1 > (AKGL_COLLISION_GRID_ROWS - 1) ) { *cy1 = AKGL_COLLISION_GRID_ROWS - 1; }
    if ( *cx1 < *cx0 ) { *cx1 = *cx0; }
    if ( *cy1 < *cy0 ) { *cy1 = *cy0; }
    if ( *cx0 > (AKGL_COLLISION_GRID_COLS - 1) ) { *cx0 = AKGL_COLLISION_GRID_COLS - 1; *cx1 = *cx0; }
    if ( *cy0 > (AKGL_COLLISION_GRID_ROWS - 1) ) { *cy0 = AKGL_COLLISION_GRID_ROWS - 1; *cy1 = *cy0; }
}

/** @brief Unlink every cell entry belonging to a proxy and give them back. */
static akerr_ErrorContext *grid_unlink(akgl_CollisionProxy *proxy)
{
    akgl_CollisionCell *entry = NULL;
    int16_t walk = 0;
    int16_t next = 0;

    PREPARE_ERROR(errctx);

    walk = proxy->first;
    while ( walk >= 0 ) {
	entry = &akgl_heap_collision_cells[walk];
	next = entry->ownernext;

	if ( entry->prev >= 0 ) {
	    akgl_heap_collision_cells[entry->prev].next = entry->next;
	} else if ( entry->cell >= 0 ) {
	    grid_heads[entry->cell] = entry->next;
	} else {
	    grid_oversized = entry->next;
	}
	if ( entry->next >= 0 ) {
	    akgl_heap_collision_cells[entry->next].prev = entry->prev;
	}

	PASS(errctx, akgl_heap_release_collision_cell(entry));
	walk = next;
    }
    proxy->first = -1;
    proxy->cx0 = 0;
    proxy->cy0 = 0;
    proxy->cx1 = -1;
    proxy->cy1 = -1;
    SUCCEED_RETURN(errctx);
}

/** @brief Claim one cell entry and push it onto a chain head. */
static akerr_ErrorContext *grid_link_one(akgl_CollisionProxy *proxy, int16_t proxyidx, int32_t cell, int16_t *head)
{
    akgl_CollisionCell *entry = NULL;

    PREPARE_ERROR(errctx);
    PASS(errctx, akgl_heap_next_collision_cell(&entry));
    PASS(errctx, akgl_collision_cell_initialize(entry, proxyidx, cell));

    entry->next = *head;
    entry->prev = -1;
    if ( *head >= 0 ) {
	akgl_heap_collision_cells[*head].prev = grid_cell_index(entry);
    }
    *head = grid_cell_index(entry);

    entry->ownernext = proxy->first;
    proxy->first = grid_cell_index(entry);
    SUCCEED_RETURN(errctx);
}

/** @brief Put a proxy into every cell its bounds cover. */
static akerr_ErrorContext *grid_link(akgl_CollisionProxy *proxy, int32_t cx0, int32_t cy0, int32_t cx1, int32_t cy1)
{
    int16_t proxyidx = 0;
    int32_t span = 0;
    int32_t x = 0;
    int32_t y = 0;

    PREPARE_ERROR(errctx);

    proxyidx = grid_proxy_index(proxy);
    FAIL_NONZERO_RETURN(errctx, (proxyidx < 0), AKERR_VALUE,
			"Proxy is not from the collision proxy pool");

    span = (cx1 - cx0 + 1) * (cy1 - cy0 + 1);
    if ( span > AKGL_COLLISION_GRID_MAX_SPAN ) {
	// Too big to cell. One chain, walked by every query.
	PASS(errctx, grid_link_one(proxy, proxyidx, -1, &grid_oversized));
    } else {
	for ( y = cy0; y <= cy1; y++ ) {
	    for ( x = cx0; x <= cx1; x++ ) {
		PASS(errctx, grid_link_one(proxy, proxyidx, (y * AKGL_COLLISION_GRID_COLS) + x,
					   &grid_heads[(y * AKGL_COLLISION_GRID_COLS) + x]));
	    }
	}
    }

    proxy->cx0 = cx0;
    proxy->cy0 = cy0;
    proxy->cx1 = cx1;
    proxy->cy1 = cy1;
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *grid_reset(akgl_Partitioner *self, akgl_CollisionWorld *world)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, world, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_NONZERO_RETURN(errctx, (world->cellwidth <= 0.0f), AKERR_VALUE,
			"Cell width %f is not positive", world->cellwidth);
    FAIL_NONZERO_RETURN(errctx, (world->cellheight <= 0.0f), AKERR_VALUE,
			"Cell height %f is not positive", world->cellheight);

    memset(grid_heads, 0xFF, sizeof(grid_heads));
    grid_oversized = -1;
    grid_cellw = world->cellwidth;
    grid_cellh = world->cellheight;
    grid_originx = world->originx;
    grid_originy = world->originy;
    grid_sweep = 0;
    PASS(errctx, akgl_heap_init_collision_cells());
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *grid_insert(akgl_Partitioner *self, akgl_CollisionProxy *proxy)
{
    int32_t cx0 = 0;
    int32_t cy0 = 0;
    int32_t cx1 = 0;
    int32_t cy1 = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, proxy, AKERR_NULLPOINTER, "NULL proxy reference");

    if ( proxy->first >= 0 ) {
	PASS(errctx, grid_unlink(proxy));
    }
    grid_cellrect(&proxy->bounds, &cx0, &cy0, &cx1, &cy1);
    PASS(errctx, grid_link(proxy, cx0, cy0, cx1, cy1));
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *grid_remove(akgl_Partitioner *self, akgl_CollisionProxy *proxy)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, proxy, AKERR_NULLPOINTER, "NULL proxy reference");

    // Removing something that is not in is success. A caller tearing down does
    // not want to track which proxies it already dropped.
    if ( proxy->first < 0 ) {
	SUCCEED_RETURN(errctx);
    }
    PASS(errctx, grid_unlink(proxy));
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *grid_move(akgl_Partitioner *self, akgl_CollisionProxy *proxy)
{
    int32_t cx0 = 0;
    int32_t cy0 = 0;
    int32_t cx1 = 0;
    int32_t cy1 = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, proxy, AKERR_NULLPOINTER, "NULL proxy reference");

    if ( proxy->first < 0 ) {
	PASS(errctx, grid_insert(self, proxy));
	SUCCEED_RETURN(errctx);
    }

    grid_cellrect(&proxy->bounds, &cx0, &cy0, &cx1, &cy1);

    /*
     * The early return is the entire point of the structure. An actor that has
     * moved within the cells it already occupies performs no pool operation and
     * writes nothing, which is the steady state for a walking character and the
     * permanent state for a static proxy. Replacing this with remove-then-insert
     * would turn the incremental grid into the rebuild-every-frame tree that was
     * measured and rejected.
     */
    if ( (cx0 == proxy->cx0) && (cy0 == proxy->cy0) &&
	 (cx1 == proxy->cx1) && (cy1 == proxy->cy1) ) {
	SUCCEED_RETURN(errctx);
    }

    PASS(errctx, grid_unlink(proxy));
    PASS(errctx, grid_link(proxy, cx0, cy0, cx1, cy1));
    SUCCEED_RETURN(errctx);
}

/** @brief Walk one chain, reporting each proxy at most once this sweep. */
static akerr_ErrorContext *grid_walk_chain(int16_t head, SDL_FRect *area, uint32_t mask,
					   akgl_CollisionVisitFunc visit, void *data, bool *stop)
{
    akgl_CollisionProxy *proxy = NULL;
    akgl_CollisionCell *entry = NULL;
    akerr_ErrorContext *inner = NULL;
    int16_t walk = 0;

    PREPARE_ERROR(errctx);

    walk = head;
    while ( (walk >= 0) && (*stop == false) ) {
	entry = &akgl_heap_collision_cells[walk];
	walk = entry->next;
	proxy = &akgl_heap_collision_proxies[entry->proxy];

	// A proxy in four cells appears on four chains. The stamp is what makes
	// a query report it once, and costs one comparison to do it.
	if ( proxy->stamp == grid_sweep ) {
	    continue;
	}
	proxy->stamp = grid_sweep;

	if ( (proxy->shape.layermask & mask) == 0 ) {
	    continue;
	}
	if ( !SDL_HasRectIntersectionFloat(&proxy->bounds, area) ) {
	    continue;
	}

	/*
	 * The visitor's status is taken into a local rather than handed to CATCH
	 * here: CATCH reports by breaking, and a break inside this loop would
	 * leave the loop rather than the function, so a visitor's failure would
	 * be swallowed and the walk would look like it finished.
	 */
	inner = visit(proxy, data);
	if ( inner != NULL ) {
	    *stop = true;
	    PASS(errctx, inner);
	}
    }
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *grid_query(akgl_Partitioner *self, SDL_FRect *area, uint32_t mask,
				      akgl_CollisionVisitFunc visit, void *data)
{
    bool stop = false;
    int32_t cx0 = 0;
    int32_t cy0 = 0;
    int32_t cx1 = 0;
    int32_t cy1 = 0;
    int32_t x = 0;
    int32_t y = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, area, AKERR_NULLPOINTER, "NULL query area reference");
    FAIL_ZERO_RETURN(errctx, visit, AKERR_NULLPOINTER, "NULL visitor reference");

    grid_sweep += 1;
    grid_cellrect(area, &cx0, &cy0, &cx1, &cy1);

    for ( y = cy0; (y <= cy1) && (stop == false); y++ ) {
	for ( x = cx0; (x <= cx1) && (stop == false); x++ ) {
	    PASS(errctx, grid_walk_chain(grid_heads[(y * AKGL_COLLISION_GRID_COLS) + x],
					 area, mask, visit, data, &stop));
	}
    }
    if ( stop == false ) {
	PASS(errctx, grid_walk_chain(grid_oversized, area, mask, visit, data, &stop));
    }
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Whether this cell is the one a pair should be reported from.
 *
 * Two proxies sharing four cells would otherwise be reported four times. The
 * rule is to report only from the lowest-numbered cell both occupy, which is
 * computable from the two cell rectangles in constant time with no marks and no
 * per-pair storage -- and which works when a walk is re-entered, where a stamp
 * would not.
 */
static bool grid_owns_pair(akgl_CollisionProxy *a, akgl_CollisionProxy *b, int32_t cell)
{
    int32_t x0 = (a->cx0 > b->cx0) ? a->cx0 : b->cx0;
    int32_t y0 = (a->cy0 > b->cy0) ? a->cy0 : b->cy0;
    int32_t x1 = (a->cx1 < b->cx1) ? a->cx1 : b->cx1;
    int32_t y1 = (a->cy1 < b->cy1) ? a->cy1 : b->cy1;

    if ( (x0 > x1) || (y0 > y1) ) {
	// They share no cell, so this pair came off the oversized chain.
	return true;
    }
    return (cell == ((y0 * AKGL_COLLISION_GRID_COLS) + x0));
}

static akerr_ErrorContext *grid_each_pair(akgl_Partitioner *self, akgl_CollisionPairFunc visit, void *data)
{
    akgl_CollisionProxy *a = NULL;
    akgl_CollisionProxy *b = NULL;
    akerr_ErrorContext *inner = NULL;
    bool stop = false;
    int32_t cell = 0;
    int16_t outer = 0;
    int16_t walk = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");
    FAIL_ZERO_RETURN(errctx, visit, AKERR_NULLPOINTER, "NULL visitor reference");

    for ( cell = 0; (cell < (AKGL_COLLISION_GRID_ROWS * AKGL_COLLISION_GRID_COLS)) && (stop == false); cell++ ) {
	outer = grid_heads[cell];
	while ( (outer >= 0) && (stop == false) ) {
	    a = &akgl_heap_collision_proxies[akgl_heap_collision_cells[outer].proxy];
	    walk = akgl_heap_collision_cells[outer].next;
	    while ( (walk >= 0) && (stop == false) ) {
		b = &akgl_heap_collision_proxies[akgl_heap_collision_cells[walk].proxy];
		walk = akgl_heap_collision_cells[walk].next;
		if ( !grid_owns_pair(a, b, cell) ) {
		    continue;
		}
		inner = visit(a, b, data);
		if ( inner != NULL ) {
		    stop = true;
		    PASS(errctx, inner);
		}
	    }
	    outer = akgl_heap_collision_cells[outer].next;
	}
    }

    // Everything oversized against everything else, since it is in no cell.
    outer = grid_oversized;
    while ( (outer >= 0) && (stop == false) ) {
	a = &akgl_heap_collision_proxies[akgl_heap_collision_cells[outer].proxy];
	walk = akgl_heap_collision_cells[outer].next;
	while ( (walk >= 0) && (stop == false) ) {
	    b = &akgl_heap_collision_proxies[akgl_heap_collision_cells[walk].proxy];
	    walk = akgl_heap_collision_cells[walk].next;
	    inner = visit(a, b, data);
	    if ( inner != NULL ) {
		stop = true;
		PASS(errctx, inner);
	    }
	}
	outer = akgl_heap_collision_cells[outer].next;
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_partitioner_init_grid(akgl_Partitioner *self)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");

    memset(self, 0x00, sizeof(akgl_Partitioner));
    PASS(errctx, aksl_strncpy(self->name, sizeof(self->name), "grid", sizeof(self->name) - 1));
    self->reset = grid_reset;
    self->insert = grid_insert;
    self->remove = grid_remove;
    self->move = grid_move;
    self->query = grid_query;
    self->each_pair = grid_each_pair;
    self->state = NULL;
    SUCCEED_RETURN(errctx);
}
