/**
 * @file collision.c
 * @brief The narrowphase, and the only place libccd is visible.
 *
 * Nothing else in the tree includes `<ccd/ccd.h>`. The translation between
 * libakgl's shapes and libccd's support-function protocol is `static` here, so
 * changing narrowphase library is a change to one file and no header -- and so
 * that `akgl.pc` never has to name a dependency that is compiled in.
 */

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

#include <akerror.h>

#include <akgl/collision.h>
#include <akstdlib.h>
#include <akgl/collision_arena.h>
#include <akgl/error.h>
#include <akgl/types.h>

#include <ccd/ccd.h>
#include <ccd/quat.h>
#include <ccd/vec3.h>

/**
 * @brief Ceiling on narrowphase iterations.
 *
 * libccd's own default, set by `CCD_INIT`, is `(unsigned long)-1` -- an
 * unbounded loop inside a frame. A pair that reaches this cap is reported as not
 * colliding, which is the same answer a missed collision gives, but it
 * terminates. Bounding the iterations also bounds what the arena can be asked
 * for, since the polytope grows with them.
 */
#define AKGL_COLLISION_MAX_ITERATIONS   100

/** @brief Below this, a normal is treated as having no length at all. */
#define AKGL_COLLISION_EPSILON          1e-6f

/** @brief One positioned shape, in the form libccd's callbacks read. */
typedef struct {
    uint8_t kind;       /**< AKGL_COLLISION_SHAPE_*. */
    ccd_vec3_t pos;     /**< World centre: owner position plus shape offset. */
    ccd_vec3_t half;    /**< Half-extents. `v[0]` doubles as the radius for a circle. */
} collision_ccdobj;

/**
 * @brief Furthest point of a shape in a given direction.
 *
 * The support function *is* the shape as far as libccd is concerned. One
 * function dispatching on kind rather than one per kind, because a pair may mix
 * kinds and `ccd_t` has no way to choose per object beyond this.
 *
 * There is no rotation, here or anywhere in this library yet -- `util.h` says so
 * and both example games depend on it. Adding it means rotating @p dir into the
 * shape's local frame at the top of this function and rotating the answer back,
 * and nothing else in the narrowphase changes. See `TODO.md`, "Actor rotation".
 */
static void collision_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *dest)
{
    const collision_ccdobj *shape = (const collision_ccdobj *)obj;
    ccd_real_t planar = 0.0;
    ccd_real_t len = 0.0;

    switch ( shape->kind ) {
    case AKGL_COLLISION_SHAPE_CIRCLE:
	/*
	 * A circle extruded along z is a cylinder, not a sphere. A sphere's caps
	 * curve away from the plane and would let a shape slide past a corner
	 * that a 2D game expects to catch.
	 */
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3X(&shape->half) / len;
	    ccdVec3Set(dest, ccdVec3X(dir) * planar, ccdVec3Y(dir) * planar, CCD_ZERO);
	} else {
	    ccdVec3Set(dest, CCD_ZERO, CCD_ZERO, CCD_ZERO);
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    case AKGL_COLLISION_SHAPE_CAPSULE_X:
	// A segment on x with a circle of radius hy swept along it.
	ccdVec3Set(dest,
		   ccdSign(ccdVec3X(dir)) * (ccdVec3X(&shape->half) - ccdVec3Y(&shape->half)),
		   CCD_ZERO,
		   CCD_ZERO);
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3Y(&shape->half) / len;
	    dest->v[0] += ccdVec3X(dir) * planar;
	    dest->v[1] += ccdVec3Y(dir) * planar;
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    case AKGL_COLLISION_SHAPE_CAPSULE_Y:
	ccdVec3Set(dest,
		   CCD_ZERO,
		   ccdSign(ccdVec3Y(dir)) * (ccdVec3Y(&shape->half) - ccdVec3X(&shape->half)),
		   CCD_ZERO);
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3X(&shape->half) / len;
	    dest->v[0] += ccdVec3X(dir) * planar;
	    dest->v[1] += ccdVec3Y(dir) * planar;
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    default:
	// A box, and the fallback for anything unrecognised: the corner in the
	// direction's octant.
	ccdVec3Set(dest,
		   ccdSign(ccdVec3X(dir)) * ccdVec3X(&shape->half),
		   ccdSign(ccdVec3Y(dir)) * ccdVec3Y(&shape->half),
		   ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half));
	break;
    }
    ccdVec3Add(dest, &shape->pos);
}

