/**
 * @file physics.c
 * @brief Implements the physics subsystem.
 */

#include <math.h>
#include <akstdlib.h>
#include <akgl/collision.h>
#include <akgl/physics.h>
#include <akgl/actor.h>
#include <akgl/game.h>
#include <akgl/error.h>
#include <akgl/heap.h>
#include <akgl/registry.h>

akerr_ErrorContext *akgl_physics_null_gravity(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_null_collide(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");
    (void)dt;

    /*
     * Nothing collides, and that is an answer rather than a gap. A caller who
     * wants a world with no collision in it installs this backend and gets it,
     * without the simulation having to branch on whether collision exists.
     */
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_null_move(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_init_null(akgl_PhysicsBackend *self)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");

    self->gravity = akgl_physics_null_gravity;
    self->collide = akgl_physics_null_collide;
    self->move = akgl_physics_null_move;
    self->simulate = akgl_physics_simulate;

    // Set for the same reason as the arcade backend: the null backend still
    // goes through akgl_physics_simulate, which still computes a dt and still
    // commits velocity to position through akgl_physics_null_move.
    self->gravity_time = SDL_GetTicksNS();
    self->max_timestep = AKGL_PHYSICS_DEFAULT_MAX_TIMESTEP;

    SUCCEED_RETURN(errctx);
}


akerr_ErrorContext *akgl_physics_arcade_gravity(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");

    if ( self->gravity_x != 0 ) {
	// Assume the X origin is - (screen left)
	actor->ex -= (self->gravity_x * dt);
    }
    if ( self->gravity_y != 0 ) {
	// Assume Y origin is + (down screen)
	actor->ey += (self->gravity_y * dt);
    }
    if ( self->gravity_z != 0 ) {
	// Assume Z origin is - (behind the camera)
	actor->ez -= (self->gravity_z * dt);
    }

    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_arcade_collide(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");

    // No world means no collision, and costs one comparison. This is what makes
    // the feature opt-in: a backend nobody attached a world to runs exactly as
    // it did before any of this existed.
    if ( self->collision == NULL ) {
	SUCCEED_RETURN(errctx);
    }
    PASS(errctx, akgl_collision_resolve(self->collision, actor, dt));
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_arcade_move(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "actor");
    actor->x += actor->vx * dt;
    actor->y += actor->vy * dt;
    actor->z += actor->vz * dt;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_init_arcade(akgl_PhysicsBackend *self)
{
    akgl_String *tmp;
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    PASS(errctx, akgl_heap_next_string(&tmp));

    self->gravity = akgl_physics_arcade_gravity;
    self->collide = akgl_physics_arcade_collide;
    self->move = akgl_physics_arcade_move;
    self->simulate = akgl_physics_simulate;

    // The epoch the first step's dt is measured from. Nothing set it, so it
    // stayed at whatever the backend's storage held -- zero, for the default
    // backend in BSS -- and the first akgl_physics_simulate() measured dt as
    // the entire time since SDL started. A host that spends a quarter of a
    // second loading before its first frame got a quarter-second step: 101
    // pixels of fall where a 60 Hz frame is 0.44.
    self->gravity_time = SDL_GetTicksNS();

    ATTEMPT {
	CATCH(errctx, akgl_get_property("physics.gravity.x", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->gravity_x));
	CATCH(errctx, akgl_get_property("physics.gravity.y", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->gravity_y));
	CATCH(errctx, akgl_get_property("physics.gravity.z", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->gravity_z));
	CATCH(errctx, akgl_get_property("physics.drag.x", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->drag_x));
	CATCH(errctx, akgl_get_property("physics.drag.y", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->drag_y));
	CATCH(errctx, akgl_get_property("physics.drag.z", &tmp, "0.0"));
	CATCH(errctx, aksl_atof(tmp->data, &self->drag_z));
	CATCH(errctx, akgl_get_property("physics.max_timestep", &tmp, "0.05"));
	CATCH(errctx, aksl_atof(tmp->data, &self->max_timestep));
    } CLEANUP {
	IGNORE(akgl_heap_release_string(tmp));
    } PROCESS(errctx) {
    } FINISH(errctx, true);

    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_simulate(akgl_PhysicsBackend *self, akgl_Iterator *opflags)
{
    PREPARE_ERROR(errctx);
    akgl_Iterator defflags = {
	.flags = 0,
	.layerid = 0
    };
    SDL_Time curtime = 0;
    float32_t dt = 0;
    float32_t overshoot = 0.0f;
    float32_t thrustscale = 0.0f;
    float32_t subdt = 0.0f;
    int substeps = 1;
    int s = 0;
    akgl_Actor *actor = NULL;

    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, self->move, AKERR_NULLPOINTER, "self->move");
    // `gravity` was never checked, and is dereferenced unconditionally below. A
    // backend built by hand with only `move` filled in crashed here rather than
    // reporting; `collide` is allowed to be NULL and means "nothing collides".
    FAIL_ZERO_RETURN(errctx, self->gravity, AKERR_NULLPOINTER, "self->gravity");

    // Reading the elapsed time requires self, so it cannot be hoisted above
    // the null check.
    curtime = SDL_GetTicksNS();
    dt = (float32_t)(curtime - self->gravity_time) / (float32_t)AKGL_TIME_ONESEC_NS;

    // Bound the step. Everything below multiplies by dt, so one enormous dt
    // moves every actor by however far its velocity carries it over the whole
    // stall -- a quarter-second load is a hundred pixels of fall under
    // platformer gravity, straight through whatever was underneath. Advancing
    // the world in slow motion through a hitch is the trade to make here.
    //
    // A zero or negative max_timestep disables the bound, for a caller who
    // would rather have the real elapsed time and handle it themselves.
    if ( (self->max_timestep > 0.0) && (dt > (float32_t)self->max_timestep) ) {
	dt = (float32_t)self->max_timestep;
    }
    // A clock that went backwards is not a step. SDL_GetTicksNS is monotonic,
    // but gravity_time is a public field and a caller can put anything in it.
    if ( dt < 0.0f ) {
	dt = 0.0f;
    }

    if ( opflags == NULL ) {
	opflags = &defflags;
    }

    // One pass to give every shaped actor a proxy and take one back from
    // anything that lost its shape, so the loop below never has to.
    if ( self->collision != NULL ) {
	PASS(errctx, akgl_collision_sync_actors(self->collision));
    }


    for ( int i = 0; i < AKGL_MAX_HEAP_ACTOR; i++ ) {
	actor = &akgl_heap_actors[i];
	if ( actor->refcount == 0 ) {
	    continue;
	}
	if ( actor->parent != NULL ) {
	    // Children don't move independently of their parents, they just have an offset
	    actor->x = actor->parent->x + actor->vx;
	    actor->y = actor->parent->y + actor->vy;
	    actor->z = actor->parent->z + actor->vz;
	    continue;
	} else if ( actor->basechar == NULL ) {
	    continue;
	}
	if ( AKGL_BITMASK_HAS(opflags->flags, AKGL_ITERATOR_OP_LAYERMASK) ) {
	    if ( actor->layer != opflags->layerid ) {
		continue;
	    }
	}

	// thrust is a function of acceleration on a given axis
	if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_LEFT) ||
	     AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_RIGHT) ) {
	    actor->tx += actor->ax * dt;
	}
	if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_UP) ||
	     AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_DOWN) ) {
	    actor->ty += actor->ay * dt;
	}

	// Cap the thrust *vector* against the ellipse the character's per-axis
	// top speeds describe, rather than each axis against its own cap.
	//
	// Capping the axes independently lets the corner of the box through: an
	// actor holding two directions at once got both caps at once and
	// travelled their diagonal, which measured 41% faster than either alone.
	// Scaling to the ellipse keeps a character whose horizontal and vertical
	// speeds differ moving at the ratio it asked for, and makes them equal
	// where the speeds are.
	//
	// An axis with a top speed of zero cannot be thrust along at all, which
	// is what the old per-axis clamp did with it, and it stays out of the
	// magnitude entirely -- dividing by it would not end well.
	overshoot = 0.0f;
	if ( actor->sx != 0.0f ) {
	    overshoot += (actor->tx / actor->sx) * (actor->tx / actor->sx);
	} else {
	    actor->tx = 0.0f;
	}
	if ( actor->sy != 0.0f ) {
	    overshoot += (actor->ty / actor->sy) * (actor->ty / actor->sy);
	} else {
	    actor->ty = 0.0f;
	}
	if ( actor->sz != 0.0f ) {
	    overshoot += (actor->tz / actor->sz) * (actor->tz / actor->sz);
	} else {
	    actor->tz = 0.0f;
	}
	if ( overshoot > 1.0f ) {
	    thrustscale = 1.0f / sqrtf(overshoot);
	    actor->tx *= thrustscale;
	    actor->ty *= thrustscale;
	    actor->tz *= thrustscale;
	}
	ATTEMPT {
	    CATCH(errctx, actor->movementlogicfunc(actor, dt));
	    PASS(errctx, self->gravity(self, actor, dt));

	    // Counteract velocity with atmospheric drag
	    if ( self->drag_x != 0 ) {
		actor->ex -= actor->ex * self->drag_x * dt;
	    }
	    if ( self->drag_y != 0 ) {
		actor->ey -= actor->ey * self->drag_y * dt;
	    }
	    if ( self->drag_z != 0 ) {
		actor->ez -= actor->ez * self->drag_z * dt;
	    }

	    actor->vx = actor->ex + actor->tx;
	    actor->vy = actor->ey + actor->ty;
	    actor->vz = actor->ez + actor->tz;

	    /*
	     * Only `move` is subdivided. Gravity, drag, the thrust integration
	     * and the speed-ellipse cap above all ran once against the whole dt,
	     * and sum(subdt) is dt -- so an actor with nothing to collide with
	     * takes exactly one sub-step of exactly dt and follows the arithmetic
	     * path it always did. That is what lets the integrator stay untouched
	     * and every recorded physics number stay valid.
	     */
	    if ( (self->collision == NULL) || (self->collide == NULL) ) {
		/*
		 * No collision: exactly the call this has always made, with
		 * nothing added around it. Not merely equivalent -- CATCH and
		 * PASS walk AKERR_ARRAY_ERROR to validate the context, which
		 * costs more than several of the calls being measured, so even
		 * a check that answers "no" instantly is not free if it is
		 * reached through one. Measured: routing the off path through
		 * one extra CATCH took the 64-actor sweep from 1.2 us to 2.0.
		 */
		PASS(errctx, self->move(self, actor, dt));
	    } else {
		CATCH(errctx, akgl_collision_substeps(self->collision, actor, dt, &substeps));
		subdt = dt / (float32_t)substeps;
		for ( s = 0; s < substeps; s++ ) {
		    PASS(errctx, self->move(self, actor, subdt));
		    PASS(errctx, self->collide(self, actor, subdt));
		}
	    }
	} CLEANUP {
	} PROCESS(errctx) {
	} HANDLE(errctx, AKGL_ERR_LOGICINTERRUPT) {
	    // noop
	} FINISH(errctx, true);
    }
    /*
     * Re-snap children after everything has moved, but only when collision is
     * on. The in-loop snap uses whatever position the parent had when the
     * child's own slot came up in pool order, which is already sometimes a
     * frame stale -- harmless while a parent only ever moved by `v * dt`, and
     * not harmless once a parent can also be pushed out of a wall part way
     * through its own sub-steps. Gating it keeps a collision-free world
     * byte-for-byte what it was.
     */
    if ( self->collision != NULL ) {
	for ( int i = 0; i < AKGL_MAX_HEAP_ACTOR; i++ ) {
	    actor = &akgl_heap_actors[i];
	    if ( (actor->refcount == 0) || (actor->parent == NULL) ) {
		continue;
	    }
	    actor->x = actor->parent->x + actor->vx;
	    actor->y = actor->parent->y + actor->vy;
	    actor->z = actor->parent->z + actor->vz;
	}
    }

    self->gravity_time = curtime;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_physics_factory(akgl_PhysicsBackend *self, akgl_String *type)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(errctx, type, AKERR_NULLPOINTER, "type");

    if ( strncmp(type->data, "null", 4) == 0) {
	PASS(errctx, akgl_physics_init_null(self));
	SUCCEED_RETURN(errctx);
    }
    if ( strncmp(type->data, "arcade", 6) == 0) {
	PASS(errctx, akgl_physics_init_arcade(self));
	SUCCEED_RETURN(errctx);
    }
    FAIL_RETURN(errctx, AKERR_KEY, "Invalid physics engine %s", type->data);
}