#include"game.h"

#include"k4.h"
#include<assert.h>
#include"resman.h"
#include<cglm/vec3.h>

#define SUBFEET(cp) (cp->capsule.radius)

struct Game Game;

void game_init() {
	static int initode = 0;
	if(initode == 0) {
		dInitODE();
		initode = 1;
	}
	
	k3Log(k3_INFO, "ODE config: %s", dGetConfiguration());
	
	memset(&Game, 0, sizeof(Game));
	Game.spectated = ENT_ID_INVALID;
	Game.controlled = ENT_ID_INVALID;
	
	// Assume singleplayer first
	Game.isMultiplayer = 0;
	Game.isAuthority = 1;
	Game.reconciliate = 1;
	
	game_cleanup();
}

struct CollisionPair {
	uint16_t e1; // greater
	uint16_t e2; // lesser
	uint8_t x;
} __attribute__((packed));
static size_t activeCollisionCount, activeCollisionCapacity;
static struct CollisionPair *activeCollisions;
int pair_comparator(const void *a_, const void *b_) {
	const struct CollisionPair *a = a_;
	const struct CollisionPair *b = b_;
	if(a->e1 == b->e1) {
		return (intmax_t) a->e2 - (intmax_t) b->e2;
	} else {
		return (intmax_t) a->e1 - (intmax_t) b->e1;
	}
}
static int activate_pair(uint16_t e1, uint16_t e2) {
	struct CollisionPair p = {
		.e1 = e1 > e2 ? e1 : e2,
		.e2 = e1 > e2 ? e2 : e1,
	};
	
	struct CollisionPair *peepee = bsearch(&p, activeCollisions, activeCollisionCount, sizeof(struct CollisionPair), pair_comparator);
	
	if(peepee) {
		peepee->x = 2;
		return TRIGGER_EV_CONTINUOUS;
	}
	
	if(activeCollisionCount == activeCollisionCapacity) {
		activeCollisions = realloc(activeCollisions, sizeof(*activeCollisions) * (activeCollisionCapacity += 32));
	}
	
	p.x = 2;
	
	memcpy(&activeCollisions[activeCollisionCount++], &p, sizeof(p));
	
	qsort(activeCollisions, activeCollisionCount, sizeof(struct CollisionPair), pair_comparator);
	
	return TRIGGER_EV_IN;
}
static void tick_pairs() {
	for(size_t i = 0; i < activeCollisionCount;) {
		if(--activeCollisions[i].x == 0) {
			uint16_t e1 = activeCollisions[i].e1;
			uint16_t e2 = activeCollisions[i].e2;
			
			if(e1 != ENT_ID_INVALID) {
				struct CPhysics *p1 = game_getcomponent(e1, physics);
				if(p1 && p1->trigger != TRIGGER_INVALID) {
					Game.triggers[p1->trigger - 1](p1->trigger, e1, e2, TRIGGER_EV_OUT);
				}
			}
			
			if(e2 != ENT_ID_INVALID) {
				struct CPhysics *p2 = game_getcomponent(e2, physics);
				if(p2 && p2->trigger != TRIGGER_INVALID) {
					Game.triggers[p2->trigger - 1](p2->trigger, e2, e1, TRIGGER_EV_OUT);
				}
			}
			
			memmove(&activeCollisions[i], &activeCollisions[i + 1], sizeof(*activeCollisions) * (activeCollisionCount - i - 1));
			activeCollisionCount--;
		} else {
			i++;
		}
	}
}

static void vec3_project_to_plane(vec3 v, vec3 n, vec3 ret) {
	float scale = glm_vec3_dot(v, n) / glm_vec3_dot(n, n);
	
	vec3 n2;
	glm_vec3_scale(n, scale, n2);
	
	glm_vec3_sub(v, n2, ret);
}

