#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, SDL_Time curtime)
{
    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, SDL_Time curtime)
{
    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, SDL_Time curtime)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    // Do nothing
    // Calculate the drop of all relevant actors which is a function of their mass and the
    // world's gravity

    //Gravity is applied in units per second. So we apply
    // (unit / milliseconds per second) * (milliseconds since last update)
    actor->vx += ((self->gravity_x / AKGL_TIME_ONESEC_NS) * (curtime - self->gravity_time));
    actor->vy += ((self->gravity_y / AKGL_TIME_ONESEC_NS) * (curtime - self->gravity_time));
    actor->vz += ((self->gravity_z / AKGL_TIME_ONESEC_NS) * (curtime - self->gravity_time));

    // Apply atmospheric drag
    actor->vx -= actor->vx * self->drag_x * (curtime - self->gravity_time);
    actor->vy -= actor->vy * self->drag_x * (curtime - self->gravity_time);
    actor->vz -= actor->vz * self->drag_x * (curtime - self->gravity_time);
    // Need a euler function
    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, SDL_Time curtime)
{
    PREPARE_ERROR(e);
    FAIL_ZERO_RETURN(e, self, AKERR_NULLPOINTER, "self");
    FAIL_ZERO_RETURN(e, actor, AKERR_NULLPOINTER, "actor");

    if ( actor->parent != NULL ) {
	// Children don't move independently of their parents, they just have an offset
	SUCCEED_RETURN(e);
    } else if ( actor->basechar == NULL ) {
	SUCCEED_RETURN(e);
    } else { 
	if ( (curtime - actor->movetimer) >= actor->basechar->speedtime ) {
	    actor->movetimer = curtime;
	    ATTEMPT {
		CATCH(e, actor->movementlogicfunc(actor,curtime));
	    } CLEANUP {
	    } PROCESS(e) {
	    } HANDLE(e, AKGL_ERR_LOGICINTERRUPT) {
		// The actor told us NOT to process them, they handled their own update
		SUCCEED_RETURN(e);
	    } FINISH(e, true);
	    if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_LEFT) ) {
		actor->x += -actor->vx;
	    }
	    if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_RIGHT) ) {
		actor->x += actor->vx;
	    }
	    if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_UP) ) {
		actor->y += -actor->vy;
	    }
	    if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_DOWN) ) {
		actor->y += actor->vy;
	    }
	}
    }
    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();
    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 ( AKGL_BITMASK_HAS(opflags->flags, AKGL_ITERATOR_OP_LAYERMASK) ) {
	    if ( actor->layer != opflags->layerid ) {
		continue;
	    }
	}
	if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_LEFT) ||
	    AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_RIGHT) ) {
	    actor->vx += actor->basechar->ax;
	}
	if ( AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_UP) ||
	    AKGL_BITMASK_HAS(actor->state, AKGL_ACTOR_STATE_MOVING_DOWN) ) {
	    actor->vy += actor->basechar->ay;
	}
	PASS(e, self->gravity(self, actor, curtime));
	PASS(e, self->move(self, actor, curtime)); 
	self->gravity_time = curtime;
    }
    SUCCEED_RETURN(e);
}