// written in the D programming language

module chipmunkd.cpShape;

import chipmunkd.chipmunk;

import std.stdio;

struct cpSegmentQueryInfo {
	cpShape *shape; // shape that was hit, null if no collision
	cpFloat t; // Distance along query segment, will always be in the range [0, 1].
	cpVect n; // normal of hit surface
}

// Enumeration of shape types.
enum cpShapeType{
	CP_CIRCLE_SHAPE,
	CP_SEGMENT_SHAPE,
	CP_POLY_SHAPE,
	CP_NUM_SHAPES
}

// Shape class. Holds function pointers and type data.
struct cpShapeClass {
	cpShapeType type;
	
	// Called by cpShapeCacheBB().
	cpBB function(cpShape *shape, cpVect p, cpVect rot) cacheData;
	// Called to by cpShapeDestroy().
	void function(cpShape *shape) destroy;
	
	// called by cpShapePointQuery().
	cpBool function(cpShape *shape, cpVect p)pointQuery;
	
	// called by cpShapeSegmentQuery()
	 void function(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) segmentQuery;
}

// Basic shape struct that the others inherit from.
struct cpShape{
	// The "class" of a shape as defined above 
	/+const+/ cpShapeClass *klass;
	
	// cpBody that the shape is attached to.
	cpBody* _body;
	
	// Cached BBox for the shape.
	cpBB bb;
	
	// Sensors invoke callbacks, but do not generate collisions
	cpBool sensor;
	
	// *** Surface properties.
	
	// Coefficient of restitution. (elasticity)
	cpFloat e;
	// Coefficient of friction.
	cpFloat u;
	// Surface velocity used when solving for friction.
	cpVect surface_v;
	
	// *** User Definable Fields
	
	// User defined data pointer for the shape.
	cpDataPointer data;
	
	// User defined collision type for the shape.
	cpCollisionType collision_type;
	// User defined collision group for the shape.
	cpGroup group;
	// User defined layer bitmask for the shape.
	cpLayers layers;
	
	// *** Internally Used Fields
	
	// Shapes form a linked list when added to space on a non-null body
	cpShape* next;
	
	// Unique id used as the hash value.
	cpHashValue hashid;
}

//
//// Low level shape initialization func.
//cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body);
//
//// Basic destructor functions. (allocation functions are not shared)
//void cpShapeDestroy(cpShape *shape);
//void cpShapeFree(cpShape *shape);
//
//// Cache the BBox of the shape.
//cpBB cpShapeCacheBB(cpShape *shape);
//
//// Test if a point lies within a shape.
//cpBool cpShapePointQuery(cpShape *shape, cpVect p);

template CP_DefineShapeGetter(string _struct,string type,string member,string name)
{
	enum CP_DefineShapeGetter = 
		type~" "~_struct~"Get"~name~"(cpShape *shape){"~
		"assert(shape.klass == &"~_struct~"Class, \"shape is not a "~_struct~"\");"~
		"return (cast("~_struct~"*)shape)."~member~";"~
		"}";
}

// Circle shape structure.
struct cpCircleShape{
	cpShape shape;
	
	// Center in body space coordinates
	cpVect c;
	// Radius.
	cpFloat r;
	
	// Transformed center. (world space coordinates)
	cpVect tc;
}

//// Basic allocation functions for cpCircleShape.
//cpCircleShape *cpCircleShapeAlloc(void);
//cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset);
//cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset);

mixin(CP_DefineShapeGetter!("cpCircleShape", "cpVect", "c", "Offset"));
mixin(CP_DefineShapeGetter!("cpCircleShape", "cpFloat", "r", "Radius"));

// Segment shape structure.
struct cpSegmentShape{
	cpShape shape;
	
	// Endpoints and normal of the segment. (body space coordinates)
	cpVect a, b, n;
	// Radius of the segment. (Thickness)
	cpFloat r;

	// Transformed endpoints and normal. (world space coordinates)
	cpVect ta, tb, tn;
}
//
//// Basic allocation functions for cpSegmentShape.
//cpSegmentShape* cpSegmentShapeAlloc(void);
//cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius);
//cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius);

mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "a", "A"));
mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "b", "B"));
mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "n", "Normal"));
mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpFloat", "r", "Radius"));
//
//// For determinism, you can reset the shape id counter.
//void cpResetShapeIdCounter(void);
//
//// Directed segment queries against individual shapes.
//void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);
//
//cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
//
static cpVect
cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
{
	return cpvlerp(start, end, info.t);
}

