// written in the D programming language

module chipmunkd.cpBody;

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

alias void function(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)cpBodyVelocityFunc;
alias void function(cpBody *_body, cpFloat dt)cpBodyPositionFunc;

//extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault;
//extern cpBodyPositionFunc cpBodyUpdatePositionDefault;

// Structure to hold information about the contact graph components
// when putting groups of objects to sleep.
// No interesting user accessible fields.
struct cpComponentNode {
	cpBody *parent;
	cpBody *next;
	int rank;
	cpFloat idleTime;
}

struct cpBody{
	// *** Integration Functions.

	// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
	cpBodyVelocityFunc velocity_func;
	
	// Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition)
	cpBodyPositionFunc position_func;
	
	// *** Mass Properties
	
	// Mass and it's inverse.
	// Always use cpBodySetMass() whenever changing the mass as these values must agree.
	cpFloat m, m_inv;
	
	// Moment of inertia and it's inverse.
	// Always use cpBodySetMoment() whenever changing the moment as these values must agree.
	cpFloat i, i_inv;
	
	// *** Positional Properties
	
	// Linear components of motion (position, velocity, and force)
	cpVect p, v, f;
	
	// Angular components of motion (angle, angular velocity, and torque)
	// Always use cpBodySetAngle() to set the angle of the body as a and rot must agree.
	cpFloat a, w, t;
	
	// Cached unit length vector representing the angle of the body.
	// Used for fast vector rotation using cpvrotate().
	cpVect rot;
	
	// *** User Definable Fields
	
	// User defined data pointer.
	cpDataPointer data;
	
	// *** Other Fields
	
	// Maximum velocities this body can move at after integrating velocity
	cpFloat v_limit, w_limit;
	
	// *** Internally Used Fields
	
	// Velocity bias values used when solving penetrations and correcting constraints.
	cpVect v_bias;
	cpFloat w_bias;
	
	// Space this body has been added to
	cpSpace* space;
	
	// Pointer to the shape list.
	// Shapes form a linked list using cpShape.next when added to a space.
	cpShape *shapesList;
	
	// Used by cpSpaceStep() to store contact graph information.
	cpComponentNode node;
}

//// Basic allocation/destruction functions
//cpBody *cpBodyAlloc(void);
//cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
//cpBody *cpBodyNew(cpFloat m, cpFloat i);
//
//void cpBodyDestroy(cpBody *body);
//void cpBodyFree(cpBody *body);
//
//// Wake up a sleeping or idle body. (defined in cpSpace.c)
//void cpBodyActivate(cpBody *body);
//
//// Force a body to sleep;
//void cpBodySleep(cpBody *body);
////void cpBodySleepGroup(cpBody *body, ...);

static cpBool
cpBodyIsSleeping(const cpBody *_body)
{
	return (_body.node.next !is null);
}

//cpBool cpBodyIsStatic(const cpBody *body);

static cpBool
cpBodyIsRogue(const cpBody* _body)
{
	return (_body.space is null);
}


//#define CP_DefineBodyGetter(type, member, name) \
//static inline type cpBodyGet##name(const cpBody *body){return body.member;}
//
//#define CP_DefineBodySetter(type, member, name) \
//static inline void \
//cpBodySet##name(cpBody *body, const type value){ \
//	cpBodyActivate(body); \
//	body.member = value; \
//} \

//#define CP_DefineBodyProperty(type, member, name) \
//CP_DefineBodyGetter(type, member, name) \
//CP_DefineBodySetter(type, member, name)


//// Accessors for cpBody struct members
//CP_DefineBodyGetter(cpFloat, m, Mass);
//void cpBodySetMass(cpBody *body, cpFloat m);
//
//CP_DefineBodyGetter(cpFloat, i, Moment);
//void cpBodySetMoment(cpBody *body, cpFloat i);
//
//
//CP_DefineBodyProperty(cpVect, p, Pos);
//CP_DefineBodyProperty(cpVect, v, Vel);
//CP_DefineBodyProperty(cpVect, f, Force);
static cpFloat cpBodyGetAngle(const cpBody *_body){return _body.a;}
static cpFloat cpBodySetAngle(cpBody *_body, const cpFloat value){cpBodyActivate(_body); return _body.a = value;}
//CP_DefineBodyProperty(cpFloat, w, AngVel);
//CP_DefineBodyProperty(cpFloat, t, Torque);
//CP_DefineBodyGetter(cpVect, rot, Rot);
//CP_DefineBodyProperty(cpFloat, v_limit, VelLimit);
//CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit);

////  Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep.
//// Intended for objects that are moved manually with a custom velocity integration function.
//void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);
//
//// Default Integration functions.
//void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
//void cpBodyUpdatePosition(cpBody *body, cpFloat dt);

// Convert body local to world coordinates
static cpVect
cpBodyLocal2World(const cpBody *_body, const cpVect v)
{
	return cpvadd(_body.p, cpvrotate(v, _body.rot));
}

