/**
 * @file collision.c
 * @brief The collision libakgl does not have.
 *
 * This file exists because of a gap, and the gap is worth stating exactly:
 *
 * - `akgl_physics_arcade_collide` raises `AKERR_API` with the message "Not
 *   implemented".
 * - `akgl_physics_simulate` never calls `collide` at all -- not for the arcade
 *   backend, not for the null one. The vtable slot exists and the simulation
 *   does not use it.
 * - `akgl_physics_arcade_move` is `position += velocity * dt` and nothing else.
 *   It does not clamp to the map, consult the tilemap, or test anything.
 *
 * So an actor walks through a wall and off the edge of the world, and the only
 * hook that runs inside the physics step is the actor's own `movementlogicfunc`.
 * That is where this game's collision lives, and everything here is called from
 * there.
 *
 * The awkward part is the ordering. `movementlogicfunc` runs *before* gravity,
 * drag, the velocity recomputation and `move`, so it cannot look at where the
 * actor ended up -- the step has not happened yet. ss_collide_predict therefore
 * repeats the arithmetic akgl_physics_simulate is about to do, and the sweep
 * resolves against that predicted position. Get the prediction wrong and the
 * actor is resolved against a step it never takes.
 */

#include <math.h>

#include <akstdlib.h>

#include <akgl/heap.h>
#include <akgl/physics.h>

#include "sidescroller.h"

/**
 * @brief Half a pixel-thousandth, taken off the far edge of a box before it is
 *        turned into tile indices.
 *
 * A box whose right edge sits exactly on a tile boundary does not overlap the
 * tile on the far side of it. Without this, an actor standing flush against a
 * wall reads as inside it, and the sweep snaps it back a tile every frame.
 */
#define SS_COLLIDE_EPSILON      0.001f

/** @brief Longest sub-step the sweep takes, in pixels. Half a tile cannot tunnel. */
#define SS_COLLIDE_SUBSTEP      (SS_TILE_SIZE / 2.0f)

/** @brief Tiles ss_collide_settle will lift a spawn point before giving up. */
#define SS_COLLIDE_SETTLE_TILES 4

static akgl_TilemapLayer *ss_terrain = NULL;

/**
 * @brief Is this map cell solid?
 *
 * Off the left or right edge of the map is solid, so the level has walls at its
 * ends. Off the top or the bottom is not: the sky is open and the pit in the
 * middle of level1.tmj has to be fallable-into, which is the whole point of it.
 */
