#include <errno.h>
#include <stdio.h>
#include <ode/ode.h>

static inline bool
eq(float a, float b)
{
	return (a < b ? b - a : a - b) < 0.01;
}

static inline bool
match(float a, float b, float c, float x, float y, float z)
{
	return ((eq(a, x) && eq(b, y) && eq(c, z)) ||
	    (eq(a, x) && eq(b, z) && eq(c, x)) ||
	    (eq(a, y) && eq(b, x) && eq(c, z)) ||
	    (eq(a, y) && eq(b, z) && eq(c, x)) ||
	    (eq(a, z) && eq(b, x) && eq(c, y)) ||
	    (eq(a, z) && eq(b, y) && eq(c, x)));
}

int main(int argc, char *argv[])
{
	FILE *f;
	char s[8192], *p, *q;
	struct {
		int n;
		float l, w, h;
	} in[100];
	float l, w, h, d[3];
	float volsum = 0.0;
	float surface, lb;
	dVector3 v[8], a, b, c;
	dMatrix3 R;
	dGeomID g[100];
	dContactGeom contact[16];
	int k, m = 0, n = 0;
	bool found = false;

	if (argc != 3) {
		fprintf(stderr, "usage: %s input-file output-file\n", argv[0]);
		exit(1);
	}

	if (!(f = fopen(argv[1], "r"))) {
		fprintf(stderr, "fopen: %s: %s\n", argv[1], strerror(errno));
		exit(1);
	}
	while (fgets(s, sizeof(s), f)) {
		sscanf(s, "%d %f %f %f",
		    &in[m].n, &in[m].l, &in[m].w, &in[m].h);
		m++;
	}
	fclose(f);

	dInitODE();

	if (!(f = fopen(argv[2], "r"))) {
		fprintf(stderr, "fopen: %s: %s\n", argv[2], strerror(errno));
		exit(1);
	}
	if (!fgets(s, sizeof(s), f) ||
	    sscanf(s, "%f %f %f", &l, &w, &h) != 3) {
		fprintf(stderr, "error: reading dimensions\n");
		exit(1);
	}
	while (fgets(s, sizeof(s), f)) {
		if (n > m || n == 100) {
			fprintf(stderr, "error: too many lines\n");
			exit(1);
		}
		if (sscanf(s, "%d", &k) != 1 || k != in[n].n) {
			fprintf(stderr, "error: reading id: %s", s);
			exit(1);
		}
		p = s;
		for (int i = 0; i < 8 && p; ++i) {

			if (!(p = strchr(p, '(')) ||
			    !(q = strchr(p, ')')) ||
			    sscanf(p + 1, "%f, %f, %f",
			    &d[0], &d[1], &d[2]) != 3) {
				fprintf(stderr, "error: reading: %s", s);
				exit(1);
			}
			if (d[0] < 0.0 || d[0] > l + 0.005) {
				fprintf(stderr, "error: x %.10f > l %.10f\n",
				    d[0], l);
				exit(1);
			}
			if (d[1] < 0.0 || d[1] > w + 0.005) {
				fprintf(stderr, "error: y %.10f > w %.10f\n",
				    d[1], w);
				exit(1);
			}
			if (d[2] < 0.0 || d[2] > h + 0.005) {
				fprintf(stderr, "error: z %.10f > h %.10f\n",
				    d[2], h);
				exit(1);
			}
			v[i][0] = d[0]; v[i][1] = d[1]; v[i][2] = d[2];
			p = q;
		}
		if (!p) {
			fprintf(stderr, "error: reading: %s", s);
			exit(1);
		}
		d[0] = dCalcPointsDistance3(v[0], v[1]);
		d[1] = dCalcPointsDistance3(v[0], v[3]);
		d[2] = dCalcPointsDistance3(v[0], v[4]);
		if (!match(d[0], d[1], d[2], in[n].l, in[n].w, in[n].h)) {
			fprintf(stderr, "error: id %d size mismatch: (%.4f, "
			    "%.4f, %.4f) != (%.4f, %.4f, %.4f)\n", k,
			    d[0], d[1], d[2], in[n].l, in[n].w, in[n].h);
			exit(1);
		}
		volsum += d[0] * d[1] * d[2];
		g[n] = dCreateBox(0, d[0], d[1], d[2]);
		c[0] = (v[0][0] + v[6][0]) / 2.0;
		c[1] = (v[0][1] + v[6][1]) / 2.0;
		c[2] = (v[0][2] + v[6][2]) / 2.0;
		dGeomSetPosition(g[n], c[0], c[1], c[2]);
		dSubtractVectors3(a, v[1], v[0]);
		dSubtractVectors3(b, v[3], v[0]);
		dRFrom2Axes(R, a[0], a[1], a[2], b[0], b[1], b[2]);
		dGeomSetRotation(g[n], R);
		n++;
	}
	if (n != m) {
		fprintf(stderr, "error: not enough lines\n");
		exit(1);
	}
	fclose(f);

	for (int i = 0; i < n - 1; ++i) {
		for (int j = i + 1; j < n; ++j) {
			int r = dCollide(g[i], g[j], 16, &contact[0],
			    sizeof(dContactGeom));
			if (!r)
				continue;
			float maxdepth = 0.0;
			for (int k = 0; k < r; ++k)
				if (contact[k].depth > maxdepth)
					maxdepth = contact[k].depth;
			if (maxdepth < 0.06)
				continue;
			fprintf(stderr, "error: collision for id %d:%d "
			    "with %d contacts:", i + 1, j + 1, r);
			for (int k = 0; k < r; ++k)
				fprintf(stderr, " %.3f", contact[k].depth);
			fprintf(stderr, "\n");
			found = true;
		}
	}
	if (found)
		exit(1);
	printf("verified N=%d OK\n", n);
	printf("  dimensions: %.2f x %.2f x %.2f\n", l, w, h);
	surface = 2.0 * (l * w + l * h + w * h);
	printf("  surface: %.2f\n", surface);
	lb = pow(volsum, 1.0 / 3.0);
	lb = 6.0 * lb * lb;
	printf("  lower bound: %.2f (plus %.2f percent)\n",
	    lb, (surface - lb) * 100.0 / lb);
	dCloseODE();
	return 0;
}