#include <math.h>
#include <akstdlib.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 AKERR_NOIGNORE *akgl_physics_null_gravity(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    SUCCEED_RETURN(e);
}

akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_null_collide(akgl_PhysicsBackend *self, akgl_Actor *a1, akgl_Actor *a2)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    SUCCEED_RETURN(e);
}

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

akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_init_null(akgl_PhysicsBackend *self)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, 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;

    SUCCEED_RETURN(e);
}


akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_ss_gravity(akgl_PhysicsBackend *self, akgl_Actor *actor, float32_t dt)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(e, actor, AKERR_NULLPOINTER, "actor");
    
    // Assume the X origin is - (screen left)
    actor->ex -= (self->gravity_x * dt);
    // Assume Y origin is + (down screen)
    actor->ey += (self->gravity_y * dt);
    // Assume Z origin is - (behind the camera)
    actor->ez -= (self->gravity_z * dt);

    SUCCEED_RETURN(e);
}

akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_ss_collide(akgl_PhysicsBackend *self, akgl_Actor *a1, akgl_Actor *a2)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    FAIL_RETURN(e, AKERR_API, "Not implemented");
    SUCCEED_RETURN(e);
}

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

akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_init_sidescroller(akgl_PhysicsBackend *self)
{
    akgl_String *tmp;
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    PASS(e, akgl_heap_next_string(&tmp));
    
    self->gravity = akgl_physics_ss_gravity;
    self->collide = akgl_physics_ss_collide;
    self->move = akgl_physics_ss_move;
    self->simulate = akgl_physics_simulate;

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

akerr_ErrorContext AKERR_NOIGNORE *akgl_physics_simulate(akgl_PhysicsBackend *self, akgl_Iterator *opflags)
{
    PREPARE_ERROR(e);
    akgl_Iterator defflags = {
	.flags = 0,
	.layerid = 0
    };
    SDL_Time curtime = SDL_GetTicksNS();
    float32_t dt = (float32_t)(curtime - self->gravity_time) / (float32_t)AKGL_TIME_ONESEC_NS;
    akgl_Actor *actor = NULL;

    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(e, self->move, AKERR_NULLPOINTER, "self->move");

    if ( opflags == NULL ) {
	opflags = &defflags;
    }
    
    
    for ( int i = 0; i < AKGL_MAX_HEAP_ACTOR; i++ ) {
	actor = &HEAP_ACTOR[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;
	}

	// velocity equals thrust unless thrust exceeds max speed
	if ( fabsf(actor->tx) > fabsf(actor->sx) ) {
	    if ( actor->tx < 0 ) {
		actor->tx = -actor->sx;		
	    } else {
		actor->tx = actor->sx;
	    }
	}
	if ( fabsf(actor->ty) > fabsf(actor->sy) ) {
	    if ( actor->ty < 0 ) {
		actor->ty = -actor->sy;
	    } else {
		actor->ty = actor->sy;
	    }
	}
	if ( fabsf(actor->tz) > fabsf(actor->sz) ) {
	    if ( actor->tz < 0 ) {
		actor->tz = -actor->sz;
	    } else {
		actor->tz = actor->sz;
	    }
	}
	ATTEMPT {
	    CATCH(e, actor->movementlogicfunc(actor, dt));
	    PASS(e, self->gravity(self, actor, dt));

	    // Counteract velocity with atmospheric drag
	    actor->ex -= actor->ex * self->drag_x * dt;
	    actor->ey -= actor->ey * self->drag_y * dt;
	    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;

	    PASS(e, self->move(self, actor, dt)); 
	} CLEANUP {
	} PROCESS(e) {
	} HANDLE(e, AKGL_ERR_LOGICINTERRUPT) {
	    // noop
	} FINISH(e, true);
    }
    self->gravity_time = curtime; 
    SUCCEED_RETURN(e);
}