// written in the D programming language

module chipmunkd.cpSpaceQuery;

import chipmunkd.chipmunk;
import chipmunkd.chipmunk_types;

struct pointQueryContext {
	cpLayers layers;
	cpGroup group;
	cpSpacePointQueryFunc func;
	void *data;
}

static void 
pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
{
	if(
		!(shape.group && context.group == shape.group) && (context.layers&shape.layers) &&
		cpShapePointQuery(shape, *point)
	){
		context.func(shape, context.data);
	}
}

void
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
{
	pointQueryContext context = {layers, group, func, data};
	
	cpBool locked = space.locked; space.locked = cpTrue; {
		cpSpaceHashPointQuery(space.activeShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
		cpSpaceHashPointQuery(space.staticShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
	} space.locked = locked;
}

static void
rememberLastPointQuery(cpShape *shape, cpShape **outShape)
{
	if(!shape.sensor) *outShape = shape;
}

cpShape *
cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
{
	cpShape *shape = null;
	cpSpacePointQuery(space, point, layers, group, cast(cpSpacePointQueryFunc)&rememberLastPointQuery, &shape);
	
	return shape;
}

void
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
{
	cpArray *bodies = space.bodies;
	
	for(int i=0; i<bodies.num; i++)
		func(cast(cpBody *)bodies.arr[i], data);
}

//#pragma mark Segment Query Functions

struct segQueryContext {
	cpVect start, end;
	cpLayers layers;
	cpGroup group;
	cpSpaceSegmentQueryFunc func;
}

static cpFloat
segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
{
	cpSegmentQueryInfo info;
	
	if(
		!(shape.group && context.group == shape.group) && (context.layers&shape.layers) &&
		cpShapeSegmentQuery(shape, context.start, context.end, &info)
	){
		context.func(shape, info.t, info.n, data);
	}
	
	return 1.0f;
}

void
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
{
	segQueryContext context = {
		start, end,
		layers, group,
		func,
	};
	
	cpBool locked = space.locked; space.locked = cpTrue; {
		cpSpaceHashSegmentQuery(space.staticShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
		cpSpaceHashSegmentQuery(space.activeShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
	} space.locked = locked;
}

struct segQueryFirstContext {
	cpVect start, end;
	cpLayers layers;
	cpGroup group;
}

static cpFloat
segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *_out)
{
	cpSegmentQueryInfo info;
	
	if(
		!(shape.group && context.group == shape.group) &&
		(context.layers&shape.layers) &&
		!shape.sensor &&
		cpShapeSegmentQuery(shape, context.start, context.end, &info) &&
		info.t < _out.t
	){
		*_out = info;
	}
	
	return _out.t;
}

cpShape *
cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *_out)
{
	cpSegmentQueryInfo info = {null, 1.0f, cpvzero};
	if(_out){
		(*_out) = info;
  } else {
		_out = &info;
	}
	
	segQueryFirstContext context = {
		start, end,
		layers, group
	};
	
	cpSpaceHashSegmentQuery(space.staticShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFirst, _out);
	cpSpaceHashSegmentQuery(space.activeShapes, &context, start, end, _out.t, cast(cpSpaceHashSegmentQueryFunc)&segQueryFirst, _out);
	
	return _out.shape;
}

//#pragma mark BB Query functions

struct bbQueryContext {
	cpLayers layers;
	cpGroup group;
	cpSpaceBBQueryFunc func;
	void *data;
}

static void 
bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
{
	if(
		!(shape.group && context.group == shape.group) && (context.layers&shape.layers) &&
		cpBBintersects(*bb, shape.bb)
	){
		context.func(shape, context.data);
	}
}

void
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
{
	bbQueryContext context = {layers, group, func, data};
	
	cpBool locked = space.locked; space.locked = cpTrue; {
	cpSpaceHashQuery(space.activeShapes, &bb, bb, cast(cpSpaceHashQueryFunc)&bbQueryHelper, &context);
	cpSpaceHashQuery(space.staticShapes, &bb, bb, cast(cpSpaceHashQueryFunc)&bbQueryHelper, &context);
	} space.locked = locked;
}

//#pragma mark Shape Query Functions

struct shapeQueryContext {
	cpSpaceShapeQueryFunc func;
	void *data;
	cpBool anyCollision;
}

// Callback from the spatial hash.
static void
shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context)
{
	// Reject any of the simple cases
	if(
		(a.group && a.group == b.group) ||
		!(a.layers & b.layers) ||
		a.sensor || b.sensor
	) return;
	
	cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
	int numContacts = 0;
	
	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
	if(a.klass.type <= b.klass.type){
		numContacts = cpCollideShapes(a, b, contacts.ptr);
	} else {
		numContacts = cpCollideShapes(b, a, contacts.ptr);
		for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
	}
	
	if(numContacts){
		context.anyCollision = cpTrue;
		
		if(context.func){
			cpContactPointSet set; set.count = numContacts;
			for(int i=0; i<set.count; i++){
				set.points[i].point = contacts[i].p;
				set.points[i].normal = contacts[i].p;
				set.points[i].dist = contacts[i].dist;
			}
			
			context.func(b, &set, context.data);
		}
	}
}

cpBool
cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
{
	cpBB bb = cpShapeCacheBB(shape);
	shapeQueryContext context = {func, data, cpFalse};
	
	cpBool locked = space.locked; space.locked = cpTrue; {
		cpSpaceHashQuery(space.activeShapes, shape, bb, cast(cpSpaceHashQueryFunc)&shapeQueryHelper, &context);
		cpSpaceHashQuery(space.staticShapes, shape, bb, cast(cpSpaceHashQueryFunc)&shapeQueryHelper, &context);
	} space.locked = locked;
	
	return context.anyCollision;
}