// written in the D programming language

module chipmunkd.cpPolyShape;

import chipmunkd.chipmunk;

// Axis structure used by cpPolyShape.
struct cpPolyShapeAxis{
	// normal
	cpVect n;
	// distance from origin
	cpFloat d;
}

// Convex polygon shape structure.
struct cpPolyShape{
	cpShape shape;
	
	// Vertex and axis lists.
	int numVerts;
	cpVect *verts;
	cpPolyShapeAxis *axes;

	// Transformed vertex and axis lists.
	cpVect *tVerts;
	cpPolyShapeAxis *tAxes;
}

//// Basic allocation functions.
//cpPolyShape *cpPolyShapeAlloc(void);
//cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset);
//cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset);
//
//cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height);
//cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
//
//// Check that a set of vertexes has a correct winding and that they are convex
//cpBool cpPolyValidate(cpVect *verts, int numVerts);
//
//int cpPolyShapeGetNumVerts(cpShape *shape);
//cpVect cpPolyShapeGetVert(cpShape *shape, int idx);

// *** inlined utility functions

// Returns the minimum distance of the polygon to the axis.
static cpFloat
cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
{
	const cpVect *verts = poly.tVerts;
	cpFloat min = cpvdot(n, verts[0]);
	
	int i;
	for(i=1; i<poly.numVerts; i++)
		min = cpfmin(min, cpvdot(n, verts[i]));
	
	return min - d;
}

// Returns true if the polygon contains the vertex.
static cpBool
cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
{
	const cpPolyShapeAxis *axes = poly.tAxes;
	
	int i;
	for(i=0; i<poly.numVerts; i++){
		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
		if(dist > 0.0f) return cpFalse;
	}
	
	return cpTrue;
}

// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal.
static cpBool
cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
{
	const cpPolyShapeAxis *axes = poly.tAxes;
	
	int i;
	for(i=0; i<poly.numVerts; i++){
		if(cpvdot(axes[i].n, n) < 0.0f) continue;
		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
		if(dist > 0.0f) return cpFalse;
	}
	
	return cpTrue;
}

// cpPolyShape.c ---------------------------------


cpPolyShape *
cpPolyShapeAlloc()
{
	return cast(cpPolyShape *)cpcalloc(1, cpPolyShape.sizeof);
}

static void
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
	cpVect *src = poly.verts;
	cpVect *dst = poly.tVerts;
	
	for(int i=0; i<poly.numVerts; i++)
		dst[i] = cpvadd(p, cpvrotate(src[i], rot));
}

static void
cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot)
{
	cpPolyShapeAxis *src = poly.axes;
	cpPolyShapeAxis *dst = poly.tAxes;
	
	for(int i=0; i<poly.numVerts; i++){
		cpVect n = cpvrotate(src[i].n, rot);
		dst[i].n = n;
		dst[i].d = cpvdot(p, n) + src[i].d;
	}
}

static cpBB
cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
	cpPolyShape *poly = cast(cpPolyShape *)shape;
	
	cpFloat l, b, r, t;
	
	cpPolyShapeTransformAxes(poly, p, rot);
	cpPolyShapeTransformVerts(poly, p, rot);
	
	cpVect *verts = poly.tVerts;
	l = r = verts[0].x;
	b = t = verts[0].y;
	
	// TODO do as part of cpPolyShapeTransformVerts?
	for(int i=1; i<poly.numVerts; i++){
		cpVect v = verts[i];
		
		l = cpfmin(l, v.x);
		r = cpfmax(r, v.x);
		
		b = cpfmin(b, v.y);
		t = cpfmax(t, v.y);
	}
	
	return cpBBNew(l, b, r, t);
}

static void
cpPolyShapeDestroy(cpShape *shape)
{
	cpPolyShape *poly = cast(cpPolyShape *)shape;
	
	cpfree(poly.verts);
	cpfree(poly.tVerts);
	
	cpfree(poly.axes);
	cpfree(poly.tAxes);
}

