/* map3d.c */

#include <stdio.h>
#include <math.h>
#include <strings.h>
#include <fcntl.h>
#include <unistd.h>

#include "vm.h"

#define NLIGHTS 16

#define PANMAX 127
#define PANRANGE M_PI_2
#define PANOFFSET 128
#define TILTMAX 127
#define TILTRANGE M_PI_4
#define TILTOFFSET 128

#define MAP3D_FILENAME ".map3d.caldata"

struct light {
	double M[4][3];
	double cp[3][3];
	double pan[3];
	double tilt[3];
};

struct light map3d_cal[NLIGHTS];

#define MAG(x) (sqrt(SQUARE(x[0]) + SQUARE(x[1]) + SQUARE(x[2])))
#define SQUARE(x) ((x) * (x))
#define PYTHAG3(a, b) sqrt(SQUARE(a[0]-b[0]) + SQUARE(a[1]-b[1]) + SQUARE(a[2]-b[2]))

#define DEG(x) (180 * (x) / M_PI)

void normalise(double *v)
{
	double mag;
	int i;

	mag = MAG(v);

	for (i = 0; i < 3; i++)
		v[i] /= mag;
}

#define MM map3d_cal[light].M

void multiply(int light, double *i, double *o) {
	o[0] = i[0] * MM[0][0] + i[1] * MM[1][0] + i[2] * MM[2][0] + MM[3][0];
	o[1] = i[0] * MM[0][1] + i[1] * MM[1][1] + i[2] * MM[2][1] + MM[3][1];
	o[2] = i[0] * MM[0][2] + i[1] * MM[1][2] + i[2] * MM[2][2] + MM[3][2];
}

#undef MM

void map3d_init(void)
{
}

void map3d_close(void)
{
}

void map3d_save(void)
{
	int fd, rv;

	fd = open(MAP3D_FILENAME, O_WRONLY | O_CREAT, 0666);
	rv = write(fd, map3d_cal, sizeof(map3d_cal));
	if (rv != sizeof(map3d_cal))
		printf("Warning: Calibration data not saved correctly\n");
	close(fd);
}

int map3d_load(void)
{
	int fd, rv;

	fd = open(MAP3D_FILENAME, O_RDONLY, 0666);
	if (!fd)
		return 0;
	rv = read(fd, map3d_cal, sizeof(map3d_cal));
	if (rv != sizeof(map3d_cal))
		printf("Warning: Calibration data not read correctly\n");
	close(fd);
	return 1;
}

void map3d_transform(int light, double x, double y, double z,
    int *pan, int *tilt)
{
	double pv[3];
	double rv[3];
	double p, t;

//	printf("Transforming for light %d: (%f, %f, %f)\n", light, x, y, z);
	fflush(stdout);
	pv[0] = x;
	pv[1] = y;
	pv[2] = z;
	multiply(light, pv, rv);
	normalise(rv);
	t = asin(rv[1]);
	p = asin(rv[0]/cos(t));

	*pan = (int)round((p * PANMAX)/PANRANGE) + PANOFFSET;
	*tilt = (int)round((t * TILTMAX)/TILTRANGE) + TILTOFFSET;
	if (*pan < 0)
		*pan = 0;
	if (*pan > 255)
		*pan = 255;
	if (*tilt < 0)
		*tilt = 0;
	if (*tilt > 255)
		*tilt = 255;
//	printf("pan = %d, tilt = %d\n", *pan, *tilt);
}

void map3d_setcal(int light, int n, double x, double y, double z,
	int pan, int tilt)
{
	printf("setcal(%d, %d, %f, %f, %f, %d, %d)\n", light, n, x, y, z, pan, tilt);
	map3d_cal[light].cp[n][0] = x;
	map3d_cal[light].cp[n][1] = y;
	map3d_cal[light].cp[n][2] = z;
	map3d_cal[light].pan[n] = PANRANGE * ((double)pan - PANOFFSET) / PANMAX;
	map3d_cal[light].tilt[n] = TILTRANGE * ((double)tilt - TILTOFFSET)
	    / TILTMAX;
	printf("Setcal: pan = %f, tilt = %f\n", map3d_cal[light].pan[n], map3d_cal[light].tilt[n]);
}