static akerr_ErrorContext *solid_tile(int tilex, int tiley, bool *dest)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "dest");
    FAIL_ZERO_RETURN(errctx, ss_terrain, AKERR_NULLPOINTER, "ss_collide_bind has not run");

    if ( (tilex < 0) || (tilex >= ss_terrain->width) ) {
	*dest = true;
    } else if ( (tiley < 0) || (tiley >= ss_terrain->height) ) {
	*dest = false;
    } else {
	/* A tile layer's data is global tile ids in row-major order, and 0 is
	 * the empty cell. Any tile at all on the terrain layer is solid: the
	 * level says what is solid by which layer the tile is drawn on. */
	*dest = (ss_terrain->data[(tiley * ss_terrain->width) + tilex] != 0);
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_bind(akgl_Tilemap *map)
{
    int i = 0;
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, map, AKERR_NULLPOINTER, "map");

    ss_terrain = NULL;
    for ( i = 0; i < map->numlayers; i++ ) {
	if ( (map->layers[i].type == AKGL_TILEMAP_LAYER_TYPE_TILES) &&
	     (map->layers[i].id == SS_TERRAIN_LAYER_ID) ) {
	    ss_terrain = &map->layers[i];
	}
    }
    FAIL_ZERO_RETURN(
	errctx,
	ss_terrain,
	AKERR_KEY,
	"Map has no tile layer with id %d to collide against",
	SS_TERRAIN_LAYER_ID
	);
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_solid_at(float32_t x, float32_t y, bool *dest)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "dest");
    PASS(errctx, solid_tile((int)floorf(x / SS_TILE_SIZE), (int)floorf(y / SS_TILE_SIZE), dest));
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_box_blocked(SDL_FRect *box, bool *dest)
{
    int x0 = 0;
    int x1 = 0;
    int y0 = 0;
    int y1 = 0;
    int tilex = 0;
    int tiley = 0;
    bool solid = false;
    PREPARE_ERROR(errctx);

    FAIL_ZERO_RETURN(errctx, box, AKERR_NULLPOINTER, "box");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "dest");
    FAIL_NONZERO_RETURN(
	errctx,
	((box->w <= 0.0f) || (box->h <= 0.0f)),
	AKERR_VALUE,
	"Collision box is %fx%f; both sides must be positive",
	box->w,
	box->h
	);

    *dest = false;
    x0 = (int)floorf(box->x / SS_TILE_SIZE);
    x1 = (int)floorf((box->x + box->w - SS_COLLIDE_EPSILON) / SS_TILE_SIZE);
    y0 = (int)floorf(box->y / SS_TILE_SIZE);
    y1 = (int)floorf((box->y + box->h - SS_COLLIDE_EPSILON) / SS_TILE_SIZE);

    for ( tiley = y0; tiley <= y1; tiley++ ) {
	for ( tilex = x0; tilex <= x1; tilex++ ) {
	    PASS(errctx, solid_tile(tilex, tiley, &solid));
	    if ( solid == true ) {
		*dest = true;
		SUCCEED_RETURN(errctx);
	    }
	}
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_predict(akgl_Actor *obj, float32_t dt, float32_t *dx, float32_t *dy)
{
    float32_t ex = 0.0f;
    float32_t ey = 0.0f;
    PREPARE_ERROR(errctx);

    FAIL_ZERO_RETURN(errctx, obj, AKERR_NULLPOINTER, "obj");
    FAIL_ZERO_RETURN(errctx, dx, AKERR_NULLPOINTER, "dx");
    FAIL_ZERO_RETURN(errctx, dy, AKERR_NULLPOINTER, "dy");
    FAIL_ZERO_RETURN(errctx, akgl_physics, AKERR_NULLPOINTER, "akgl_physics");

    /*
     * Everything below is akgl_physics_simulate's own arithmetic, in its own
     * order: gravity onto the environmental term, then drag off it, then
     * velocity as environmental plus thrust, then position plus velocity times
     * dt. The `!= 0` guards are the library's too -- it skips each axis whose
     * constant is zero rather than multiplying by it -- and are repeated here
     * because dropping them would make a zero-gravity axis pick up drag.
     *
     * This mirrors the *arcade* backend. Against the null backend gravity does
     * nothing, so the prediction reduces to `(ex + tx) * dt`, which is still
     * right.
     */
    ex = obj->ex;
    ey = obj->ey;
    if ( akgl_physics->gravity_x != 0 ) {
	ex -= (float32_t)akgl_physics->gravity_x * dt;
    }
    if ( akgl_physics->gravity_y != 0 ) {
	ey += (float32_t)akgl_physics->gravity_y * dt;
    }
    if ( akgl_physics->drag_x != 0 ) {
	ex -= ex * (float32_t)akgl_physics->drag_x * dt;
    }
    if ( akgl_physics->drag_y != 0 ) {
	ey -= ey * (float32_t)akgl_physics->drag_y * dt;
    }
    *dx = (ex + obj->tx) * dt;
    *dy = (ey + obj->ty) * dt;
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Walk @p box along (@p dx, @p dy) in sub-steps, stopping it at terrain.
 *
 * Each axis is tested on its own, which is what lets an actor slide along a wall
 * instead of sticking to it, and the sub-step is capped at half a tile so a
 * fast fall cannot pass through a floor between two samples. At 900 px/s^2 with
 * the step bounded to `physics.max_timestep` (0.05 s) a fall covers 30 px in one
 * step, which is nearly two tiles -- so this is not a theoretical concern.
 *
 * On a blocked axis the box is snapped to the tile boundary it was about to
 * cross rather than simply not moved, so an actor lands flush on a floor at
 * whatever speed it arrives.
 */
static akerr_ErrorContext *sweep(SDL_FRect *box, float32_t dx, float32_t dy, ss_Contact *dest)
{
    SDL_FRect trial;
    float32_t span = 0.0f;
    float32_t stepx = 0.0f;
    float32_t stepy = 0.0f;
    int steps = 0;
    int i = 0;
    bool solid = false;
    PREPARE_ERROR(errctx);

    FAIL_ZERO_RETURN(errctx, box, AKERR_NULLPOINTER, "box");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "dest");

    span = fabsf(dx);
    if ( fabsf(dy) > span ) {
	span = fabsf(dy);
    }
    steps = (int)(span / SS_COLLIDE_SUBSTEP) + 1;
    stepx = dx / (float32_t)steps;
    stepy = dy / (float32_t)steps;

    for ( i = 0; i < steps; i++ ) {
	if ( stepx != 0.0f ) {
	    trial = *box;
	    trial.x += stepx;
	    PASS(errctx, ss_collide_box_blocked(&trial, &solid));
	    if ( solid == true ) {
		if ( stepx > 0.0f ) {
		    box->x = (floorf((trial.x + trial.w) / SS_TILE_SIZE) * SS_TILE_SIZE) - trial.w;
		} else {
		    box->x = (floorf(trial.x / SS_TILE_SIZE) + 1.0f) * SS_TILE_SIZE;
		}
		stepx = 0.0f;
		dest->blocked_x = true;
	    } else {
		box->x = trial.x;
	    }
	}
	if ( stepy != 0.0f ) {
	    trial = *box;
	    trial.y += stepy;
	    PASS(errctx, ss_collide_box_blocked(&trial, &solid));
	    if ( solid == true ) {
		if ( stepy > 0.0f ) {
		    box->y = (floorf((trial.y + trial.h) / SS_TILE_SIZE) * SS_TILE_SIZE) - trial.h;
		} else {
		    box->y = (floorf(trial.y / SS_TILE_SIZE) + 1.0f) * SS_TILE_SIZE;
		}
		stepy = 0.0f;
		dest->blocked_y = true;
	    } else {
		box->y = trial.y;
	    }
	}
	if ( (stepx == 0.0f) && (stepy == 0.0f) ) {
	    break;
	}
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_resolve(akgl_Actor *obj, SDL_FRect *body, float32_t dt, ss_Contact *dest)
{
    SDL_FRect box;
    SDL_FRect probe;
    float32_t dx = 0.0f;
    float32_t dy = 0.0f;
    bool solid = false;
    PREPARE_ERROR(errctx);

    FAIL_ZERO_RETURN(errctx, obj, AKERR_NULLPOINTER, "obj");
    FAIL_ZERO_RETURN(errctx, body, AKERR_NULLPOINTER, "body");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "dest");

    dest->blocked_x = false;
    dest->blocked_y = false;
    dest->grounded = false;

    PASS(errctx, ss_collide_predict(obj, dt, &dx, &dy));

    box.x = obj->x + body->x;
    box.y = obj->y + body->y;
    box.w = body->w;
    box.h = body->h;
    PASS(errctx, sweep(&box, dx, dy, dest));

    /*
     * Only a blocked axis is written back. The free axis is left for
     * akgl_physics_arcade_move to advance by exactly the amount this function
     * predicted -- writing it here as well would move the actor twice.
     *
     * `ex`/`ey` are where gravity accumulates and `tx`/`ty` are the actor's own
     * effort; a blocked axis has to give up both, or the actor keeps pressing
     * into the wall and the next step's prediction is wrong by everything it
     * accumulated while stuck.
     *
     * The environmental term is not set to zero, it is set to *minus one step of
     * gravity*, and that is not a nicety. Zeroing it leaves the step about to add
     * `gravity_y * dt` back, which commits `gravity_y * dt^2` of fall -- a
     * quarter of a pixel at 60 Hz. A quarter of a pixel is invisible and it is
     * still fatal: the box now overlaps the floor tile, so the *horizontal*
     * sweep on the next step finds itself blocked wherever it is, and the actor
     * is snapped back a whole tile every time it tries to walk. Pre-loading the
     * cancellation leaves the actor resting exactly on the surface instead.
     */
    if ( dest->blocked_x == true ) {
	obj->x = box.x - body->x;
	obj->ex = (float32_t)akgl_physics->gravity_x * dt;
	obj->tx = 0.0f;
    }
    if ( dest->blocked_y == true ) {
	obj->y = box.y - body->y;
	obj->ey = -(float32_t)akgl_physics->gravity_y * dt;
	obj->ty = 0.0f;
    }

    /*
     * Standing on a floor is not a state the library records, so it is measured:
     * one pixel below where the box will be at the end of this step. Note that
     * zeroing `ey` above does not stop gravity -- the step still adds
     * `gravity_y * dt` to it and `move` still commits `gravity_y * dt^2` of
     * fall, which is a quarter of a pixel at 60 Hz. The next step's sweep takes
     * it straight back off, so a standing actor rests within a pixel of the
     * surface forever rather than sinking through it.
     */
    probe = box;
    probe.y += 1.0f;
    PASS(errctx, ss_collide_box_blocked(&probe, &solid));
    dest->grounded = solid;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *ss_collide_settle(akgl_Actor *obj, SDL_FRect *body)
{
    SDL_FRect box;
    bool solid = false;
    int i = 0;
    PREPARE_ERROR(errctx);

    FAIL_ZERO_RETURN(errctx, obj, AKERR_NULLPOINTER, "obj");
    FAIL_ZERO_RETURN(errctx, body, AKERR_NULLPOINTER, "body");

    /*
     * A hand-drawn level places a 32-pixel sprite on a 16-pixel grid, so an
     * actor can start out with its box partly inside a step -- level1.tmj puts
     * the player at x=32 and a block at tiles (3,11), which is under the right
     * half of the player's frame.
     *
     * The swept resolution cannot fix that. It stops an actor *entering*
     * terrain and has nothing to say about one that began inside it; what it
     * does instead is refuse every horizontal move, because the box is already
     * blocked wherever it goes. So a spawn point gets lifted clear once, before
     * the first step, a tile at a time.
     */
    for ( i = 0; i < SS_COLLIDE_SETTLE_TILES; i++ ) {
	box.x = obj->x + body->x;
	box.y = obj->y + body->y;
	box.w = body->w;
	box.h = body->h;
	PASS(errctx, ss_collide_box_blocked(&box, &solid));
	if ( solid == false ) {
	    SUCCEED_RETURN(errctx);
	}
	obj->y -= (float32_t)SS_TILE_SIZE;
	SDL_Log("Lifted %s out of the terrain it spawned in, to y=%f", (char *)obj->name, obj->y);
    }
    FAIL_RETURN(
	errctx,
	AKERR_VALUE,
	"%s spawned inside more than %d tiles of terrain at %f, %f",
	(char *)obj->name,
	SS_COLLIDE_SETTLE_TILES,
	obj->x,
	obj->y
	);
}