static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
	uint16_t e1 = (uintptr_t) dGeomGetData(g1);
	uint16_t e2 = (uintptr_t) dGeomGetData(g2);
	
	if(e1 != ENT_ID_INVALID && e1 == e2) return;
	
	dBodyID b1 = dGeomGetBody(g1);
	dBodyID b2 = dGeomGetBody(g2);
	
	int movingPlatform = 0;
	if(b1 && dBodyIsKinematic(b1)) movingPlatform = 1;
	else if(b2 && dBodyIsKinematic(b2)) movingPlatform = 2;
	
	#define MAX_CONTACTS 8
	dContact contact[MAX_CONTACTS];
	memset(contact, 0, sizeof(contact));
	
	int numc = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
	
	for(int i = 0; i < numc; i++) {
		contact[i].surface.mode = dContactRolling;
		contact[i].surface.mu = 2;
		contact[i].surface.mu2 = 0;
		contact[i].surface.bounce = 0.01;
		contact[i].surface.bounce_vel = 0.1;
		
		if(movingPlatform) {
			const dReal *platvel = dBodyGetLinearVel(movingPlatform == 1 ? b1 : b2);
			
			contact[i].surface.mode |= dContactMotion1 | dContactMotion2 | dContactMotionN | dContactFDir1;
			contact[i].surface.mode |= dContactSoftERP;
			contact[i].surface.soft_erp = 0.9;
			
			const dReal *normal = contact[i].geom.normal;
			dVector3 fdir1, fdir2;
			dPlaneSpace(normal, fdir1, fdir2);
			
			glm_vec3_copy(contact[i].fdir1, fdir1);
			
			dReal inv = movingPlatform == 1 ? -1 : 1;
			
			contact[i].surface.motion1 = dDOT(platvel, fdir1);
			contact[i].surface.motion2 = inv * dDOT(platvel, fdir2);
			contact[i].surface.motionN = -fabs(dDOT(platvel, normal)); //This makes no physical sense but makes objects not bounce on moving platforms going up
		}
	}
	
	int ghost = 0;
	
	if(dGeomGetCategoryBits(g1) & CATEGORY_GHOST) ghost = 1;
	if(dGeomGetCategoryBits(g2) & CATEGORY_GHOST) ghost = 1;
	
	if(numc) {
		int triggerType = activate_pair(e1, e2);
		
		if(e1 != ENT_ID_INVALID) {
			struct CPhysics *cp = game_getcomponent(e1, physics);
			if(!cp) {
				// Entity being killed maybe
				return;
			}
			
			if(cp->dynamics & CPHYSICS_GHOST) {
				ghost = 1;
			}
			
			struct CMovement *cm = game_getcomponent(e1, movement);
			if(cm && cm->holding != ENT_ID_INVALID && cm->holding == e2) {
				ghost = 1;
			}
		}
		
		if(e2 != ENT_ID_INVALID) {
			struct CPhysics *cp = game_getcomponent(e2, physics);
			if(!cp) {
				// Entity being killed maybe
				return;
			}
			
			if(cp->dynamics & CPHYSICS_GHOST) {
				ghost = 1;
			}
			
			struct CMovement *cm = game_getcomponent(e2, movement);
			if(cm && cm->holding != ENT_ID_INVALID && cm->holding == e1) {
				ghost = 1;
			}
		}
		
		float friction = 1;
		
		if(e1 != ENT_ID_INVALID) {
			struct CPhysics *c = game_getcomponent(e1, physics);
			
			friction *= c->friction;
			
			if(c->trigger != TRIGGER_INVALID) {
				Game.triggers[c->trigger - 1](c->trigger, e1, e2, triggerType);
			}
		}
		
		if(e2 != ENT_ID_INVALID) {
			struct CPhysics *c = game_getcomponent(e2, physics);
			
			friction *= c->friction;
			
			if(c->trigger != TRIGGER_INVALID) {
				Game.triggers[c->trigger - 1](c->trigger, e2, e1, triggerType);
			}
		}
		
		for(int i = 0; i < numc; i++) {
			contact[i].surface.mu = friction;
		}
	}
	
	if(!ghost) for(int i = 0; i < numc; i++) {
		dJointID c = dJointCreateContact(Game.phys, Game.contactgroup, contact + i);
		dJointAttach(c, dGeomGetBody(g1), dGeomGetBody(g2));
	}
}

struct RaycastCtx {
	struct LocalRay *rays;
	size_t count;
};
static void raycast_handler(void *data, dGeomID g1, dGeomID g2) {
	struct RaycastCtx *ctx = data;
	
	if(dGeomGetClass(g1) != dRayClass && dGeomGetClass(g2) != dRayClass) {
		return;
	}
	
	dContact contact[1];
	memset(contact, 0, sizeof(contact));
	
	int numc = dCollide(g1, g2, 1, &contact[0].geom, sizeof(dContact));
	
	uint16_t e1 = (uintptr_t) dGeomGetData(g1);
	uint16_t e2 = (uintptr_t) dGeomGetData(g2);
	
	if(numc) {
		
		if(dGeomGetClass(g1) == dRayClass) {
			struct LocalRay *r = &ctx->rays[(uintptr_t) dGeomGetData(g1)];
			if(r->ignore != e2 && contact->geom.depth < r->depth) {
				struct CPhysics *asdf = game_getcomponent(e2, physics);
				if(!asdf || !(asdf->dynamics & CPHYSICS_GHOST)) {
					glm_vec3_copy(contact[0].geom.pos, r->out);
					r->hit = e2;
					r->depth = contact->geom.depth;
				}
			}
			return;
		} else if(dGeomGetClass(g2) == dRayClass) {
			struct LocalRay *r = &ctx->rays[(uintptr_t) dGeomGetData(g2)];
			if(r->ignore != e1 && contact->geom.depth < r->depth) {
				struct CPhysics *asdf = game_getcomponent(e1, physics);
				if(!asdf || !(asdf->dynamics & CPHYSICS_GHOST)) {
					glm_vec3_copy(contact[0].geom.pos, r->out);
					r->hit = e1;
					r->depth = contact->geom.depth;
				}
			}
			return;
		}
	}
}
void game_raycast(struct LocalRay *rays, size_t count) {
	dGeomID rayGeoms[count];
	
	for(int i = 0; i < count; i++) {
		const float len = rays[i].maxlen;
		
		glm_vec3_copy(rays[i].pos, rays[i].out);
		glm_vec3_muladds(rays[i].dir, len, rays[i].out);
		
		rayGeoms[i] = dCreateRay(Game.space, len);
		
		dGeomSetData(rayGeoms[i], (void*) (uintptr_t) i);
		
		dGeomSetCategoryBits(rayGeoms[i], CATEGORY_RAY);
		dGeomSetCollideBits(rayGeoms[i], CATEGORY_STATIC | CATEGORY_ENTITY);
		
		dGeomRaySet(rayGeoms[i], rays[i].pos[0], rays[i].pos[1], rays[i].pos[2], rays[i].dir[0], rays[i].dir[1], rays[i].dir[2]);
		
		// Broken. We manually find closest hit with contact depths.
		//dGeomRaySetClosestHit(rayGeoms[i], 1);
		
		rays[i].hit = ENT_ID_INVALID;
		rays[i].depth = HUGE_VALF;
	}
	
	dSpaceCollide(Game.space, &(struct RaycastCtx) {
		.rays = rays,
		.count = count,
	}, raycast_handler);
	
	for(int i = 0; i < count; i++) {
		dGeomDestroy(rayGeoms[i]);
	}
}

