// written in the D programming language

module chipmunkd.cpBody;

import chipmunkd.chipmunk;

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);
}

static cpBool
cpBodyIsStatic(const cpBody *_body)
{
	return _body.node.idleTime == INFINITY;
}

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

//TODO
//#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)

template CP_DefineBodyGetter(string type,string member,string name)
{
	enum CP_DefineBodyGetter = 
		"static "~type~" BodyGet"~name~"(const cpBody *_body){"~
		"return _body."~member~";"~
		"}";
}

template CP_DefineBodySetter(string type,string member,string name)
{
	enum CP_DefineBodySetter = 
		"static void BodySet"~name~"(cpBody *_body, const "~type~" value){"~
		"cpBodyActivate(_body);"~
		"_body."~member~" = value;"~
		"}";
}

template CP_DefineBodyProperty(string type,string member,string name)
{
	enum CP_DefineBodyProperty = 
		CP_DefineBodyGetter!(type,member,name)~CP_DefineBodySetter!(type,member,name);
		
}

//// Accessors for cpBody struct members
mixin(CP_DefineBodyGetter!("cpFloat","m","Mass"));
//void cpBodySetMass(cpBody *body, cpFloat m);
//
mixin(CP_DefineBodyGetter!("cpFloat","i","Moment"));
//void cpBodySetMoment(cpBody *body, cpFloat i);
//
//

mixin(CP_DefineBodyProperty!("cpVect","p","Pos"));
mixin(CP_DefineBodyProperty!("cpVect","v","Vel"));
mixin(CP_DefineBodyProperty!("cpVect","f","Force"));
mixin(CP_DefineBodyProperty!("cpFloat","a","Angle"));
mixin(CP_DefineBodyProperty!("cpFloat","w","AngVel"));
mixin(CP_DefineBodyProperty!("cpFloat","t","Torque"));
mixin(CP_DefineBodyProperty!("cpVect","rot","Rot"));
mixin(CP_DefineBodyProperty!("cpFloat","v_limit","VelLimit"));
mixin(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);
}

cpBody *
cpBodyInitStatic(cpBody *_body)
{
	cpBodyInit(_body, cast(cpFloat)INFINITY, cast(cpFloat)INFINITY);
	_body.node.idleTime = cast(cpFloat)INFINITY;
	
	return _body;
}

cpBody *
cpBodyNewStatic()
{
	return cpBodyInitStatic(cpBodyAlloc());
}

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);
}