static cpFloat
cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
{
	return cpvdist(start, end)*info.t;
}

cpHashValue SHAPE_ID_COUNTER = 0;

void
cpResetShapeIdCounter()
{
	SHAPE_ID_COUNTER = 0;
}

cpShape*
cpShapeInit(cpShape *shape, /+const+/ cpShapeClass *klass, cpBody *_body)
{
	shape.klass = klass;
	
	shape.hashid = SHAPE_ID_COUNTER;
	SHAPE_ID_COUNTER++;
	
	shape._body = _body;
	shape.sensor = 0;
	
	shape.e = 0.0f;
	shape.u = 0.0f;
	shape.surface_v = cpvzero;
	
	shape.collision_type = 0;
	shape.group = CP_NO_GROUP;
	shape.layers = CP_ALL_LAYERS;
	
	shape.data = null;
	shape.next = null;
	
//	cpShapeCacheBB(shape);
	
	return shape;
}

void
cpShapeDestroy(cpShape *shape)
{
	if(shape.klass.destroy) shape.klass.destroy(shape);
}

void
cpShapeFree(cpShape *shape)
{
	if(shape){
		cpShapeDestroy(shape);
		cpfree(shape);
	}
}

// TODO this function should really take a position and rotation explicitly and be renamed
cpBB
cpShapeCacheBB(cpShape *shape)
{
	cpBody *_body = shape._body;
	
	shape.bb = shape.klass.cacheData(shape, _body.p, _body.rot);
	return shape.bb;
}

cpBool
cpShapePointQuery(cpShape *shape, cpVect p){
	return shape.klass.pointQuery(shape, p);
}

cpBool
cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
	cpSegmentQueryInfo blank = {null, 0.0f, cpvzero};
	(*info) = blank;
	
	shape.klass.segmentQuery(shape, a, b, info);
	return (info.shape !is null);
}

void
cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
{
	writefln("Segment Query:");
	writefln("\tt: %s", info.t);
//	writefln("\tdist: %f\n", info.dist);
//	writefln("\tpoint: %s\n", cpvstr(info.point));
	writefln("\tn: %s", cpvstr(info.n));
}

cpCircleShape *
cpCircleShapeAlloc()
{
	return cast(cpCircleShape *)cpcalloc(1, cpCircleShape.sizeof);
}

static cpBB
bbFromCircle(const cpVect c, const cpFloat r)
{
	return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
}

static cpBB
cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
	cpCircleShape *circle = cast(cpCircleShape *)shape;
	
	circle.tc = cpvadd(p, cpvrotate(circle.c, rot));
	return bbFromCircle(circle.tc, circle.r);
}

static cpBool
cpCircleShapePointQuery(cpShape *shape, cpVect p){
	cpCircleShape *circle = cast(cpCircleShape *)shape;
	return cpvnear(circle.tc, p, circle.r);
}

static void
circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	// offset the line to be relative to the circle
	a = cpvsub(a, center);
	b = cpvsub(b, center);
	
	cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
	cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
	cpFloat qc = cpvdot(a, a) - r*r;
	
	cpFloat det = qb*qb - 4.0f*qa*qc;
	
	if(det >= 0.0f){
		cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
		if(0.0f<= t && t <= 1.0f){
			info.shape = shape;
			info.t = t;
			info.n = cpvnormalize(cpvlerp(a, b, t));
		}
	}
}

static void
cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpCircleShape *circle = cast(cpCircleShape *)shape;
	circleSegmentQuery(shape, circle.tc, circle.r, a, b, info);
}

static /+const+/ cpShapeClass cpCircleShapeClass = {
	cpShapeType.CP_CIRCLE_SHAPE,
	&cpCircleShapeCacheData,
	null,
	&cpCircleShapePointQuery,
	&cpCircleShapeSegmentQuery,
};

cpCircleShape *
cpCircleShapeInit(cpCircleShape *circle, cpBody *_body, cpFloat radius, cpVect offset)
{
	circle.c = offset;
	circle.r = radius;
	
	cpShapeInit(cast(cpShape *)circle, &cpCircleShapeClass, _body);
	
	return circle;
}

cpShape *
cpCircleShapeNew(cpBody *_body, cpFloat radius, cpVect offset)
{
	return cast(cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), _body, radius, offset);
}

cpSegmentShape *
cpSegmentShapeAlloc()
{
	return cast(cpSegmentShape *)cpcalloc(1, cpSegmentShape.sizeof);
}