// Convert world to body local coordinates
static cpVect
cpBodyWorld2Local(const cpBody *_body, const cpVect v)
{
	return cpvunrotate(cpvsub(v, _body.p), _body.rot);
}

// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates).
static void
cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r)
{
	_body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv));
	_body.w += _body.i_inv*cpvcross(r, j);
}

//// Zero the forces on a body.
//void cpBodyResetForces(cpBody *body);
//// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates).
//void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);

static cpFloat
cpBodyKineticEnergy(const cpBody *_body)
{
	// Need to do some fudging to avoid NaNs
	cpFloat vsq = cpvdot(_body.v, _body.v);
	cpFloat wsq = _body.w*_body.w;
	return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f);
}

//// Apply a damped spring force between two bodies.
//// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead.
//void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt);

// initialized in cpInitChipmunk()
cpBody cpStaticBodySingleton;

cpBody*
cpBodyAlloc()
{
	return cast(cpBody *)cpmalloc(cpBody.sizeof);
}

cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity;
cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition;

cpBody*
cpBodyInit(cpBody *_body, cpFloat m, cpFloat i)
{
	_body.velocity_func = cpBodyUpdateVelocityDefault;
	_body.position_func = cpBodyUpdatePositionDefault;
	
	cpBodySetMass(_body, m);
	cpBodySetMoment(_body, i);

	_body.p = cpvzero;
	_body.v = cpvzero;
	_body.f = cpvzero;
	
	cpBodySetAngle(_body, 0.0f);
	_body.w = 0.0f;
	_body.t = 0.0f;
	
	_body.v_bias = cpvzero;
	_body.w_bias = 0.0f;
	
	_body.data = null;
	_body.v_limit = INFINITY;
	_body.w_limit = INFINITY;
	
	_body.space = null;
	_body.shapesList = null;
	
	cpComponentNode node = {null, null, 0, 0.0f};
	_body.node = node;
	
	return _body;
}

cpBody*
cpBodyNew(cpFloat m, cpFloat i)
{
	return cpBodyInit(cpBodyAlloc(), m, i);
}

void cpBodyDestroy(cpBody *_body){}

void
cpBodyFree(cpBody *_body)
{
	if(_body){
		cpBodyDestroy(_body);
		cpfree(_body);
	}
}

void
cpBodySetMass(cpBody *_body, cpFloat mass)
{
	_body.m = mass;
	_body.m_inv = 1.0f/mass;
}

void
cpBodySetMoment(cpBody *_body, cpFloat moment)
{
	_body.i = moment;
	_body.i_inv = 1.0f/moment;
}

void
cpBodySetAngle(cpBody *_body, cpFloat angle)
{
	_body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
	_body.rot = cpvforangle(angle);
}

void
cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt)
{
	cpVect delta = cpvsub(pos, _body.p);
	_body.v = cpvmult(delta, 1.0f/dt);
}

void
cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)
{
	_body.v = cpvclamp(
		cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)), 
		_body.v_limit);
	
	cpFloat w_limit = _body.w_limit;
	_body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit);
}

void
cpBodyUpdatePosition(cpBody *_body, cpFloat dt)
{
	_body.p = cpvadd(_body.p, cpvmult(cpvadd(_body.v, _body.v_bias), dt));
	cpBodySetAngle(_body, _body.a + (_body.w + _body.w_bias)*dt);
	
	_body.v_bias = cpvzero;
	_body.w_bias = 0.0f;
}

void
cpBodyResetForces(cpBody *_body)
{
	_body.f = cpvzero;
	_body.t = 0.0f;
}

void
cpBodyApplyForce(cpBody *_body, cpVect force, cpVect r)
{
	_body.f = cpvadd(_body.f, force);
	_body.t += cpvcross(r, force);
}

void
cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
{
	// Calculate the world space anchor coordinates.
	cpVect r1 = cpvrotate(anchr1, a.rot);
	cpVect r2 = cpvrotate(anchr2, b.rot);
	
	cpVect delta = cpvsub(cpvadd(b.p, r2), cpvadd(a.p, r1));
	cpFloat dist = cpvlength(delta);
	cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
	
	cpFloat f_spring = (dist - rlen)*k;

	// Calculate the world relative velocities of the anchor points.
	cpVect v1 = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
	cpVect v2 = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
	
	// Calculate the damping force.
	// This really should be in the impulse solver and can produce problems when using large damping values.
	cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
	cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a.m_inv + b.m_inv)));
	
	// Apply!
	cpVect f = cpvmult(n, f_spring + f_damp);
	cpBodyApplyForce(a, f, r1);
	cpBodyApplyForce(b, cpvneg(f), r2);
}

cpBool
cpBodyIsStatic(/+const+/ cpBody *_body)
{
	cpSpace *space = _body.space;
	return ( (space !is null) && (_body is &space.staticBody) );
}

//void cpSpaceSleepBody(cpSpace *space, cpBody *_body);

void
cpBodySleep(cpBody *_body)
{
	if(cpBodyIsSleeping(_body)) return;
	
	assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep.");
	cpSpaceSleepBody(_body.space, _body); //TODO
}