/**
 * @brief Centre of a shape.
 *
 * MPR requires this where GJK does not: `ccdMPRPenetration` calls `center1` and
 * `center2` unconditionally, so leaving either `NULL` is a null dereference on
 * the first contact rather than a degraded answer.
 */
static void collision_center(const void *obj, ccd_vec3_t *dest)
{
    const collision_ccdobj *shape = (const collision_ccdobj *)obj;

    ccdVec3Copy(dest, &shape->pos);
}

/** @brief Put a proxy into the form the support functions read. */
static void collision_to_ccd(akgl_CollisionProxy *proxy, collision_ccdobj *dest)
{
    dest->kind = proxy->shape.kind;
    ccdVec3Set(&dest->pos,
	       (ccd_real_t)(proxy->x + proxy->shape.ox),
	       (ccd_real_t)(proxy->y + proxy->shape.oy),
	       (ccd_real_t)(proxy->z + proxy->shape.oz));
    ccdVec3Set(&dest->half,
	       (ccd_real_t)proxy->shape.hx,
	       (ccd_real_t)proxy->shape.hy,
	       (ccd_real_t)proxy->shape.hz);
}

/** @brief Configure the solver. Identical for every query. */
static void collision_configure(ccd_t *ccd)
{
    CCD_INIT(ccd);
    ccd->support1 = collision_support;
    ccd->support2 = collision_support;
    ccd->center1 = collision_center;
    ccd->center2 = collision_center;
    ccd->max_iterations = AKGL_COLLISION_MAX_ITERATIONS;
}

/**
 * @brief Box against box, in closed form.
 *
 * Overlap on each axis, take the smallest, and the normal is that axis signed
 * away from @p b. Exact rather than converged, which matters for a resting
 * actor: an iterative solver answers `(0.0001, -0.99999, 0)` where this answers
 * `(0, -1, 0)`, and that difference accumulates into a slow sideways creep along
 * a floor.
 */
