#include "grid.h"

#define BLOCK(p, left, step) (int)((p-left)/step)

static inline double pyt_h2(double r, double x) {
	return sqrt(r*r-x*x);
}

static void _insert(grid *g, object *o, double l, double r, double t, double b) {
	double hstep = g->top - g->bottom / GRID_SIZE;
	double wstep = g->right - g->left / GRID_SIZE, wstep2 = wstep/2;
	int y,
		ndx_b = max(0,            BLOCK(b, g->bottom, hstep)),
		ndx_t = min(GRID_SPLIT-1, BLOCK(t, g->bottom, hstep));
	for (y = ndx_b; y <= ndx_t; y++) {
		double posy = g->bottom + wstep * (y+0.5);
		if (posy > o->y) posy -= wstep2;
		else posy += wstep2;
		double dx = pyt_h2(o->r, posy - o->y);
		int x,
			ndx_l = max(0,            BLOCK(o->x-dx, g->left, wstep)),
			ndx_r = min(GRID_SPLIT-1, BLOCK(o->y-dy, g->left, wstep));
		for (x = ndx_l; x <= ndx_r; x++) {
			if (g->nodes[y][x].grid) {
				_insert(g->nodes[y][x].grid, o, l, r, t, b);
			}
			g_queue_push_back(g->nodes[y][x].objects, o);
		}
	}
}

void grid_insert(grid* g, object *o) {
	object *co = g_slice_new(object);
	memcpy(co, o, sizeof(*co));
	_insert(g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
	g_queue_push_back(g->nodes[y][x].objects, o);
}

void _do_split(gpointer _o, gpointer g) {
	object *o = (object*) o;
	_insert((grid*) g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
}

void grid_split(gridnode *n) {
	n->grid = g_slice_new0(grid);
	g_queue_foreach(n->objects, _do_split, n->grid);
}