static void game_character_controller_raycast_handler(void *data, dGeomID g1, dGeomID g2) {
	if(dGeomGetClass(g1) != dRayClass && dGeomGetClass(g2) != dRayClass) {
		return;
	}
	
	uint16_t e1 = (uintptr_t) dGeomGetData(g1);
	uint16_t e2 = (uintptr_t) dGeomGetData(g2);
	
	if(e1 == e2) {
		// This means the ray is hitting the entity casting it.
		return;
	}
	
	dContact contact[1];
	memset(contact, 0, sizeof(contact));
	
	int numc = dCollide(g1, g2, 1, &contact[0].geom, sizeof(dContact));
	
	if(numc) {
		
		vec3 n = {0, -1, 0};
		
		struct CPhysics *cp1 = game_getcomponent(e1, physics);
		struct CPhysics *cp2 = game_getcomponent(e2, physics);
		
		if((cp2 && (cp2->dynamics & CPHYSICS_GHOST)) || (cp1 && (cp1->dynamics & CPHYSICS_GHOST))) {
			return;
		}
		
		if(dGeomGetClass(g1) == dRayClass) {
			dBodyID bid = dGeomGetBody(cp1->geom);
			
			struct CMovement *cm = game_getcomponent(e1, movement);
			
			cm->groundDepth = dGeomRayGetLength(g1) - contact[0].geom.depth;
			
			if(glm_vec3_dot(contact[0].geom.normal, n) <= -0.7 && cm->groundDepth > SUBFEET(cp1)) {
				cm->canJump = 1;
				glm_vec3_scale(contact[0].geom.normal, -1, cm->groundNormal);
			}
		} else if(dGeomGetClass(g2) == dRayClass) {
			dBodyID bid = dGeomGetBody(cp2->geom);
			
			struct CMovement *cm = game_getcomponent(e2, movement);
			
			cm->groundDepth = dGeomRayGetLength(g2) - contact[0].geom.depth;
			
			if(glm_vec3_dot(contact[0].geom.normal, n) >= +0.7 && cm->groundDepth > SUBFEET(cp2)) {
				cm->canJump = 1;
				glm_vec3_scale(contact[0].geom.normal, +1, cm->groundNormal);
			}
		}
		
	}
}
void game_character_controller_raycast() {
	dGeomID rayGeoms[Game.entities.movementCount];
	
	for(size_t i = 0; i < Game.entities.movementCount; i++) {
		Game.entities.movement[i].canJump = 0;
	}
	
	for(int i = 0; i < Game.entities.movementCount; i++) {
		struct CPhysics *cp = game_getcomponent(Game.entities.movement[i].entity, physics);
		
		if(!cp || !cp->geom || cp->type != CPHYSICS_CAPSULE) {
			rayGeoms[i] = NULL;
			continue;
		}
		
		rayGeoms[i] = dCreateRay(Game.space, cp->capsule.length / 2 + cp->capsule.radius + SUBFEET(cp));
		
		dGeomSetData(rayGeoms[i], (void*) (uintptr_t) Game.entities.movement[i].entity);
		
		dGeomSetCategoryBits(rayGeoms[i], CATEGORY_RAY);
		dGeomSetCollideBits(rayGeoms[i], CATEGORY_STATIC | CATEGORY_ENTITY);
		
		dBodyID bid = dGeomGetBody(cp->geom);
		
		const float *position = dBodyGetPosition(bid);
		
		dGeomRaySet(rayGeoms[i], position[0], position[1], position[2], 0, -1, 0);
	}
	
	dSpaceCollide(Game.space, NULL, game_character_controller_raycast_handler);
	
	for(int i = 0; i < Game.entities.movementCount; i++) {
		if(rayGeoms[i]) {
			dGeomDestroy(rayGeoms[i]);
		}
	}
}