void unitvector(double *vec, double pan, double tilt)
{
	double tmp[3];
	double cosp, sinp, cost, sint;

	/* Precalculate some values */
	cosp = cos(pan);
	sinp = sin(pan);
	cost = cos(tilt);
	sint = sin(tilt);

	/* Start with a unit vector */
	vec[0] = 0;
	vec[1] = 0;
	vec[2] = 1;

	/* Rotate around X axis (tilt) */
	tmp[0] = vec[0];
	tmp[1] = vec[1]*cost + vec[2]*sint;
	tmp[2] = vec[2]*cost - vec[1]*sint;

	/* Rotate around Y axis (pan) */
	vec[0] = tmp[0]*cosp + tmp[2]*sinp;
	vec[1] = tmp[1];
	vec[2] = tmp[2]*cosp - tmp[0]*sinp;
}

double dotproduct(double *a, double *b)
{
	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

void crossproduct(double *a, double *b, double *r)
{
	r[0] = a[1]*b[2] - a[2]*b[1];
	r[1] = a[2]*b[0] - a[0]*b[2];
	r[2] = a[0]*b[1] - a[1]*b[0];
}

void dumpmatrix(double sim[12][13]) {
        int i, j;

	for (i = 0; i < 12; i++) {
		for (j = 0; j < 13; j++)
			printf("\t%f", sim[i][j]);
		printf("\n");
	}
	printf("\n");
}

void eliminate(double sim[12][13], double *var)
{
	int i, j, k, maxpos;
	double max, val, tmp, x;

	dumpmatrix(sim);

        for (i = 0; i < 12; i++) {
                /* Find the maximum element in the ith column */
                max = 0.0;
                maxpos = i;
                for (j = i; j < 12; j++) {
                        val = fabs(sim[j][i]);
                        if (val > max) {
                                max = val;
                                maxpos = j;
                        }
                }
                if (maxpos != i)
                        for (j = 0; j < 13; j++) {
                                tmp = sim[maxpos][j];
                                sim[maxpos][j] = sim[i][j];
                                sim[i][j] = tmp;
                        }
                if (fabs(sim[i][i]) < 0.0001)
                        printf("Warning: Lost accuracy at row %d\n", i);
                for (j = i+1; j < 12; j++) {
                        if (fabs(sim[j][i]) > 0.0) {
                                /*
                                 * Subtract x times row i from row j,
                                 * where x is chosen such that
                                 * sim[i][i] * x = sim[j][i]
                                 */
                                x = sim[j][i] / sim[i][i];
                                for (k = 0; k < 13; k++)
                                        sim[j][k] -= x*sim[i][k];
                        }
                }
        }

	dumpmatrix(sim);

        /* Next, substitute in the unknowns. */
        for (i = 12-1; i >= 0; i--) {
                var[i] = sim[i][12];
                for (j = i+1; j < 12; j++)
                        var[i] -= var[j] * sim[i][j];
                var[i] /= sim[i][i];
        }
}

/* In: Guess, length, angle, space for solutions */
void solvetriangle(double c, double a, double A, double *solp, double *soln)
{
        double root;
        double cosA;
        double med;

        cosA = cos(A);
        med = 4*c*c*cosA*cosA - 4*(c*c-a*a);
        root = sqrt(med);

        if (isnan(root))
                /* sqrt() of a negative number */
                if (med > -0.000000001)
                        root = 0;

        *solp = (2*c*cosA + root)/2;
        *soln = (2*c*cosA - root)/2;
}

double try(int n, double dguess, double a, double b, double c, double A, double B, double C)
{
	double s1, s2;
	double e, f, d;

	solvetriangle(dguess, a, A, &s1, &s2);
	e = (n>=4?s2:s1);
	n = n % 4;
	solvetriangle(e, c, C, &s1, &s2);
	f = (n>=2?s2:s1);
	n = n % 2;
	solvetriangle(f, b, B, &s1, &s2);
	d = (n>=1?s2:s1);

	return d;
}

void getsolution(int n, double dguess, double a, double b, double c, double A, double B, double C, double *e, double *f)
{
	double s1, s2;

	solvetriangle(dguess, a, A, &s1, &s2);
	*e = (n>=4?s2:s1);
	n = n % 4;
	solvetriangle(*e, c, C, &s1, &s2);
	*f = (n>=2?s2:s1);
}

#define DINCR 0.005

/*
 * A is angle opposite a, between d and e
 * B is angle opposite b, between d and f
 * C is angle opposite c, between e and f
 */
int trysolvetetra(int n, double a, double b, double c,
		  double A, double B, double C,
		  double *dr, double *er, double *fr)
{
	double dguess;
	double thresh = 0.000000000001;
	double d, lastd;
	double mind, maxd;
	double d1, d2, d3;
	double e, f;
	int found;

	lastd = 0.0/0.0; /* NaN */
	mind = maxd = 0; /* Shut up compiler */

	/* Find a bracket */
	found = 0;
	for (dguess = 0.0; dguess < 100.0; dguess+=DINCR) {
		d = try(n, dguess, a, b, c, A, B, C) - dguess;
		if (((d >= 0) && (lastd < 0)) || ((d <= 0) && (lastd > 0))) {
			mind = dguess - DINCR;
			maxd = dguess;
			printf("Found bracket: (%f, %f)\n", mind, maxd);
			found = 1;
			break;
		}
		lastd = d;
	}
	if (!found)
		return 0;

	while ((maxd-mind) > thresh) {
		d1 = try(n, mind, a, b, c, A, B, C) - mind;
		d2 = try(n, (mind+maxd)/2, a, b, c, A, B, C) - (mind+maxd)/2;
		d3 = try(n, maxd, a, b, c, A, B, C) - maxd;
		if (((d2 >= 0) && (d1 < 0)) || ((d2 < 0) && (d1 >= 0)))
			maxd = (mind+maxd)/2;
		else
			mind = (mind+maxd)/2;
	}

	d = (mind+maxd)/2;

	getsolution(n, d, a, b, c, A, B, C, &e, &f);

	printf("Found solution: (%f, %f, %f)\n", d, e, f);

	*dr = d;
	*er = e;
	*fr = f;

	if (!isfinite(d))
		return 0;
	if (!isfinite(e))
		return 0;
	if (!isfinite(f))
		return 0;
	if (d < 0)
		return 0;
	if (e < 0)
		return 0;
	if (f < 0)
		return 0;
	return 1;
}

int solvetetra(double a, double b, double c,
	       double A, double B, double C,
	       double *dr, double *er, double *fr)
{
	int i;

	for (i = 0; i < 8; i++)
		if (trysolvetetra(i, a, b, c, A, B, C, dr, er, fr))
			return 1;
	return 0;
}

int map3d_calibrate(int light)
{
	double v1[3], v2[3], v3[3], p4[3];
	double angle1, angle2, angle3;
	double a, b, c, d, e, f;
	double pAB[3], pBC[3];
	double axis1[3], axis2[3], axis3[3];
	double axm1, axm2, axm3;
	double hypot, alpha, beta, gamma, delta;
	double sim[12][13];
	double var[12];
	double pv[3], tv[3];
	int success, i;

	/*
	 * First, create unit vectors in the directions of the
	 * pan & tilt values given
	 */
	unitvector(v1, map3d_cal[light].pan[0], map3d_cal[light].tilt[0]);
	unitvector(v2, map3d_cal[light].pan[1], map3d_cal[light].tilt[1]);
	unitvector(v3, map3d_cal[light].pan[2], map3d_cal[light].tilt[2]);

	/*
	 * Now, we need the angles between them
	 */
	angle1 = acos(dotproduct(v1, v2));
	angle2 = acos(dotproduct(v2, v3));
	angle3 = acos(dotproduct(v3, v1));

	printf("angles (%f, %f, %f)\n", DEG(angle1), DEG(angle2), DEG(angle3));

	/*
	 * And the lengths of the edges which we know
	 */
	a = PYTHAG3(map3d_cal[light].cp[0], map3d_cal[light].cp[1]);
	b = PYTHAG3(map3d_cal[light].cp[1], map3d_cal[light].cp[2]);
	c = PYTHAG3(map3d_cal[light].cp[2], map3d_cal[light].cp[0]);

	/* Solve the tetrahedron */
	success = solvetetra(a, b, c, angle1, angle2, angle3, &d, &e, &f);
	if (!success) {
		printf("failed to solve tetrahedron\n");
		return 0;
	}

	/* Multiply the vectors by their magnitudes */
	for (i = 0; i < 3; i++) {
		v1[i] *= e;
		v2[i] *= d;
		v3[i] *= f;
	}

	/*
	 * Find two vectors to define the triangle between
	 * the calibration points
	 */
	for (i = 0; i < 3; i++) {
		pAB[i] = map3d_cal[light].cp[1][i]-map3d_cal[light].cp[0][i];
		pBC[i] = map3d_cal[light].cp[2][i]-map3d_cal[light].cp[1][i];
	}

	/*
	 * Create some perpendicular vectors in terms of which we can
	 * calculate a vector to the fourth point
	 */
	axis1[0] = pAB[0];
	axis1[1] = pAB[1];
	axis1[2] = pAB[2];
	normalise(axis1);
	crossproduct(axis1, pBC, axis2);
	normalise(axis2);
	crossproduct(axis1, axis2, axis3);
	normalise(axis3);

	/*
	 * Now we do some trigonometry to find out the distance to
	 * the fourth point in terms of the three axes
	 */
	beta = asin(d*sin(angle1)/a);
	gamma = asin(f*sin(angle3)/c);
	delta = acos(((a*a)+(c*c)-(b*b))/(2*a*c));

	alpha = acos((cos(gamma)-cos(beta)*cos(delta))/(sin(beta)*sin(delta)));

	hypot = e*sin(beta);
	axm1 = e*cos(beta);
	axm2 = hypot*sin(alpha);
	axm3 = hypot*cos(alpha);

	/* Now we have the magnitudes, let's get the vectors */
	for (i = 0; i < 3; i++) {
		axis1[i] *= axm1;
		axis2[i] *= axm2;
		axis3[i] *= axm3;
	}

	/*
	 * Now we can simply add these vectors to point A
	 * to get the fourth point
	 */
	for (i = 0; i < 3; i++)
		p4[i] = map3d_cal[light].cp[0][i] + axis1[i] +
		    axis2[i] + axis3[i];

	/*
	 * Now we can construct a matrix to represent 12 simultaneous
	 * equations, which we then solve to get our transformation matrix.
	 */
	bzero(sim, sizeof(sim));

	/* First point */
	sim[0][0] = map3d_cal[light].cp[0][0];
	sim[0][1] = map3d_cal[light].cp[0][1];
	sim[0][2] = map3d_cal[light].cp[0][2];
	sim[0][9] = 1;
	sim[0][12] = v1[0];

	sim[1][3] = map3d_cal[light].cp[0][0];
	sim[1][4] = map3d_cal[light].cp[0][1];
	sim[1][5] = map3d_cal[light].cp[0][2];
	sim[1][10] = 1;
	sim[1][12] = v1[1];

	sim[2][6] = map3d_cal[light].cp[0][0];
	sim[2][7] = map3d_cal[light].cp[0][1];
	sim[2][8] = map3d_cal[light].cp[0][2];
	sim[2][11] = 1;
	sim[2][12] = v1[2];

	/* Second point */
	sim[3][0] = map3d_cal[light].cp[1][0];
	sim[3][1] = map3d_cal[light].cp[1][1];
	sim[3][2] = map3d_cal[light].cp[1][2];
	sim[3][9] = 1;
	sim[3][12] = v2[0];

	sim[4][3] = map3d_cal[light].cp[1][0];
	sim[4][4] = map3d_cal[light].cp[1][1];
	sim[4][5] = map3d_cal[light].cp[1][2];
	sim[4][10] = 1;
	sim[4][12] = v2[1];

	sim[5][6] = map3d_cal[light].cp[1][0];
	sim[5][7] = map3d_cal[light].cp[1][1];
	sim[5][8] = map3d_cal[light].cp[1][2];
	sim[5][11] = 1;
	sim[5][12] = v2[2];

	/* Third point */
	sim[6][0] = map3d_cal[light].cp[2][0];
	sim[6][1] = map3d_cal[light].cp[2][1];
	sim[6][2] = map3d_cal[light].cp[2][2];
	sim[6][9] = 1;
	sim[6][12] = v3[0];

	sim[7][3] = map3d_cal[light].cp[2][0];
	sim[7][4] = map3d_cal[light].cp[2][1];
	sim[7][5] = map3d_cal[light].cp[2][2];
	sim[7][10] = 1;
	sim[7][12] = v3[1];

	sim[8][6] = map3d_cal[light].cp[2][0];
	sim[8][7] = map3d_cal[light].cp[2][1];
	sim[8][8] = map3d_cal[light].cp[2][2];
	sim[8][11] = 1;
	sim[8][12] = v3[2];

	/* Fourth point */
	sim[9][0] = p4[0];
	sim[9][1] = p4[1];
	sim[9][2] = p4[2];
	sim[9][9] = 1;
	sim[9][12] = 0;

	sim[10][3] = p4[0];
	sim[10][4] = p4[1];
	sim[10][5] = p4[2];
	sim[10][10] = 1;
	sim[10][12] = 0;

	sim[11][6] = p4[0];
	sim[11][7] = p4[1];
	sim[11][8] = p4[2];
	sim[11][11] = 1;
	sim[11][12] = 0;

	eliminate(sim, var);

	map3d_cal[light].M[0][0] = var[0];
	map3d_cal[light].M[1][0] = var[1];
	map3d_cal[light].M[2][0] = var[2];
	map3d_cal[light].M[0][1] = var[3];
	map3d_cal[light].M[1][1] = var[4];
	map3d_cal[light].M[2][1] = var[5];
	map3d_cal[light].M[0][2] = var[6];
	map3d_cal[light].M[1][2] = var[7];
	map3d_cal[light].M[2][2] = var[8];
	map3d_cal[light].M[3][0] = var[9];
	map3d_cal[light].M[3][1] = var[10];
	map3d_cal[light].M[3][2] = var[11];

	printf("%f\t%f\t%f\t\t%f\n", var[0], var[3], var[6], var[9]);
	printf("%f\t%f\t%f\t\t%f\n", var[1], var[4], var[7], var[10]);
	printf("%f\t%f\t%f\t\t%f\n", var[2], var[5], var[8], var[11]);

	printf("Calibration points are:\n");
	multiply(light, map3d_cal[light].cp[0], tv);
	printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[0][0], map3d_cal[light].cp[0][1], map3d_cal[light].cp[0][2], tv[0], tv[1], tv[2]);
	multiply(light, map3d_cal[light].cp[1], tv);
	printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[1][0], map3d_cal[light].cp[1][1], map3d_cal[light].cp[1][2], tv[0], tv[1], tv[2]);
	multiply(light, map3d_cal[light].cp[2], tv);
	printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[2][0], map3d_cal[light].cp[2][1], map3d_cal[light].cp[2][2], tv[0], tv[1], tv[2]);
	multiply(light, p4, tv);
	printf("(%f, %f, %f) => (%f, %f, %f)\n", p4[0], p4[1], p4[2], tv[0], tv[1], tv[2]);
	for (i = 0; i < 10; i++) {
		pv[0] = 0;
		pv[1] = (double)i/10.0;
		pv[2] = 0;
		multiply(light, pv, tv);
		printf("(%f, %f, %f) => (%f, %f, %f)\n", pv[0], pv[1], pv[2], tv[0], tv[1], tv[2]);
	}
	return 1;
}

