/**
 * @file collision_bsp.c
 * @brief A binary space partition, and why it is not the default.
 *
 * This exists so that "pluggable" means something. A vtable with one
 * implementation behind it has never been asked to be a vtable, and the
 * partitioner suite runs its whole contract against every entry in a table --
 * so a second implementation is what turns that contract from a comment into a
 * check.
 *
 * It rebuilds the tree whenever the proxy set changes rather than maintaining it
 * incrementally, which is the shape `PERFORMANCE.md` records Phaser using and
 * capping out around five thousand bodies. The grid stays the default for the
 * reason recorded there: an object that has not left its cell costs a grid
 * nothing, and costs this a whole rebuild.
 *
 * @section bsp_no_iterate Why aksl_tree_iterate is not used
 *
 * It cannot be, for three independent reasons, and a comment is cheaper than the
 * next person rediscovering them:
 *
 * 1. **It is a complete traversal.** Every search mode visits every node, and
 *    the only control signal the callback has is `AKERR_ITERATOR_BREAK`, which
 *    stops the *entire* walk. There is no way to say "do not descend into this
 *    subtree", and that is the only operation a spatial query is made of. Using
 *    it would visit every node in the tree -- strictly worse than the linear
 *    scan the tree is supposed to replace.
 * 2. **It carries no per-node context.** A pruning descent needs each node's
 *    bounds and split plane; the callback gets a node and one `void *` that is
 *    the same object at every node.
 * 3. **Its breadth-first modes allocate.** Only the depth-first ones are
 *    malloc-free, and those are the ones with no pruning.
 *
 * `aksl_tree_insert` and `aksl_tree_find` are equally unusable and for a
 * different reason: they are a comparator-ordered binary search tree.
 * `aksl_TreeCompareFunc` is handed two leaves and asked which is smaller, and a
 * spatial insert has to compare a leaf against a *plane*, which that signature
 * cannot express. Inserting through it builds a tree ordered by pointer value.
 *
 * What is used from libakstdlib is the link structure and the lists: aksl_TreeNode
 * for left/right/parent, aksl_List and aksl_ListNode for a node's items. Neither
 * allocates, which is why they fit here at all.
 */

#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 Proxies at or below which a node stops splitting. */
#define AKGL_COLLISION_BSP_LEAF_ITEMS   8
/** @brief Deepest the tree may go. Bounds the descent stack, which is an array. */
#define AKGL_COLLISION_BSP_MAX_DEPTH    16

/** @brief The root, or NULL when there is nothing to index. */
static akgl_BspNode *bsp_root;
/** @brief Every proxy currently registered, in insertion order. */
static akgl_CollisionProxy *bsp_members[AKGL_MAX_HEAP_COLLISION_PROXY];
static int bsp_count;
/** @brief Set when the tree no longer describes bsp_members. */
static bool bsp_dirty;
/** @brief World extent, for the root node's bounds. */
static SDL_FRect bsp_extent;
/** @brief Bumped per query, stamped on proxies so one is reported once. */
static uint32_t bsp_sweep;

/** @brief Give every node back and forget the tree. */
static akerr_ErrorContext *bsp_drop_tree(void)
{
    PREPARE_ERROR(errctx);
    PASS(errctx, akgl_heap_init_bspnodes());
    bsp_root = NULL;
    SUCCEED_RETURN(errctx);
}