void game_update() {
	game_character_controller_raycast();
	
	for(size_t i = 0; i < Game.entities.playerctrlCount; i++) {
		struct CPlayerCtrl *cc = &Game.entities.playerctrl[i];
		
		struct CMovement *cm = game_getcomponent(cc->entity, movement);
		
		struct CPhysics *cp = game_getcomponent(cc->entity, physics);
		
		if(cm && cp) {
			vec3 shift = {0, 0, 0};
			
			if(cc->keys & (1 << k4_control_get_id_by_name("forward"))) shift[2] -= 1;
			if(cc->keys & (1 << k4_control_get_id_by_name("back"))) shift[2] += 1;
			if(cc->keys & (1 << k4_control_get_id_by_name("left"))) shift[0] -= 1;
			if(cc->keys & (1 << k4_control_get_id_by_name("right"))) shift[0] += 1;
			glm_vec3_rotate(shift, cc->yaw, (vec3) {0, 1, 0});
			
			if(Game.isAuthority || Game.reconciliate) {
				glm_vec3_copy(shift, cm->dir);
			}
			cm->pointing = cc->yaw;
			cm->jump = !!(cc->keys & (1 << k4_control_get_id_by_name("jump")));
			
			if(cm->holding == ENT_ID_INVALID && (cc->keys & (1 << k4_control_get_id_by_name("grab")))) {
				vec3 d = {0, 0, -1};
				glm_vec3_rotate(d, cc->pitch, (vec3) {1, 0, 0});
				glm_vec3_rotate(d, cc->yaw, (vec3) {0, 1, 0});
				
				glm_vec3_copy(dGeomGetPosition(cp->geom), cc->holdray.pos);
				glm_vec3_copy(d, cc->holdray.dir);
				cc->holdray.ignore = cc->entity;
				
				cc->holdray.maxlen = 3;
				
				game_raycast(&cc->holdray, 1);
			} else if(!(cc->keys & (1 << k4_control_get_id_by_name("grab")))) {
				cm->holding = ENT_ID_INVALID;
			}
		}
	}
	
	for(size_t i = 0; i < Game.entities.movementCount; i++) {
		struct CPhysics *cp = game_getcomponent(Game.entities.movement[i].entity, physics);
		if(cp && cp->geom) {
			dBodyID bid = dGeomGetBody(cp->geom);
			if(Game.entities.movement[i].canJump) {
				if(dBodyGetLinearVel(bid)[1] <= 0) {
					Game.entities.movement[i].isJumping = false;
				}
				
				if(!Game.entities.movement[i].isJumping) {
					const float *pos = dBodyGetPosition(bid);
					dBodySetPosition(bid, pos[0], pos[1] + Game.entities.movement[i].groundDepth - cp->capsule.radius / 2 - SUBFEET(cp), pos[2]);
					
					dBodySetLinearVel(bid, 0, 0, 0);
				}
			}
			dBodySetGravityMode(bid, Game.entities.movement[i].canJump && !Game.entities.movement[i].isJumping ? 0 : 1);
		}
	}
	
	size_t ci = 0, mi = 0;
	for(size_t i = 0; i < Game.entities.physicsCount; i++) {
		if(Game.entities.physics[i].geom == NULL) {
			/* Create */
			
			dMass mass;
			dMassSetZero(&mass);
			
			switch(Game.entities.physics[i].type) {
			default:
				
				if(!Game.entities.physics[i].trimesh.cache) {
					Game.entities.physics[i].trimesh.cache = resman_ref(RESMAN_PHYSICS, Game.entities.physics[i].trimesh.name);
				}
				
				Game.entities.physics[i].geom = dCreateTriMesh(Game.space, Game.entities.physics[i].trimesh.cache->trid, NULL, NULL, NULL);
				
				dMassSetTrimesh(&mass, 1, Game.entities.physics[i].geom);
				
				break;
			case CPHYSICS_BOX:
				Game.entities.physics[i].geom = dCreateBox(Game.space,
					Game.entities.physics[i].box.w,
					Game.entities.physics[i].box.h,
					Game.entities.physics[i].box.l);
				
				dMassSetBoxTotal(&mass, Game.entities.physics[i].mass,
					Game.entities.physics[i].box.w,
					Game.entities.physics[i].box.h,
					Game.entities.physics[i].box.l);
				
				break;
			case CPHYSICS_CAPSULE: {
				dGeomID top = dCreateCapsule(Game.space,
					Game.entities.physics[i].capsule.radius,
					Game.entities.physics[i].capsule.length - Game.entities.physics[i].capsule.radius);
					
				dGeomID feet = dCreateSphere(Game.space,
					Game.entities.physics[i].capsule.radius);
				
				Game.entities.physics[i].geom = top;
				Game.entities.physics[i].geom2 = feet;
				
				dMassSetCapsuleTotal(&mass, Game.entities.physics[i].mass, 2, Game.entities.physics[i].capsule.radius, Game.entities.physics[i].capsule.length);
				
				break;
			}
			case CPHYSICS_SPHERE:
				Game.entities.physics[i].geom = dCreateSphere(Game.space,
					Game.entities.physics[i].sphere.radius);
				
				dMassSetSphereTotal(&mass, Game.entities.physics[i].mass, Game.entities.physics[i].sphere.radius);
				
				break;
			}
			
			dGeomSetCategoryBits(Game.entities.physics[i].geom, CATEGORY_ENTITY);
			dGeomSetCollideBits(Game.entities.physics[i].geom, Game.entities.physics[i].collide);
			dGeomSetData(Game.entities.physics[i].geom, (void*) Game.entities.physics[i].entity);
			
			if(Game.entities.physics[i].type == CPHYSICS_CAPSULE) {
				dGeomSetCategoryBits(Game.entities.physics[i].geom2, CATEGORY_ENTITY | CATEGORY_GHOST);
				dGeomSetCollideBits(Game.entities.physics[i].geom2, Game.entities.physics[i].collide);
				dGeomSetData(Game.entities.physics[i].geom2, (void*) Game.entities.physics[i].entity);
			}
			
			if((Game.entities.physics[i].dynamics & ~CPHYSICS_GHOST) != CPHYSICS_STATIC) {
				dBodyID body = dBodyCreate(Game.phys);
				
				if((Game.entities.physics[i].dynamics & ~CPHYSICS_GHOST) == CPHYSICS_KINEMATIC) {
					dBodySetKinematic(body);
				} else {
					dBodySetMass(body, &mass);
				}
				
				dGeomSetBody(Game.entities.physics[i].geom, body);
				if(Game.entities.physics[i].geom2) {
					dGeomSetBody(Game.entities.physics[i].geom2, body);
				}
				
				if(Game.entities.physics[i].type == CPHYSICS_CAPSULE) {
					dBodySetMaxAngularSpeed(body, 0);
					
					dGeomSetOffsetPosition(Game.entities.physics[i].geom, 0, Game.entities.physics[i].capsule.radius / 2, 0);
					dGeomSetOffsetPosition(Game.entities.physics[i].geom2, 0, -Game.entities.physics[i].capsule.length / 2, 0);
					
					// Rotate to Y-up
					dQuaternion q;
					dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5);
					dGeomSetOffsetQuaternion(Game.entities.physics[i].geom, q);
				}
			}
			
			dGeomSetPosition(Game.entities.physics[i].geom,
				Game.entities.physics[i].start[0],
				Game.entities.physics[i].start[1],
				Game.entities.physics[i].start[2]);
			
			dGeomSetQuaternion(Game.entities.physics[i].geom, (dQuaternion) {
				Game.entities.physics[i].rot[3],
				Game.entities.physics[i].rot[0],
				Game.entities.physics[i].rot[1],
				Game.entities.physics[i].rot[2]
			});
			
			if((Game.entities.physics[i].dynamics & ~CPHYSICS_GHOST) == CPHYSICS_DYNAMIC) {
				dBodySetLinearVel(dGeomGetBody(Game.entities.physics[i].geom),
					Game.entities.physics[i].vel[0],
					Game.entities.physics[i].vel[1],
					Game.entities.physics[i].vel[2]);
			}
		}
		
		dGeomID gid = Game.entities.physics[i].geom;
		dBodyID bid = dGeomGetBody(gid);
		
		// Find corresponding movement component
		while(mi < Game.entities.movementCount && Game.entities.movement[mi].entity < Game.entities.physics[i].entity) {
			mi++;
		}
		
		if(mi < Game.entities.movementCount && Game.entities.movement[mi].entity == Game.entities.physics[i].entity) {
			if(Game.entities.movement[mi].holding != ENT_ID_INVALID) {
				struct CPhysics *hp = game_getcomponent(Game.entities.movement[mi].holding, physics);
				const float *p = dGeomGetPosition(gid);
				dGeomSetPosition(hp->geom, p[0] - sinf(Game.entities.movement[mi].pointing) * 1.4, p[1], p[2] - cosf(Game.entities.movement[mi].pointing) * 1.4);
				dBodyID bid = dGeomGetBody(hp->geom);
				if(bid) {
					dBodySetLinearVel(bid, 0, 0, 0);
				}
			}
			
			vec3 wishvel;
			glm_vec3_scale(Game.entities.movement[mi].dir, Game.entities.physics[i].speed, wishvel);
			
#ifdef RETARDED_MOVEMENT
			dBodySetLinearVel(bid, wishvel[0], dBodyGetLinearVel(bid)[1], wishvel[2]);
#else
			int inAir = !Game.entities.movement[mi].canJump;
			
			float factor = (inAir ? 8 : 20) * (GAME_TPS / 25);
			
			if(inAir) {
				if(glm_vec3_norm(wishvel) > 3) {
					glm_vec3_scale_as(wishvel, 3, wishvel);
				}
			} else {
				// We project the movement vector onto the ground plane (set by game_character_controller_raycast_handler)
				// to "slide" along the ground with minimal bouncing
				vec3_project_to_plane(wishvel, Game.entities.movement[mi].groundNormal, wishvel);
			}
			
			float currentspeed = glm_vec3_dot(wishvel, dBodyGetLinearVel(bid));
			float addspeed = glm_vec3_norm(wishvel) - currentspeed;
			if(addspeed > 0) {
				dMass massiveness;
				dBodyGetMass(bid, &massiveness);
				
				vec3 accel;
				glm_vec3_scale_as(wishvel, addspeed, accel);
				
				vec3 force;
				glm_vec3_scale(accel, massiveness.mass * factor, force);
				
				dBodyAddForce(bid, force[0], force[1], force[2]);
			}
#endif
			
			dQuaternion q;
			dQFromAxisAndAngle(q, 0, 1, 0, Game.entities.movement[mi].pointing + M_PI);
			dBodySetQuaternion(bid, q);
			
			if(Game.entities.movement[mi].jump && Game.entities.movement[mi].canJump) {
				dVector3 force = {};
				dWorldImpulseToForce(Game.phys, 1.f / GAME_TPS, 0, 6.5, 0, force);
				
				if(bid) {
					dBodyAddForce(bid, force[0], force[1], force[2]);
				}
				
				Game.entities.movement[mi].isJumping = true;
			}
		}
	}
	
	dSpaceCollide(Game.space, 0, &contact_callback);
	
	tick_pairs();
	
	dWorldSetQuickStepNumIterations(Game.phys, 60);
	dWorldSetQuickStepW(Game.phys, 1);
	dWorldQuickStep(Game.phys, 1.0f / GAME_TPS);
	
	dJointGroupEmpty(Game.contactgroup);
	
	for(size_t i = 0; i < Game.entities.playerctrlCount; i++) {
		struct CPlayerCtrl *cc = &Game.entities.playerctrl[i];
		struct CMovement *cm = game_getcomponent(cc->entity, movement);
		
		if(cc->holdray.hit != ENT_ID_INVALID && (cc->keys & (1 << k4_control_get_id_by_name("grab"))) && cm->holding == ENT_ID_INVALID) {
			dGeomID hd = game_getcomponent(cc->holdray.hit, physics)->geom;
			dBodyID hb = dGeomGetBody(hd);
			if(hb && !dBodyIsKinematic(hb)) {
				cm->holding = cc->holdray.hit;
			}
		}
	}
	
	for(size_t ci = 0; ci < Game.entities.renderCount; ci++) {
		struct CPhysics *cp = game_getcomponent(Game.entities.render[ci].entity, physics);
		
		if(!Game.entities.render[ci].cache && strlen(Game.entities.render[ci].mdl)) {
			struct k3Mdl *srccache = resman_ref(RESMAN_MODEL, Game.entities.render[ci].mdl);
			Game.entities.render[ci].cache = k3MdlCopySubs(srccache);
		}
		
		size_t boneCount = Game.entities.render[ci].cache ? k3MdlGetBoneCount(Game.entities.render[ci].cache) : 0;
		if(boneCount) {
			/*struct CBoned *cb = game_getcomponent(Game.entities.render[ci].entity, boned);
			
			if(!cb) {
				cb = game_ensurecomponent(Game.entities.render[ci].entity, boned);
			}
			
			if(cb->size < boneCount) {
				size_t start;
				
				if(cb->bones) {
					start = cb->size;
					
					struct k3AnimationBone *nu = _mm_malloc(sizeof(*cb->bones) * boneCount, 16);
					memcpy(nu, cb->bones, sizeof(*cb->bones) * cb->size);
					struct k3AnimationBone *ol = cb->bones;
					cb->bones = nu;
					_mm_free(ol);
				} else {
					start = 0;
					
					cb->bones = _mm_malloc(sizeof(*cb->bones) * boneCount, 16);
				}
				
				cb->size = boneCount;
				
				for(size_t asdf = start; asdf < boneCount; asdf++) {
					cb->bones[asdf].rotation[3] = cb->bones[asdf].translation[3] = 1;
				}
			}
			
			if(cb->anim.id) {
				if(!cb->anim.cache.base) {
					memset(&cb->anim.cache, 0, sizeof(cb->anim.cache));
					cb->anim.cache.base = k3MdlGetAnim(Game.entities.render[ci].cache, cb->anim.id);
					cb->anim.cache.loop = true;
					
					k3AnimatorSet(&cb->anim.cache, 0);
				} else {
					float spd = 1.f;
					
					if(cb->anim.standard && cb->anim.id != 1 && cp && cp->geom) {
						dBodyID bid = dGeomGetBody(cp->geom);
						if(bid) {
							spd = glm_vec3_norm(dBodyGetLinearVel(bid)) * 0.3;
						}
					}
					
					k3AnimatorStep(&cb->anim.cache, spd / GAME_TPS);
				}
				
				for(size_t m = 0; m < boneCount; m++) {
					vec3 scaleignore;
					mat4 rot;
					glm_decompose(cb->anim.cache.inter[m], cb->bones[m].translation, rot, scaleignore);
					glm_mat4_quat(cb->anim.cache.inter[m], cb->bones[m].rotation);
				}
			}*/
		}
		
		dGeomID gid = cp->geom;
		
		if(!gid) continue;
		
		dBodyID bid = dGeomGetBody(gid);
		
		if(bid) {
			const float *q = dBodyGetQuaternion(bid);
			
			Game.entities.render[ci].rot[0] = q[1];
			Game.entities.render[ci].rot[1] = q[2];
			Game.entities.render[ci].rot[2] = q[3];
			Game.entities.render[ci].rot[3] = q[0];
		} else {
			dQuaternion q;
			dGeomGetQuaternion(gid, q);
			
			Game.entities.render[ci].rot[0] = q[1];
			Game.entities.render[ci].rot[1] = q[2];
			Game.entities.render[ci].rot[2] = q[3];
			Game.entities.render[ci].rot[3] = q[0];
		}
		
		const float *src = bid ? dBodyGetPosition(bid) : dGeomGetPosition(gid);
		Game.entities.render[ci].pos[0] = src[0];
		Game.entities.render[ci].pos[1] = src[1];
		Game.entities.render[ci].pos[2] = src[2];
		Game.entities.render[ci].pos[3] = 1;
	}
	
	for(size_t cm = 0; cm < Game.entities.movementCount; cm++) {
		struct CBoned *cb = game_getcomponent(Game.entities.movement[cm].entity, boned);
		/*if(cb && cb->anim.standard) {
			struct CPhysics *cp = game_getcomponent(Game.entities.movement[cm].entity, physics);
			if(cp && cp->geom) {
				dBodyID bid = dGeomGetBody(cp->geom);
				if(bid) {
					uint16_t newID = glm_vec3_norm(dBodyGetLinearVel(bid)) > 1 ? 2 : 1;
					
					if(cb->anim.id != newID) {
						cb->anim.id = newID;
						cb->anim.cache.base = NULL;
					}
				}
			}
		}*/
	}
	
	for(size_t i = 0; i < Game.conveyorCount; i++) {
		struct Conveyor *conveyor = Game.conveyors[i];
		
		struct CPhysics *cp = conveyor->entity == ENT_ID_INVALID ? NULL : game_getcomponent(conveyor->entity, physics);
		
		dGeomID gid = cp ? cp->geom : NULL;
		
		conveyor->effectivelyActiveLast = conveyor->effectivelyActive;
		
		if(conveyor->type == CONVEYOR_TYPE_LOOP) {
			if(!conveyor->active) {
				conveyor->effectivelyActive = 0;
				continue;
			}
			
			conveyor->effectivelyActive = 1;
			
			// Teleport to first point
			if(conveyor->position >= conveyor->pointCount - 1) {
				conveyor->position -= conveyor->pointCount - 1;
			}
			
			size_t currentPoint = floorf(conveyor->position);
			size_t nextPoint = currentPoint + 1;
			float alpha = fmodf(conveyor->position, 1.f);
			
			if(gid) {
				vec3 lerped;
				glm_vec3_lerp(conveyor->points[currentPoint], conveyor->points[nextPoint], alpha, lerped);
				dGeomSetPosition(gid, lerped[0], lerped[1], lerped[2]);
				
				dBodyID bid = dGeomGetBody(gid);
				if(bid) {
					vec3 dif;
					glm_vec3_sub(conveyor->points[nextPoint], conveyor->points[currentPoint], dif);
					glm_vec3_scale_as(dif, conveyor->speed, dif);
					dBodySetLinearVel(bid, dif[0], dif[1], dif[2]); // doesn't affect movement, but necessary for proper collision response
				}
			}
			
			conveyor->position += conveyor->speed / glm_vec3_distance(conveyor->points[currentPoint], conveyor->points[nextPoint]) / GAME_TPS;
		} else if(conveyor->type == CONVEYOR_TYPE_BINARY) {
			int stopped = 0;
			
			if(conveyor->active > 0 && conveyor->position >= conveyor->pointCount - 1) {
				conveyor->position = conveyor->pointCount - 1;
				stopped = 1;
			} else if(conveyor->active <= 0 && conveyor->position <= 0) {
				conveyor->position = 0;
				stopped = 1;
			}
			
			conveyor->effectivelyActive = !stopped;
			
			size_t currentPoint, nextPoint;
			if(!stopped) {
				currentPoint = (size_t) floorf(conveyor->position) % conveyor->pointCount;
				nextPoint = (currentPoint + 1) % conveyor->pointCount;
				float alpha = fmodf(conveyor->position, 1.f);
				
				if(gid) {
					vec3 lerped;
					glm_vec3_lerp(conveyor->points[currentPoint], conveyor->points[nextPoint], alpha, lerped);
					dGeomSetPosition(gid, lerped[0], lerped[1], lerped[2]);
				}
				
				float offset = conveyor->speed / glm_vec3_distance(conveyor->points[currentPoint], conveyor->points[nextPoint]) / GAME_TPS;
				
				if(conveyor->active > 0) {
					conveyor->position += offset;
				} else {
					conveyor->position -= offset;
				}
			}
			
			if(gid) {
				dBodyID bid = dGeomGetBody(gid);
				if(bid) {
					if(stopped) {
						dBodySetLinearVel(bid, 0, 0, 0);
					} else {
						vec3 dif;
						glm_vec3_sub(conveyor->points[nextPoint], conveyor->points[currentPoint], dif);
						glm_vec3_scale_as(dif, conveyor->speed * (conveyor->active <= 0 ? -1 : 1), dif);
						dBodySetLinearVel(bid, dif[0], dif[1], dif[2]); // doesn't affect movement, but necessary for proper collision response
					}
				}
			}
		}
		
		if(conveyor->trigger != TRIGGER_INVALID) {
			if(conveyor->effectivelyActive && !conveyor->effectivelyActiveLast) {
				Game.triggers[conveyor->trigger - 1](conveyor->trigger, ENT_ID_INVALID, ENT_ID_INVALID, 0);
			} else if(conveyor->effectivelyActive && conveyor->effectivelyActiveLast) {
				Game.triggers[conveyor->trigger - 1](conveyor->trigger, ENT_ID_INVALID, ENT_ID_INVALID, 1);
			} else if(!conveyor->effectivelyActive && conveyor->effectivelyActiveLast) {
				Game.triggers[conveyor->trigger - 1](conveyor->trigger, ENT_ID_INVALID, ENT_ID_INVALID, 2);
			}
		}
	}
	
	Game.frame++;
}