/* Set parameters based on position (x, y, z) and direction (tx, ty, tz) */
/* Temp: x, y = pan, tilt measured to (0, 0, 0)
         tx, ty = pan, tilt of light fixture
	 tz = distance from light fixture to (0, 0, 0)
 */
void map3d_setparams(int light, int opan, int otilt, double lpan, double ltilt, double dist)
{
	double n[3];
	double up[3];
	double right[3];
	double v[3];
	double op, ot;

	op = PANRANGE * ((double)opan - PANOFFSET) / PANMAX;
	ot = TILTRANGE * ((double)otilt - TILTOFFSET) / TILTMAX;

	unitvector(v, op, ot);
	unitvector(n, lpan, ltilt);

	up[0] = 0;
	up[1] = (n[1] + sqrt(n[1]*n[1]+4*n[2]*n[2])) /
		(4*n[2]);
	up[2] = -up[1]*n[1]/n[2];
	normalise(up);

	crossproduct(n, up, right);
	normalise(right);

	printf("n:\t%f\t%f\t%f\n", n[0], n[1], n[2]);
	printf("up:    \t%f\t%f\t%f\n", up[0], up[1], up[2]);
	printf("right: \t%f\t%f\t%f\n", right[0], right[1], right[2]);
	printf("\n");

	/* Construct matrix */

	map3d_cal[light].M[0][0] = right[0];
	map3d_cal[light].M[0][1] = right[1];
	map3d_cal[light].M[0][2] = right[2];
	map3d_cal[light].M[1][0] = up[0];
	map3d_cal[light].M[1][1] = up[1];
	map3d_cal[light].M[1][2] = up[2];
	map3d_cal[light].M[2][0] = n[0];
	map3d_cal[light].M[2][1] = n[1];
	map3d_cal[light].M[2][2] = n[2];
	map3d_cal[light].M[3][0] = dist*v[0];
	map3d_cal[light].M[3][1] = dist*v[1];
	map3d_cal[light].M[3][2] = dist*v[2];

	printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[0][0], map3d_cal[light].M[0][1], map3d_cal[light].M[0][2], map3d_cal[light].M[3][0]);
	printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[1][0], map3d_cal[light].M[1][1], map3d_cal[light].M[1][2], map3d_cal[light].M[3][1]);
	printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[2][0], map3d_cal[light].M[2][1], map3d_cal[light].M[2][2], map3d_cal[light].M[3][2]);
	printf("\n");

}