static akerr_ErrorContext *collision_box_box(akgl_CollisionProxy *a, akgl_CollisionProxy *b, akgl_Contact *dest, bool *hit)
{
    float32_t delta[3];
    float32_t overlap[3];
    float32_t acentre[3];
    float32_t bcentre[3];
    int axis = 0;
    int i = 0;

    PREPARE_ERROR(errctx);

    acentre[0] = a->x + a->shape.ox;
    acentre[1] = a->y + a->shape.oy;
    acentre[2] = a->z + a->shape.oz;
    bcentre[0] = b->x + b->shape.ox;
    bcentre[1] = b->y + b->shape.oy;
    bcentre[2] = b->z + b->shape.oz;

    delta[0] = bcentre[0] - acentre[0];
    delta[1] = bcentre[1] - acentre[1];
    delta[2] = bcentre[2] - acentre[2];

    overlap[0] = (a->shape.hx + b->shape.hx) - fabsf(delta[0]);
    overlap[1] = (a->shape.hy + b->shape.hy) - fabsf(delta[1]);
    overlap[2] = (a->shape.hz + b->shape.hz) - fabsf(delta[2]);

    *hit = false;
    for ( i = 0; i < 3; i++ ) {
	if ( overlap[i] <= 0.0f ) {
	    SUCCEED_RETURN(errctx);
	}
    }

    for ( i = 1; i < 3; i++ ) {
	if ( overlap[i] < overlap[axis] ) {
	    axis = i;
	}
    }

    dest->nx = 0.0f;
    dest->ny = 0.0f;
    dest->nz = 0.0f;
    /*
     * Away from b. If b's centre is to the right of a's, then a leaves to the
     * left and the normal is negative on that axis. A delta of exactly 0 means
     * the two are concentric here and either direction is as good; pick one
     * rather than emit a zero-length normal, which would be a wall that moves
     * nothing.
     */
    if ( axis == 0 ) {
	dest->nx = (delta[0] > 0.0f) ? -1.0f : 1.0f;
    } else if ( axis == 1 ) {
	dest->ny = (delta[1] > 0.0f) ? -1.0f : 1.0f;
    } else {
	dest->nz = (delta[2] > 0.0f) ? -1.0f : 1.0f;
    }
    dest->depth = overlap[axis];

    // Midpoint of the two centres. For two boxes that sits inside the overlap.
    dest->px = (acentre[0] + bcentre[0]) / 2.0f;
    dest->py = (acentre[1] + bcentre[1]) / 2.0f;
    dest->pz = (acentre[2] + bcentre[2]) / 2.0f;

    *hit = true;
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Flatten a normal into the xy plane, and rescue a degenerate one.
 *
 * See #AKGL_COLLISION_TEST_PLANAR. The fallback matters more than it looks: two
 * shapes at exactly the same centre have no planar direction to separate along,
 * and level authors put things on top of each other constantly. Answering with a
 * zero-length normal there would move an actor by nothing and report success,
 * which is a wall that does not stop anything.
 */
static void collision_flatten(akgl_CollisionProxy *a, akgl_CollisionProxy *b, akgl_Contact *dest)
{
    float32_t len = 0.0f;
    float32_t dx = 0.0f;
    float32_t dy = 0.0f;

    dest->nz = 0.0f;
    len = sqrtf((dest->nx * dest->nx) + (dest->ny * dest->ny));
    if ( len > AKGL_COLLISION_EPSILON ) {
	dest->nx = dest->nx / len;
	dest->ny = dest->ny / len;
	return;
    }

    // Nothing planar survived. Separate along whichever axis they overlap least.
    dx = (a->shape.hx + b->shape.hx) - fabsf((b->x + b->shape.ox) - (a->x + a->shape.ox));
    dy = (a->shape.hy + b->shape.hy) - fabsf((b->y + b->shape.oy) - (a->y + a->shape.oy));
    if ( dx <= dy ) {
	dest->nx = (((b->x + b->shape.ox) - (a->x + a->shape.ox)) > 0.0f) ? -1.0f : 1.0f;
	dest->ny = 0.0f;
	dest->depth = dx;
    } else {
	dest->nx = 0.0f;
	dest->ny = (((b->y + b->shape.oy) - (a->y + a->shape.oy)) > 0.0f) ? -1.0f : 1.0f;
	dest->depth = dy;
    }
}

akerr_ErrorContext *akgl_collision_test(akgl_CollisionProxy *a, akgl_CollisionProxy *b, uint32_t flags, akgl_Contact *dest, bool *hit)
{
    collision_ccdobj obja;
    collision_ccdobj objb;
    ccd_t ccd;
    ccd_real_t depth = 0.0;
    ccd_vec3_t dir;
    ccd_vec3_t pos;
    int result = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, a, AKERR_NULLPOINTER, "NULL first proxy reference");
    FAIL_ZERO_RETURN(errctx, b, AKERR_NULLPOINTER, "NULL second proxy reference");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "NULL contact reference");
    FAIL_ZERO_RETURN(errctx, hit, AKERR_NULLPOINTER, "NULL hit flag reference");

    memset(dest, 0x00, sizeof(akgl_Contact));
    dest->tilex = -1;
    dest->tiley = -1;
    dest->tilelayer = -1;
    *hit = false;

    if ( (a->shape.kind == AKGL_COLLISION_SHAPE_NONE) ||
	 (b->shape.kind == AKGL_COLLISION_SHAPE_NONE) ) {
	SUCCEED_RETURN(errctx);
    }

    /*
     * The bounds are already computed and already stored, so the cheapest
     * rejection in the system costs four comparisons and no arithmetic. Most
     * candidate pairs a broad phase hands over die right here.
     */
    if ( !SDL_HasRectIntersectionFloat(&a->bounds, &b->bounds) ) {
	SUCCEED_RETURN(errctx);
    }

    if ( (a->shape.kind == AKGL_COLLISION_SHAPE_BOX) &&
	 (b->shape.kind == AKGL_COLLISION_SHAPE_BOX) ) {
	PASS(errctx, collision_box_box(a, b, dest, hit));
	if ( (*hit == true) && ((flags & AKGL_COLLISION_TEST_PLANAR) != 0) ) {
	    collision_flatten(a, b, dest);
	}
	SUCCEED_RETURN(errctx);
    }

    collision_to_ccd(a, &obja);
    collision_to_ccd(b, &objb);
    collision_configure(&ccd);

    // One query, one arena. Resetting on entry rather than on exit means a query
    // that returns early still leaves it clean for the next one.
    akgl_ccd_arena_reset();

    /*
     * MPR and not GJK+EPA. MPR allocates nothing at all, converges in fewer
     * iterations, and its weakness -- a coarser contact *point* -- is on a field
     * the blocking resolver never reads. EPA stays compiled and available for a
     * caller who one day wants an accurate manifold and will pay the arena for
     * it.
     */
    result = ccdMPRPenetration(&obja, &objb, &ccd, &depth, &dir, &pos);
    if ( result == -2 ) {
	FAIL_RETURN(
	    errctx,
	    AKGL_ERR_COLLISION,
	    "Collision arena exhausted at %zu of %d bytes; raise AKGL_CCD_ARENA_BYTES",
	    akgl_ccd_arena_highwater(),
	    AKGL_CCD_ARENA_BYTES
	    );
    }
    if ( result != 0 ) {
	SUCCEED_RETURN(errctx);
    }

    /*
     * libccd hands back the direction that separates the *second* object. This
     * contact points at the first, so that a caller moving `a` along the normal
     * by the depth undoes the overlap -- which is what every resolver wants, and
     * saves each of them from remembering the sign.
     */
    dest->nx = -(float32_t)ccdVec3X(&dir);
    dest->ny = -(float32_t)ccdVec3Y(&dir);
    dest->nz = -(float32_t)ccdVec3Z(&dir);
    dest->depth = (float32_t)depth;
    dest->px = (float32_t)ccdVec3X(&pos);
    dest->py = (float32_t)ccdVec3Y(&pos);
    dest->pz = (float32_t)ccdVec3Z(&pos);

    if ( (flags & AKGL_COLLISION_TEST_PLANAR) != 0 ) {
	collision_flatten(a, b, dest);
    }
    *hit = true;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_arena_selftest(float32_t separation, bool *hit, float32_t *depth)
{
    collision_ccdobj a;
    collision_ccdobj b;
    ccd_t ccd;
    ccd_real_t ccddepth = 0.0;
    ccd_vec3_t dir;
    ccd_vec3_t pos;
    int result = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, hit, AKERR_NULLPOINTER, "NULL hit flag reference");
    FAIL_ZERO_RETURN(errctx, depth, AKERR_NULLPOINTER, "NULL depth reference");

    collision_configure(&ccd);

    a.kind = AKGL_COLLISION_SHAPE_BOX;
    ccdVec3Set(&a.pos, 0.0, 0.0, 0.0);
    ccdVec3Set(&a.half, 1.0, 1.0, 1.0);
    b.kind = AKGL_COLLISION_SHAPE_BOX;
    ccdVec3Set(&b.pos, (ccd_real_t)separation, 0.0, 0.0);
    ccdVec3Set(&b.half, 1.0, 1.0, 1.0);

    akgl_ccd_arena_reset();

    /*
     * ccdGJKPenetration and not ccdMPRPenetration, deliberately. This is the EPA
     * path, and EPA is the half of libccd that allocates -- so it is the half
     * that exercises the arena. The narrowphase proper uses MPR, which does not.
     */
    result = ccdGJKPenetration(&a, &b, &ccd, &ccddepth, &dir, &pos);

    if ( result == -2 ) {
	FAIL_RETURN(
	    errctx,
	    AKGL_ERR_COLLISION,
	    "Collision arena exhausted at %zu of %d bytes; raise AKGL_CCD_ARENA_BYTES",
	    akgl_ccd_arena_highwater(),
	    AKGL_CCD_ARENA_BYTES
	    );
    }

    *hit = (result == 0);
    *depth = (float32_t)ccddepth;
    SUCCEED_RETURN(errctx);
}

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

    // NULL means "the default", which is the grid. Matched on leading
    // characters, the same way akgl_physics_factory matches its backends.
    if ( (type == NULL) || (strncmp(type, "grid", 4) == 0) ) {
	PASS(errctx, akgl_partitioner_init_grid(self));
	SUCCEED_RETURN(errctx);
    }
    FAIL_RETURN(errctx, AKERR_KEY, "No partitioner named \"%s\"", type);
}

akerr_ErrorContext *akgl_collision_world_init(akgl_CollisionWorld *self, char *type, float32_t cellwidth, float32_t cellheight)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_NONZERO_RETURN(errctx, (cellwidth <= 0.0f), AKERR_VALUE,
			"Cell width %f is not positive", cellwidth);
    FAIL_NONZERO_RETURN(errctx, (cellheight <= 0.0f), AKERR_VALUE,
			"Cell height %f is not positive", cellheight);

    memset(self, 0x00, sizeof(akgl_CollisionWorld));
    self->cellwidth = cellwidth;
    self->cellheight = cellheight;
    // Every game today is 2D, so the guard is on by default. A caller with a
    // real third axis to resolve on clears it.
    self->flags = AKGL_COLLISION_TEST_PLANAR;

    PASS(errctx, akgl_partitioner_factory(&self->partitioner, type));
    PASS(errctx, self->partitioner.reset(&self->partitioner, self));
    SUCCEED_RETURN(errctx);
}