cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
    cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
    cpAssertHard(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");

    return ((cpPolyShape *)shape)->verts[idx];
}

cpVect
cpPolyShapeGetVert(const cpShape *shape, int i)
{
	cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
	
	int count = cpPolyShapeGetCount(shape);
	cpAssertHard(0 <= i && i < count, "Index out of range.");
	
	return ((cpPolyShape *)shape)->planes[i + count].v0;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time.");
	cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second.");
	cpAssertSpaceUnlocked(space);
	
	cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
	body->space = space;
	
	return body;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time.");
	cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second.");
	cpAssertSpaceUnlocked(space);
	
	cpArrayPush(cpBodyIsDynamic(body) ? space->dynamicBodies : space->otherBodies, body);
	body->space = space;
	
	return body;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
	cpAssertHard(!body->space, "This body is already added to a space and cannot be added to another.");
	cpAssertSpaceUnlocked(space);
	
	cpArrayPush(space->bodies, body);
	body->space = space;
	
	return body;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(!cpBodyIsStatic(body), "Do not add static bodies to a space. Static bodies do not move and should not be simulated.");
	cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time.");
	cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second.");
	cpAssertSpaceUnlocked(space);
	
	cpArrayPush(space->bodies, body);
	body->space = space;
	
	return body;
}

void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space.");
	cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
//	cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it.");
//	cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it.");
	cpAssertSpaceUnlocked(space);
	
	cpBodyActivate(body);
//	cpSpaceFilterArbiters(space, body, NULL);
	cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
	body->space = NULL;
}

void
cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase)
{
	cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint.");
	cpConstraintActivateBodies(constraint);
	((cpGearJoint *)constraint)->phase = phase;
}

void
cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle)
{
	cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
	cpConstraintActivateBodies(constraint);
	((cpDampedRotarySpring *)constraint)->restAngle = restAngle;
}

void
cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness)
{
	cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
	cpConstraintActivateBodies(constraint);
	((cpDampedRotarySpring *)constraint)->stiffness = stiffness;
}

cpVect
cpArbiterGetPoint(const cpArbiter *arb, int i)
{
	cpAssertHard(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter");
	
	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
}

void
cpShapeSetElasticity(cpShape *shape, cpFloat elasticity)
{
	cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive and non-zero.");
	cpBodyActivate(shape->body);
	shape->e = elasticity;
}

void
cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
{
    cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
    cpPolyShapeDestroy((cpPolyShape *)shape);
    setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
}

cpFloat
cpArbiterGetDepth(const cpArbiter *arb, int i)
{
	cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");

	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
}

void
cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias)
{
	cpAssertHard(errorBias >= 0.0f, "errorBias must be positive.");
	cpConstraintActivateBodies(constraint);
	constraint->errorBias = errorBias;
}

cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
	cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
	cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
	cpAssertHard(cpBodyIsRogue(shape->body), "You are adding a static shape to a dynamic body. Did you mean to attach it to a static or rogue body? See the documentation for more information.");
	cpAssertSpaceUnlocked(space);
	
	cpBody *body = shape->body;
	cpBodyAddShape(body, shape);
	cpShapeUpdate(shape, body->p, body->rot);
	cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
	shape->space = space;
	
	return shape;
}

void
cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc)
{
	cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
	cpConstraintActivateBodies(constraint);
	((cpDampedRotarySpring *)constraint)->springTorqueFunc = springTorqueFunc;
}

void
cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
{
	space->locked--;
	cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow.");
	
	if(space->locked == 0 && runPostStep && !space->skipPostStep){
		space->skipPostStep = cpTrue;
		
		cpArray *waking = space->rousedBodies;
		for(int i=0, count=waking->num; i<count; i++){
			cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
			waking->arr[i] = NULL;
		}
		
		cpArray *arr = space->postStepCallbacks;
		for(int i=0; i<arr->num; i++){
			cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
			cpPostStepFunc func = callback->func;
			
			// Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again.
			callback->func = NULL;
			if(func) func(space, callback->key, callback->data);
			
			arr->arr[i] = NULL;
			cpfree(callback);
		}
		
		waking->num = 0;
		arr->num = 0;
		space->skipPostStep = cpFalse;
	}
}

void
cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle)
{
	cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
	cpConstraintActivateBodies(constraint);
	((cpRatchetJoint *)constraint)->angle = angle;
}

void
cpShapeSetFriction(cpShape *shape, cpFloat friction)
{
	cpAssertHard(friction >= 0.0f, "Friction must be postive and non-zero.");
	cpBodyActivate(shape->body);
	shape->u = friction;
}