static cpBB
cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
	
	seg.ta = cpvadd(p, cpvrotate(seg.a, rot));
	seg.tb = cpvadd(p, cpvrotate(seg.b, rot));
	seg.tn = cpvrotate(seg.n, rot);
	
	cpFloat l,r,s,t;
	
	if(seg.ta.x < seg.tb.x){
		l = seg.ta.x;
		r = seg.tb.x;
	} else {
		l = seg.tb.x;
		r = seg.ta.x;
	}
	
	if(seg.ta.y < seg.tb.y){
		s = seg.ta.y;
		t = seg.tb.y;
	} else {
		s = seg.tb.y;
		t = seg.ta.y;
	}
	
	cpFloat rad = seg.r;
	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
}

static cpBool
cpSegmentShapePointQuery(cpShape *shape, cpVect p){
	if(!cpBBcontainsVect(shape.bb, p)) return cpFalse;
	
	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
	
	// Calculate normal distance from segment.
	cpFloat dn = cpvdot(seg.tn, p) - cpvdot(seg.ta, seg.tn);
	cpFloat dist = cpfabs(dn) - seg.r;
	if(dist > 0.0f) return cpFalse;
	
	// Calculate tangential distance along segment.
	cpFloat dt = -cpvcross(seg.tn, p);
	cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
	cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
	
	// Decision tree to decide which feature of the segment to collide with.
	if(dt <= dtMin){
		if(dt < (dtMin - seg.r)){
			return cpFalse;
		} else {
			return cpvlengthsq(cpvsub(seg.ta, p)) < (seg.r*seg.r);
		}
	} else {
		if(dt < dtMax){
			return cpTrue;
		} else {
			if(dt < (dtMax + seg.r)) {
				return cpvlengthsq(cpvsub(seg.tb, p)) < (seg.r*seg.r);
			} else {
				return cpFalse;
			}
		}
	}
	
	return cpTrue;	
}

static cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}

static void
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	// TODO this function could be optimized better.
	
	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
	cpVect n = seg.tn;
	// flip n if a is behind the axis
	if(cpvdot(a, n) < cpvdot(seg.ta, n))
		n = cpvneg(n);
	
	cpFloat an = cpvdot(a, n);
	cpFloat bn = cpvdot(b, n);
	
	if(an != bn){
		cpFloat d = cpvdot(seg.ta, n) + seg.r;
		cpFloat t = (d - an)/(bn - an);
		
		if(0.0f < t && t < 1.0f){
			cpVect point = cpvlerp(a, b, t);
			cpFloat dt = -cpvcross(seg.tn, point);
			cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
			cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
			
			if(dtMin < dt && dt < dtMax){
				info.shape = shape;
				info.t = t;
				info.n = n;
				
				return; // don't continue on and check endcaps
			}
		}
	}
	
	if(seg.r) {
		cpSegmentQueryInfo info1 = {null, 1.0f, cpvzero};
		cpSegmentQueryInfo info2 = {null, 1.0f, cpvzero};
		circleSegmentQuery(shape, seg.ta, seg.r, a, b, &info1);
		circleSegmentQuery(shape, seg.tb, seg.r, a, b, &info2);
		
		if(info1.t < info2.t){
			(*info) = info1;
		} else {
			(*info) = info2;
		}
	}
}

static /+const+/ cpShapeClass cpSegmentShapeClass = {
	cpShapeType.CP_SEGMENT_SHAPE,
	&cpSegmentShapeCacheData,
	null,
	&cpSegmentShapePointQuery,
	&cpSegmentShapeSegmentQuery,
};

cpSegmentShape *
cpSegmentShapeInit(cpSegmentShape *seg, cpBody *_body, cpVect a, cpVect b, cpFloat r)
{
	seg.a = a;
	seg.b = b;
	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
	
	seg.r = r;
	
	cpShapeInit(cast(cpShape *)seg, &cpSegmentShapeClass, _body);
	
	return seg;
}

cpShape*
cpSegmentShapeNew(cpBody *_body, cpVect a, cpVect b, cpFloat r)
{
	return cast(cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), _body, a, b, r);
}

// Unsafe API (chipmunk_unsafe.h)

void
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
{
	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = cast(cpCircleShape *)shape;
	
	circle.r = radius;
}

void
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
{
	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = cast(cpCircleShape *)shape;
	
	circle.c = offset;
}

void
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
{
	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
	
	seg.a = a;
	seg.b = b;
	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
}

void
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
{
	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
	
	seg.r = radius;
}