// written in the D programming language

module chipmunkd.cpSpaceComponent;

import chipmunkd.chipmunk;
import chipmunkd.chipmunk_types_h;
import chipmunkd.cpVect,chipmunkd.cpVect_h;
import chipmunkd.cpBody;

// Chipmunk uses a data structure called a disjoint set forest.
// My attempts to find a way to splice circularly linked lists in
// constant time failed, and so I found this neat data structure instead.

static cpBody *
componentNodeRoot(cpBody *_body)
{
	cpBody *parent = _body.node.parent;
	
	if(parent){
		// path compression, attaches this node directly to the root
		return (_body.node.parent = componentNodeRoot(parent));
	} else {
		return _body;
	}
}

static void
componentNodeMerge(cpBody *a_root, cpBody *b_root)
{
	if(a_root.node.rank < b_root.node.rank){
		a_root.node.parent = b_root;
	} else if(a_root.node.rank > b_root.node.rank){
		b_root.node.parent = a_root;
	} else if(a_root != b_root){
		b_root.node.parent = a_root;
		a_root.node.rank++;
	}
}

static void
componentActivate(cpBody *root)
{
	if(!cpBodyIsSleeping(root)) return;
	
	cpSpace *space = root.space;
	assert(space, "Trying to activate a body that was never added to a space.");
	
	cpBody* _body = root;
	cpBody* next;
	do {
		next = _body.node.next;
		
		cpComponentNode node = {null, null, 0, 0.0f};
		_body.node = node;
		cpArrayPush(space.bodies, _body);
		
		for(cpShape *shape=_body.shapesList; shape; shape=shape.next){
			cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
			cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
		}
	} while((_body = next) != root);
	
	cpArrayDeleteObj(space.sleepingComponents, root);
}

void
cpBodyActivate(cpBody *_body)
{
	// Reset the idle time even if it's not in a currently sleeping component
	// Like a body resting on or jointed to a rogue body.
	_body.node.idleTime = 0.0f;
	componentActivate(componentNodeRoot(_body));
}

static void
mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
{
	// Don't merge with the static body
	if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
	
	cpBody *a_root = componentNodeRoot(a);
	cpBody *b_root = componentNodeRoot(b);
	
	cpBool a_sleep = cpBodyIsSleeping(a_root);
	cpBool b_sleep = cpBodyIsSleeping(b_root);
	
	if(a_sleep && b_sleep){
		return;
	} else if(a_sleep || b_sleep){
		componentActivate(a_root);
		componentActivate(b_root);
	} 
	
	// Add any rogue bodies (bodies not added to the space)
	if(!a.space) cpArrayPush(rogueBodies, a);
	if(!b.space) cpArrayPush(rogueBodies, b);
	
	componentNodeMerge(a_root, b_root);
}

static cpBool
componentActive(cpBody *root, cpFloat threshold)
{
	cpBody *_body = root;
	cpBody *next;
	do {
		next = _body.node.next;
		if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue;
	} while((_body = next) != root);
	
	return cpFalse;
}

static void
addToComponent(cpBody *_body, cpArray *components)
{
	// Check that the body is not already added to the component list
	if(_body.node.next) return;
	cpBody *root = componentNodeRoot(_body);
	
	cpBody *next = root.node.next;
	if(!next){
		// If the root isn't part of a list yet, then it hasn't been
		// added to the components list. Do that now.
		cpArrayPush(components, root);
		// Start the list
		_body.node.next = root;
		root.node.next = _body;
	} else if(root != _body) {
		// Splice in body after the root.
		_body.node.next = next;
		root.node.next = _body;
	}
}

// TODO this function needs more commenting.
void
cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
{
	cpArray *bodies = space.bodies;
	cpArray *newBodies = cpArrayNew(bodies.num);
	cpArray *rogueBodies = cpArrayNew(16);
	cpArray *arbiters = space.arbiters;
	cpArray *constraints = space.constraints;
	cpArray *components = cpArrayNew(bodies.num/8);
	
	cpFloat dv = space.idleSpeedThreshold;
	cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt);
	// update idling
	for(int i=0; i<bodies.num; i++){
		cpBody *_body = cast(cpBody*)bodies.arr[i];
		
		cpFloat thresh = (dvsq ? _body.m*dvsq : 0.0f);
		_body.node.idleTime = (cpBodyKineticEnergy(_body) > thresh ? 0.0f : _body.node.idleTime + dt);
	}
	
	// iterate graph edges and build forests
	for(int i=0; i<arbiters.num; i++){
		cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
		mergeBodies(space, components, rogueBodies, arb.private_a._body, arb.private_b._body);
	}
	for(int j=0; j<constraints.num; j++){
		cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
		mergeBodies(space, components, rogueBodies, constraint.a, constraint.b);
	}
	
	// iterate bodies and add them to their components
	for(int i=0; i<bodies.num; i++)
		addToComponent(cast(cpBody*)bodies.arr[i], components);
	for(int i=0; i<rogueBodies.num; i++)
		addToComponent(cast(cpBody*)rogueBodies.arr[i], components);
	
	// iterate components, copy or deactivate
	for(int i=0; i<components.num; i++){
		cpBody *root = cast(cpBody*)components.arr[i];
		if(componentActive(root, space.sleepTimeThreshold)){
			cpBody *_body = root;
			cpBody *next;
			do {
				next = _body.node.next;
				
				if(!cpBodyIsRogue(_body)) cpArrayPush(newBodies, _body);
				_body.node.next = null;
				_body.node.parent = null;
				_body.node.rank = 0;
			} while((_body = next) != root);
		} else {
			cpBody *_body = root;
			cpBody *next;
			do {
				next = _body.node.next;
				
				for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
					cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
					cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
				}
			} while((_body = next) != root);
			
			cpArrayPush(space.sleepingComponents, root);
		}
	}
	
	space.bodies = newBodies;
	cpArrayFree(bodies);
	cpArrayFree(rogueBodies);
	cpArrayFree(components);
}

void
cpSpaceSleepBody(cpSpace *space, cpBody *_body){
	cpComponentNode node = {null, _body, 0, 0.0f};
	_body.node = node;
	
	for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
		cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
		
		cpShapeCacheBB(shape);
		cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
	}
	
	cpArrayPush(space.sleepingComponents, _body);
	cpArrayDeleteObj(space.bodies, _body);
}