static cpBool
cpPolyShapePointQuery(cpShape *shape, cpVect p){
	return cpBBcontainsVect(shape.bb, p) && cpPolyShapeContainsVert(cast(cpPolyShape *)shape, p);
}

static void
cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpPolyShape *poly = cast(cpPolyShape *)shape;
	cpPolyShapeAxis *axes = poly.tAxes;
	cpVect *verts = poly.tVerts;
	int numVerts = poly.numVerts;
	
	for(int i=0; i<numVerts; i++){
		cpVect n = axes[i].n;
		cpFloat an = cpvdot(a, n);
		if(axes[i].d > an) continue;
		
		cpFloat bn = cpvdot(b, n);
		cpFloat t = (axes[i].d - an)/(bn - an);
		if(t < 0.0f || 1.0f < t) continue;
		
		cpVect point = cpvlerp(a, b, t);
		cpFloat dt = -cpvcross(n, point);
		cpFloat dtMin = -cpvcross(n, verts[i]);
		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
		
		if(dtMin <= dt && dt <= dtMax){
			info.shape = shape;
			info.t = t;
			info.n = n;
		}
	}
}

static /+const+/ cpShapeClass polyClass = {
	cpShapeType.CP_POLY_SHAPE,
	&cpPolyShapeCacheData,
	&cpPolyShapeDestroy,
	&cpPolyShapePointQuery,
	&cpPolyShapeSegmentQuery,
};

cpBool
cpPolyValidate(in cpVect *verts, in int numVerts)
{
	for(int i=0; i<numVerts; i++){
		cpVect a = verts[i];
		cpVect b = verts[(i+1)%numVerts];
		cpVect c = verts[(i+2)%numVerts];
		
		if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
			return cpFalse;
	}
	
	return cpTrue;
}

int
cpPolyShapeGetNumVerts(cpShape *shape)
{
	assert(shape.klass == &polyClass, "Shape is not a poly shape.");
	return (cast(cpPolyShape *)shape).numVerts;
}

cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
	assert(shape.klass == &polyClass, "Shape is not a poly shape.");
	assert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
	
	return (cast(cpPolyShape *)shape).verts[idx];
}


static void
setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
{
	poly.numVerts = numVerts;

	poly.verts	= cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof);
	poly.tVerts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof);
	poly.axes	= cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof);
	poly.tAxes	= cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof);
	
	for(int i=0; i<numVerts; i++){
		cpVect a = cpvadd(offset, verts[i]);
		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));

		poly.verts[i] = a;
		poly.axes[i].n = n;
		poly.axes[i].d = cpvdot(n, a);
	}
}

cpPolyShape *
cpPolyShapeInit(cpPolyShape *poly, cpBody *_body, int numVerts, cpVect *verts, cpVect offset)
{
	// Fail if the user attempts to pass a concave poly, or a bad winding.
	assert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");
	
	setUpVerts(poly, numVerts, verts, offset);
	cpShapeInit(cast(cpShape *)poly, &polyClass, _body);

	return poly;
}

cpShape *
cpPolyShapeNew(cpBody *_body, int numVerts, cpVect *verts, cpVect offset)
{
	return cast(cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), _body, numVerts, verts, offset);
}

cpPolyShape *
cpBoxShapeInit(cpPolyShape *poly, cpBody *_body, cpFloat width, cpFloat height)
{
	cpFloat hw = width/2.0f;
	cpFloat hh = height/2.0f;
	
	cpVect verts[] = [
		cpv(-hw,-hh),
		cpv(-hw, hh),
		cpv( hw, hh),
		cpv( hw,-hh),
	];
	
	return cpPolyShapeInit(poly, _body, 4, verts.ptr, cpvzero);
}

cpShape *
cpBoxShapeNew(cpBody *_body, cpFloat width, cpFloat height)
{
	return cast(cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), _body, width, height);
}

// Unsafe API (chipmunk_unsafe.h)

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