/** @brief Claim a node covering a region. */
static akerr_ErrorContext *bsp_node(akgl_BspNode **dest, SDL_FRect *bounds, uint8_t depth)
{
    PREPARE_ERROR(errctx);
    PASS(errctx, akgl_heap_next_bspnode(dest));
    memset(*dest, 0x00, sizeof(akgl_BspNode));
    (*dest)->refcount = 1;
    (*dest)->bounds = *bounds;
    (*dest)->depth = depth;
    (*dest)->axis = 2;
    PASS(errctx, aksl_tree_node_init(&(*dest)->node, (void *)(*dest)));
    PASS(errctx, aksl_list_init(&(*dest)->items));
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Split a node and push down whatever fits entirely on one side.
 *
 * Median of the item centres on the longer axis, not the spatial midpoint.
 * Actors in a tile game cluster -- everything is on the floor -- and a midpoint
 * split gives one empty child and one full one for several levels running.
 *
 * A proxy straddling the plane stays at this node and is tested against
 * everything below on both sides. That is the standard BSP tax, and it is why
 * one inline list link per proxy is enough.
 */
static akerr_ErrorContext *bsp_build(akgl_BspNode *node, akgl_CollisionProxy **items, int count)
{
    akgl_CollisionProxy *left[AKGL_MAX_HEAP_COLLISION_PROXY];
    akgl_CollisionProxy *right[AKGL_MAX_HEAP_COLLISION_PROXY];
    akgl_BspNode *child = NULL;
    SDL_FRect half;
    float32_t total = 0.0f;
    int leftn = 0;
    int rightn = 0;
    int i = 0;

    PREPARE_ERROR(errctx);

    if ( (count <= AKGL_COLLISION_BSP_LEAF_ITEMS) ||
	 (node->depth >= AKGL_COLLISION_BSP_MAX_DEPTH) ) {
	for ( i = 0; i < count; i++ ) {
	    PASS(errctx, aksl_list_node_init(&items[i]->node, (void *)items[i]));
	    PASS(errctx, aksl_list_push(&node->items, &items[i]->node));
	}
	SUCCEED_RETURN(errctx);
    }

    node->axis = (node->bounds.w >= node->bounds.h) ? 0 : 1;
    for ( i = 0; i < count; i++ ) {
	if ( node->axis == 0 ) {
	    total += items[i]->bounds.x + (items[i]->bounds.w / 2.0f);
	} else {
	    total += items[i]->bounds.y + (items[i]->bounds.h / 2.0f);
	}
    }
    node->split = total / (float32_t)count;

    for ( i = 0; i < count; i++ ) {
	if ( node->axis == 0 ) {
	    if ( (items[i]->bounds.x + items[i]->bounds.w) <= node->split ) {
		left[leftn++] = items[i];
		continue;
	    }
	    if ( items[i]->bounds.x >= node->split ) {
		right[rightn++] = items[i];
		continue;
	    }
	} else {
	    if ( (items[i]->bounds.y + items[i]->bounds.h) <= node->split ) {
		left[leftn++] = items[i];
		continue;
	    }
	    if ( items[i]->bounds.y >= node->split ) {
		right[rightn++] = items[i];
		continue;
	    }
	}
	// Straddles the plane: it stays here.
	PASS(errctx, aksl_list_node_init(&items[i]->node, (void *)items[i]));
	PASS(errctx, aksl_list_push(&node->items, &items[i]->node));
    }

    // A split that separated nothing would recurse forever on the same set.
    if ( (leftn == 0) || (rightn == 0) ) {
	for ( i = 0; i < leftn; i++ ) {
	    PASS(errctx, aksl_list_node_init(&left[i]->node, (void *)left[i]));
	    PASS(errctx, aksl_list_push(&node->items, &left[i]->node));
	}
	for ( i = 0; i < rightn; i++ ) {
	    PASS(errctx, aksl_list_node_init(&right[i]->node, (void *)right[i]));
	    PASS(errctx, aksl_list_push(&node->items, &right[i]->node));
	}
	node->axis = 2;
	SUCCEED_RETURN(errctx);
    }

    half = node->bounds;
    if ( node->axis == 0 ) {
	half.w = node->split - node->bounds.x;
    } else {
	half.h = node->split - node->bounds.y;
    }
    PASS(errctx, bsp_node(&child, &half, (uint8_t)(node->depth + 1)));
    node->node.left = &child->node;
    child->node.parent = &node->node;
    PASS(errctx, bsp_build(child, left, leftn));

    half = node->bounds;
    if ( node->axis == 0 ) {
	half.x = node->split;
	half.w = (node->bounds.x + node->bounds.w) - node->split;
    } else {
	half.y = node->split;
	half.h = (node->bounds.y + node->bounds.h) - node->split;
    }
    PASS(errctx, bsp_node(&child, &half, (uint8_t)(node->depth + 1)));
    node->node.right = &child->node;
    child->node.parent = &node->node;
    PASS(errctx, bsp_build(child, right, rightn));
    SUCCEED_RETURN(errctx);
}

/** @brief Rebuild the tree from the registered proxies, if it is stale. */
static akerr_ErrorContext *bsp_rebuild(void)
{
    SDL_FRect bounds;
    int i = 0;

    PREPARE_ERROR(errctx);

    if ( bsp_dirty == false ) {
	SUCCEED_RETURN(errctx);
    }
    PASS(errctx, bsp_drop_tree());
    bsp_dirty = false;

    if ( bsp_count == 0 ) {
	SUCCEED_RETURN(errctx);
    }

    // The root covers the world, widened to hold anything outside it. A proxy
    // that fell off the map is still a collider.
    bounds = bsp_extent;
    for ( i = 0; i < bsp_count; i++ ) {
	if ( bsp_members[i]->bounds.x < bounds.x ) {
	    bounds.w += bounds.x - bsp_members[i]->bounds.x;
	    bounds.x = bsp_members[i]->bounds.x;
	}
	if ( bsp_members[i]->bounds.y < bounds.y ) {
	    bounds.h += bounds.y - bsp_members[i]->bounds.y;
	    bounds.y = bsp_members[i]->bounds.y;
	}
	if ( (bsp_members[i]->bounds.x + bsp_members[i]->bounds.w) > (bounds.x + bounds.w) ) {
	    bounds.w = (bsp_members[i]->bounds.x + bsp_members[i]->bounds.w) - bounds.x;
	}
	if ( (bsp_members[i]->bounds.y + bsp_members[i]->bounds.h) > (bounds.y + bounds.h) ) {
	    bounds.h = (bsp_members[i]->bounds.y + bsp_members[i]->bounds.h) - bounds.y;
	}
    }

    PASS(errctx, bsp_node(&bsp_root, &bounds, 0));
    PASS(errctx, bsp_build(bsp_root, bsp_members, bsp_count));
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_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);

    PASS(errctx, bsp_drop_tree());
    bsp_count = 0;
    bsp_dirty = true;
    bsp_sweep = 0;
    bsp_extent.x = world->originx;
    bsp_extent.y = world->originy;
    bsp_extent.w = world->cellwidth * 128.0f;
    bsp_extent.h = world->cellheight * 128.0f;
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_insert(akgl_Partitioner *self, akgl_CollisionProxy *proxy)
{
    int i = 0;

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

    for ( i = 0; i < bsp_count; i++ ) {
	if ( bsp_members[i] == proxy ) {
	    SUCCEED_RETURN(errctx);
	}
    }
    FAIL_NONZERO_RETURN(errctx, (bsp_count >= AKGL_MAX_HEAP_COLLISION_PROXY), AKGL_ERR_HEAP,
			"More proxies registered than the pool can hold");
    bsp_members[bsp_count] = proxy;
    bsp_count += 1;
    bsp_dirty = true;
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_remove(akgl_Partitioner *self, akgl_CollisionProxy *proxy)
{
    int i = 0;

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

    for ( i = 0; i < bsp_count; i++ ) {
	if ( bsp_members[i] != proxy ) {
	    continue;
	}
	bsp_members[i] = bsp_members[bsp_count - 1];
	bsp_count -= 1;
	bsp_dirty = true;
	SUCCEED_RETURN(errctx);
    }
    // Removing something that is not in is success, as it is for the grid.
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_move(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");

    /*
     * A tree cannot answer this cheaply. The grid compares four integers and
     * returns; this has to assume the partition is wrong now, and the next query
     * pays for a rebuild. That difference is the whole reason the grid is the
     * default, and it is visible right here rather than buried in a benchmark.
     */
    bsp_dirty = true;
    PASS(errctx, bsp_insert(self, proxy));
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Visit the items of every node whose region the query can still reach.
 *
 * An explicit stack, not recursion. The depth bound is then an array bound the
 * compiler can see rather than a hope, and this library already has one
 * documented way to blow the C stack -- akgl_heap_release_actor recursing over
 * children with no cycle check -- which is one more than it needs.
 */
static akerr_ErrorContext *bsp_descend(SDL_FRect *area, uint32_t mask,
				       akgl_CollisionVisitFunc visit, void *data)
{
    akgl_BspNode *stack[AKGL_COLLISION_BSP_MAX_DEPTH + 2];
    akgl_BspNode *node = NULL;
    akgl_CollisionProxy *proxy = NULL;
    aksl_ListNode *walk = NULL;
    akerr_ErrorContext *inner = NULL;
    bool stop = false;
    int top = 0;

    PREPARE_ERROR(errctx);

    if ( bsp_root == NULL ) {
	SUCCEED_RETURN(errctx);
    }
    stack[top++] = bsp_root;

    while ( (top > 0) && (stop == false) ) {
	node = stack[--top];

	walk = node->items.head;
	while ( (walk != NULL) && (stop == false) ) {
	    proxy = (akgl_CollisionProxy *)walk->data;
	    walk = walk->next;
	    if ( proxy->stamp == bsp_sweep ) {
		continue;
	    }
	    proxy->stamp = bsp_sweep;
	    if ( (proxy->shape.layermask & mask) == 0 ) {
		continue;
	    }
	    if ( !SDL_HasRectIntersectionFloat(&proxy->bounds, area) ) {
		continue;
	    }
	    // Taken into a local rather than handed to CATCH: a break here would
	    // leave this loop, not the function, and the failure would look like
	    // the end of the list.
	    inner = visit(proxy, data);
	    if ( inner != NULL ) {
		stop = true;
		PASS(errctx, inner);
	    }
	}

	if ( node->axis == 2 ) {
	    continue;
	}

	/*
	 * The pruning. A query entirely on one side of the plane pushes one
	 * child and never touches the other subtree -- which is the operation
	 * aksl_tree_iterate has no way to express, and the reason this walk is
	 * written by hand.
	 */
	if ( node->axis == 0 ) {
	    if ( (area->x < node->split) && (node->node.left != NULL) && (top < AKGL_COLLISION_BSP_MAX_DEPTH) ) {
		stack[top++] = (akgl_BspNode *)node->node.left->leaf;
	    }
	    if ( ((area->x + area->w) >= node->split) && (node->node.right != NULL) && (top < AKGL_COLLISION_BSP_MAX_DEPTH) ) {
		stack[top++] = (akgl_BspNode *)node->node.right->leaf;
	    }
	} else {
	    if ( (area->y < node->split) && (node->node.left != NULL) && (top < AKGL_COLLISION_BSP_MAX_DEPTH) ) {
		stack[top++] = (akgl_BspNode *)node->node.left->leaf;
	    }
	    if ( ((area->y + area->h) >= node->split) && (node->node.right != NULL) && (top < AKGL_COLLISION_BSP_MAX_DEPTH) ) {
		stack[top++] = (akgl_BspNode *)node->node.right->leaf;
	    }
	}
    }
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_query(akgl_Partitioner *self, SDL_FRect *area, uint32_t mask,
				     akgl_CollisionVisitFunc visit, void *data)
{
    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");

    PASS(errctx, bsp_rebuild());
    bsp_sweep += 1;
    PASS(errctx, bsp_descend(area, mask, visit, data));
    SUCCEED_RETURN(errctx);
}

static akerr_ErrorContext *bsp_each_pair(akgl_Partitioner *self, akgl_CollisionPairFunc visit, void *data)
{
    akerr_ErrorContext *inner = NULL;
    bool stop = false;
    int i = 0;
    int j = 0;

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

    PASS(errctx, bsp_rebuild());

    /*
     * Every pair whose bounds overlap, straight off the member list. A tree
     * offers no cheap way to enumerate pairs -- the grid gets that for free from
     * cell membership -- so this is honest about being the naive form rather
     * than dressing a linear scan up as a spatial one.
     */
    for ( i = 0; (i < bsp_count) && (stop == false); i++ ) {
	for ( j = i + 1; (j < bsp_count) && (stop == false); j++ ) {
	    if ( !SDL_HasRectIntersectionFloat(&bsp_members[i]->bounds, &bsp_members[j]->bounds) ) {
		continue;
	    }
	    inner = visit(bsp_members[i], bsp_members[j], data);
	    if ( inner != NULL ) {
		stop = true;
		PASS(errctx, inner);
	    }
	}
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_partitioner_init_bsp(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), "bsp", sizeof(self->name) - 1));
    self->reset = bsp_reset;
    self->insert = bsp_insert;
    self->remove = bsp_remove;
    self->move = bsp_move;
    self->query = bsp_query;
    self->each_pair = bsp_each_pair;
    self->state = NULL;
    SUCCEED_RETURN(errctx);
}