void game_setphys(struct TrimeshData *phys) {
	if(Game.statikRes) {
		resman_unref_ptr(RESMAN_PHYSICS, Game.statikRes);
	}
	if(Game.statik) {
		dGeomDestroy(Game.statik);
	}
	
	Game.statikRes = phys;
	Game.statik = dCreateTriMesh(Game.space, phys->trid, NULL, NULL, NULL);
	
	dGeomSetCategoryBits(Game.statik, CATEGORY_STATIC);
	dGeomSetCollideBits(Game.statik, 0);
	
	dGeomSetData(Game.statik, (void*) ENT_ID_INVALID);
}

void game_synccphysics() {
	for(size_t i = 0; i < Game.entities.physicsCount; i++) {
		if(Game.entities.physics[i].geom) {
			glm_vec3_copy(dGeomGetPosition(Game.entities.physics[i].geom), Game.entities.physics[i].start);
		}
	}
}

void game_cleanup() {
	// XXX: Go over ALL component types!
	
	memset(Game.entities.freeIDs, 0, sizeof(Game.entities.freeIDs));
	
	for(size_t i = 0; i < Game.entities.physicsCount; i++) {
		if(Game.entities.physics[i].type == CPHYSICS_TRIMESH) {
			resman_unref_ptr(RESMAN_PHYSICS, Game.entities.physics[i].trimesh.cache);
		}
		dGeomID gid = Game.entities.physics[i].geom;
		if(gid) {
			dBodyID bid = dGeomGetBody(gid);
			if(bid) {
				dBodyDestroy(bid);
			}
			dGeomDestroy(gid);
		}
	}
	Game.entities.physicsCount = 0;
	
	for(size_t i = 0; i < Game.entities.renderCount; i++) {
		if(Game.entities.render[i].cache) {
			resman_unref(RESMAN_MODEL, Game.entities.render[i].mdl);
		}
	}
	Game.entities.renderCount = 0;
	
	for(size_t i = 0; i < Game.entities.bonedCount; i++) {
		//free(Game.entities.boned[i].bones);
	}
	Game.entities.bonedCount = 0;
	
	Game.entities.movementCount = 0;
	Game.entities.playerctrlCount = 0;
	
	Game.entityCount = 0;
	
	for(size_t i = 0; i < Game.conveyorCount; i++) {
		free(Game.conveyors[i]->points);
		free(Game.conveyors[i]);
	}
	free(Game.conveyors);
	Game.conveyors = NULL;
	Game.conveyorCount = 0;
	
	free(Game.triggers);
	Game.triggers = NULL;
	Game.triggerCount = 0;
	
	Game.spectated = ENT_ID_INVALID;
	Game.controlled = ENT_ID_INVALID;
	
	free(activeCollisions);
	activeCollisions = NULL;
	activeCollisionCount = 0;
	activeCollisionCapacity = 0;
	
	if(Game.phys) {
		dWorldDestroy(Game.phys);
	}
	
	Game.phys = dWorldCreate();
	dWorldSetGravity(Game.phys, 0, -15, 0);
	
	dWorldSetCFM(Game.phys, 0.00002);
	dWorldSetERP(Game.phys, 0.1);
	
	Game.space = dHashSpaceCreate(NULL);
	
	Game.contactgroup = dJointGroupCreate(0);
}

void game_killentity(uint16_t eid) {
	// XXX: Go over ALL component types!
	
	struct CPhysics *cp = game_getcomponent(eid, physics);
	if(cp) {
		dBodyID bid = dGeomGetBody(cp->geom);
		
		if(bid) {
			for(dGeomID gid = dBodyGetFirstGeom(bid); gid; gid = dBodyGetNextGeom(gid)) {
				dGeomDestroy(gid);
			}
			
			dBodyDestroy(bid);
		} else {
			dGeomDestroy(cp->geom);
		}
		
		game_killcomponent_ptr(cp, physics);
	}
	
	struct CRender *cr = game_getcomponent(eid, render);
	if(cr) {
		if(cr->cache) {
			if(resman_rev(cr->cache)) {
				resman_unref_ptr(RESMAN_MODEL, cr->cache);
			}
		}
		
		game_killcomponent_ptr(cr, render);
	}
	
	game_killcomponent(eid, movement);
	game_killcomponent(eid, playerctrl);
	game_killcomponent(eid, boned);
}