/**
 * @file collision.c
 * @brief The collision 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
 * a change of narrowphase library is a change to one file and no header.
 *
 * At present this holds only the arena's end-to-end check; the shapes, the
 * contact and the resolver arrive with the rest of the collision work.
 */

#include <akerror.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 An axis-aligned box, in the shape libccd's callbacks want. */
typedef struct {
    ccd_vec3_t pos;     /**< World centre. */
    ccd_vec3_t half;    /**< Half-extents on each axis. */
} collision_box;

/**
 * @brief Furthest point of a box in a given direction.
 *
 * The support function *is* the shape as far as libccd is concerned: hand it a
 * direction, get back the extreme point. For an axis-aligned box that is the
 * corner in the direction's octant, which is three sign tests.
 *
 * There is no rotation here, and none anywhere in this library yet -- util.h
 * says so and both example games depend on it. Adding it means rotating @p dir
 * into the box's local frame at the top of this function and rotating the
 * result back, and nothing else in the narrowphase changes. See TODO.md,
 * "Actor rotation".
 */
static void collision_box_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *dest)
{
    const collision_box *box = (const collision_box *)obj;

    ccdVec3Set(dest,
	       ccdSign(ccdVec3X(dir)) * ccdVec3X(&box->half),
	       ccdSign(ccdVec3Y(dir)) * ccdVec3Y(&box->half),
	       ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&box->half));
    ccdVec3Add(dest, &box->pos);
}

/**
 * @brief Centre of a box.
 *
 * MPR requires this and 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_box_center(const void *obj, ccd_vec3_t *dest)
{
    const collision_box *box = (const collision_box *)obj;

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

akerr_ErrorContext *akgl_collision_arena_selftest(float32_t separation, bool *hit, float32_t *depth)
{
    collision_box a;
    collision_box 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");

    CCD_INIT(&ccd);
    ccd.support1 = collision_box_support;
    ccd.support2 = collision_box_support;
    ccd.center1 = collision_box_center;
    ccd.center2 = collision_box_center;
    ccd.max_iterations = AKGL_COLLISION_MAX_ITERATIONS;

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

    /*
     * The arena's lifetime is one query. Resetting on entry rather than on exit
     * means a query that returns early still leaves the arena clean for the
     * next one.
     */
    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 proves the arena is wired up. A narrowphase on the frame path
     * would prefer MPR, which allocates nothing at all.
     */
    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);
}