Merge branch 'master' of https://github.com/cnlohr/libsurvive

9 files changed, 3513 insertions, 190 deletions

diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index e81e154..9d02bb3 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 //    FLT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
 //    FLT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLT ydist  = 1.0 / urhat_len;
-    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+    printf("ydist %f\n", ydist);
+//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 	if( x_y != x_y ) x_y = 0;
 	if( y_y != y_y ) y_y = 0;
 	if( z_y != z_y ) z_y = 0;
-/*
+
     // Exhaustively flip the minus sign of the z axis until we find the right one . . .
     FLT bestErr = 9999.0;
     FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
@@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
-                printf("err %f hand %f\n", err, hand);
+//                printf("err %f hand %f\n", err, hand);
                 
                 // If we are the best right-handed frame so far
                 //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
@@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
         x_y = -x_y;
     }
-    printf("bestErr %f\n", bestErr);
-*/
+//    printf("bestErr %f\n", bestErr);
 
+/*
     //-------------------------
     // A test version of the rescaling to the proper length
     //-------------------------
@@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 		if( y_y != y_y ) y_y = 0;
 		if( z_y != z_y ) z_y = 0;
 
-		printf( "---> %f %f %f\n", x_y, y_y, z_y );
+//		printf( "---> %f %f %f\n", x_y, y_y, z_y );
 
         // Exhaustively flip the minus sign of the z axis until we find the right one . . .
         FLT bestErr = 9999.0;
@@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                     FLT cx,cy,cz;
                     CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
                     FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
-                  printf("err %f hand %f\n", err, hand);
+                  //printf("err %f hand %f\n", err, hand);
                 
                     // If we are the best right-handed frame so far
                     if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
@@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
     }
     ydist = bestYdist;
-
+*/
 /*
     for (i=0; i<nPoints; i++) {
         FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
@@ -441,7 +442,103 @@ PRINT(ab,2,1);
     T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
 
-    PRINT_MAT(T,4,4);
+
+	FLT T2[4][4];
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+	FLT temp[4][4];
+	FLT quat[4], quatNorm[4];
+	FLT euler[3];
+
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+	PRINT_MAT(T,4,4);
+
+#if 1
+//	matrix44transpose(T2, T);  //Transpose so we are
+	matrix44copy((FLT*)T2,(FLT*)T);
+	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
+	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
+	matrix44copy((FLT*)T,(FLT*)T2);
+//	matrix44transpose(T, T2);
+
+#endif
+
+	normalize3d( &T[0][0], &T[0][0] );
+	normalize3d( &T[1][0], &T[1][0] );
+	normalize3d( &T[2][0], &T[2][0] );
+	//Change handedness
+
+	T[1][0]*=-1;
+	T[1][1]*=-1;
+	T[1][2]*=-1;
+
+/*
+	//Check Orthogonality.  Yep. It's orthogonal.
+	FLT tmp[3];
+	cross3d( tmp, &T[0][0], &T[1][0] );
+	printf( "M3: %f\n", magnitude3d( tmp ) );
+	cross3d( tmp, &T[2][0], &T[1][0] );
+	printf( "M3: %f\n", magnitude3d( tmp ) );
+	cross3d( tmp, &T[2][0], &T[0][0] );
+	printf( "M3: %f\n", magnitude3d( tmp ) );
+*/
+
+//	PRINT_MAT(T,4,4);
+
+#if 1
+
+	//matrix44copy(T2,T);
+	matrix44transpose((FLT*)T2,(FLT*)T);
+
+	quatfrommatrix( quat, &T2[0][0] );
+	printf( "QM: %f\n", quatmagnitude( quat ) );
+	quatnormalize(quatNorm,quat);
+	quattoeuler(euler,quatNorm);
+	quattomatrix( &T2[0][0], quatNorm );
+
+	PRINT_MAT(T2,4,4);
+	printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat));
+//	PRINT(T,4,4);
+
+//	matrix44copy(temp,T2);
+	matrix44transpose((FLT*)temp,(FLT*)T2);
+
+
+//	matrix44transpose(T2, temp);
+//	memcpy(T2,temp,16*sizeof(float));
+	for (i=0; i<3; i++) {
+		for (j=0; j<3; j++) {
+			T[i][j] = temp[i][j];
+		}
+	}
+
+/*	PRINT(T2,4,4); */
+#endif
+
+	T[1][0]*=-1;
+	T[1][1]*=-1;
+	T[1][2]*=-1;
+
+
+/*
+    CrossProduct(T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1]);
+    CrossProduct(T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2]);
+    CrossProduct(T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0]);
+	float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]);
+	float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]);
+	float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]);
+	T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen;
+	T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen;
+	T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen;
+*/
+
+
+ //   PRINT_MAT(T,4,4);
     //-------------------
     // Plot the output points
     //-------------------
@@ -453,7 +550,7 @@ PRINT(ab,2,1);
         S_out[1][i] = atan2(Tz, Ty);   // vert
         //S_out[0][i] = Tx;
         //S_out[1][i] = Tz;
-        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
+//        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
     }
 
 }
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
new file mode 100644
index 0000000..0d8674c
--- /dev/null
+++ b/src/poser_octavioradii.c
@@ -0,0 +1,721 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+typedef struct
+{
+#define OLD_ANGLES_BUFF_LEN 3
+	FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+	int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+	int lastAxis[NUM_LIGHTHOUSES];
+
+	int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+} OctavioRadiiData;
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "linmath.h"
+#include <string.h>
+#include <stdint.h>
+#include <math.h>
+
+#define PTS 32
+#define MAX_CHECKS 40000
+#define MIN_HITS_FOR_VALID 10
+
+FLT hmd_points[PTS * 3];
+FLT hmd_norms[PTS * 3];
+FLT hmd_point_angles[PTS * 2];
+int hmd_point_counts[PTS * 2];
+int best_hmd_target = 0;
+
+//Values used for RunTest()
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+
+#define MAX_POINT_PAIRS 100
+
+typedef struct
+{
+	FLT x;
+	FLT y;
+	FLT z;
+} Point;
+
+typedef struct
+{
+	Point point; // location of the sensor on the tracked object;
+	Point normal; // unit vector indicating the normal for the sensor
+	double theta; // "horizontal" angular measurement from lighthouse radians
+	double phi; // "vertical" angular measurement from lighthouse in radians.
+	int id;
+} TrackedSensor;
+
+typedef struct
+{
+	size_t numSensors;
+	TrackedSensor sensor[0];
+} TrackedObject;
+
+typedef struct
+{
+	unsigned char index1;
+	unsigned char index2;
+	FLT KnownDistance;
+} PointPair;
+
+static FLT distance(Point a, Point b)
+{
+	FLT x = a.x - b.x;
+	FLT y = a.y - b.y;
+	FLT z = a.z - b.z;
+	return FLT_SQRT(x*x + y*y + z*z);
+}
+
+typedef struct
+{
+	FLT HorizAngle;
+	FLT VertAngle;
+} SensorAngles;
+
+#define SQUARED(x) ((x)*(x))
+
+static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < numPairs; i++)
+	{
+		FLT estimatedDistanceBetweenPoints =
+			SQUARED(radii[pairs[i].index1])
+			+ SQUARED(radii[pairs[i].index2])
+			- 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
+			* FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
+			* FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
+			+ FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);
+
+		fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+	}
+
+	return FLT_SQRT(fitness);
+}
+
+// angles is an array of angles between a sensor pair
+// pairs is an array of point pairs
+// radii is the guess at the radii of those angles
+static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < numPairs; i++)
+	{
+		// These are the vectors that represent the direction for the two points.  
+		// TODO: optimize by precomputing the tangent.
+		FLT v1[3], v2[3], diff[3];
+
+		v1[0] = 1;
+		v2[0] = 1;
+		v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed
+		v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed
+		v1[2] = tan(angles[pairs[i].index1].VertAngle);  // can be precomputed
+		v2[2] = tan(angles[pairs[i].index2].VertAngle);  // can be precomputed
+
+														 // Now, normalize the vectors
+		normalize3d(v1, v1); // can be precomputed
+		normalize3d(v2, v2); // can be precomputed
+
+							 // Now, given the specified radii, find where the new points are
+		scale3d(v1, v1, radii[pairs[i].index1]);
+		scale3d(v2, v2, radii[pairs[i].index2]);
+
+		// Cool, now find the vector between these two points
+		// TODO: optimize the following two funcs into one.
+		sub3d(diff, v1, v2);
+
+		FLT distance = magnitude3d(diff);
+
+		FLT t1 = magnitude3d(v1);
+		FLT t2 = magnitude3d(v2);
+
+
+
+		FLT estimatedDistanceBetweenPoints =
+
+			SQUARED(radii[pairs[i].index1])
+			+ SQUARED(radii[pairs[i].index2])
+			- 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
+			* FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
+			* FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
+			+ FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);
+
+
+		//fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+		fitness += SQUARED(distance - pairs[i].KnownDistance);
+	}
+
+	return FLT_SQRT(fitness);
+}
+
+static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b)
+{
+	FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle));
+	//FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
+
+	return angle;
+}
+
+// angles is an array of angles between a sensor pair
+// pairs is an array of point pairs
+// radii is the guess at the radii of those angles
+static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < numPairs; i++)
+	{
+
+		FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]);
+
+		// now we have a side-angle-side triangle, and we need to find the third side.
+
+		// The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, 
+		// where A is the angle opposite side a.
+
+		// Transform this to:
+		// a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side!
+
+		FLT b2 = (SQUARED(radii[pairs[i].index1]));
+		FLT c2 = (SQUARED(radii[pairs[i].index2]));
+		FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]);
+		FLT cosA = (FLT_COS(angle));
+
+		FLT angleInDegrees = angle * 180 / LINMATHPI;
+
+		FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + 
+			             (SQUARED(radii[pairs[i].index2])) - 
+			             (	(2 * radii[pairs[i].index1] * radii[pairs[i].index2]) *
+							(FLT_COS(angle))));
+
+
+		FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance);
+
+		if (isnan(fitnessAdder))
+		{
+			int a = 0;
+		}
+
+		//printf("  %2d %f\n", i, fitnessAdder);
+
+		//fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+		fitness += SQUARED(dist - pairs[i].KnownDistance);
+	}
+
+	//fitness = 1 / fitness;
+	return FLT_SQRT(fitness);
+}
+
+#define MAX_RADII 32
+
+// note gradientOut will be of the same degree as numRadii
+static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision)
+{
+	FLT baseline = calculateFitness(angles, radii, pairs, numPairs);
+
+	for (size_t i = 0; i < numRadii; i++)
+	{
+		FLT tmpPlus[MAX_RADII];
+		memcpy(tmpPlus, radii, sizeof(*radii) * numRadii);
+		tmpPlus[i] += precision;
+		gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline);
+	}
+
+	return;
+}
+
+static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude)
+{
+	FLT distanceIn = 0;
+
+	for (size_t i = 0; i < count; i++)
+	{
+		distanceIn += SQUARED(vectorToNormalize[i]);
+	}
+	distanceIn = FLT_SQRT(distanceIn);
+
+
+	FLT scale = desiredMagnitude / distanceIn;
+
+	for (size_t i = 0; i < count; i++)
+	{
+		vectorToNormalize[i] *= scale;
+	}
+
+	return;
+}
+
+
+static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+{
+	int i = 0;
+	FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs);
+	if (estimateOut != initialEstimate)
+	{
+		memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii);
+	}
+
+
+	// The values below are somewhat magic, and definitely tunable
+	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+	//   bigger values may be faster, especially when the initial guess is wildly off.
+	//   The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+	//   if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+	//   That's fairly unlikely with the lighthouse problem, from expereince.
+	//   The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+	//   to a size that doesn't jump us out of our minima.
+	// The terminal value of g represents how close we want to get to the local minima before we're "done"
+	// The change in value of g for each iteration is intentionally very close to 1.
+	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
+	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
+	for (FLT g = 0.4; g > 0.00001; g *= 0.9999)
+	{
+		i++;
+
+
+
+		FLT point1[MAX_RADII];
+		memcpy(point1, estimateOut, sizeof(*point1) * numRadii);
+
+		// let's get 3 iterations of gradient descent here.
+		FLT gradient1[MAX_RADII];
+		getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/);
+		normalizeAndMultiplyVector(gradient1, numRadii, g);
+
+		FLT point2[MAX_RADII];
+		for (size_t i = 0; i < numRadii; i++)
+		{
+			point2[i] = point1[i] + gradient1[i];
+		}
+		FLT gradient2[MAX_RADII];
+		getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/);
+		normalizeAndMultiplyVector(gradient2, numRadii, g);
+
+		FLT point3[MAX_RADII];
+		for (size_t i = 0; i < numRadii; i++)
+		{
+			point3[i] = point2[i] + gradient2[i];
+		}
+
+		// remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+		// Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+		// gradient descent is kinda horrible here.  Instead, think about the shape that a zig-zagging 
+		// converging gradient descent makes.  Instead of using the gradient as the best indicator of 
+		// the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+		// following *that* vector.  As it turns out, this works *amazingly* well.  
+
+		FLT specialGradient[MAX_RADII];
+		for (size_t i = 0; i < numRadii; i++)
+		{
+			specialGradient[i] = point3[i] - point1[i];
+		}
+
+		// The second parameter to this function is very much a tunable parameter.  Different values will result
+		// in a different number of iterations before we get to the minimum.  Numbers between 3-10 seem to work well
+		// It's not clear what would be optimum here.
+		normalizeAndMultiplyVector(specialGradient, numRadii, g / 4);
+
+
+		FLT point4[MAX_RADII];
+		for (size_t i = 0; i < numRadii; i++)
+		{
+			point4[i] = point3[i] + specialGradient[i];
+		}
+
+
+		FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs);
+
+
+		if (newMatchFitness < lastMatchFitness)
+		{
+			//if (logFile)
+			//{
+			//	writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+			//}
+
+			lastMatchFitness = newMatchFitness;
+			memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii);
+
+#ifdef RADII_DEBUG
+			printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g);
+#endif
+			g = g * 1.05;
+		}
+		else
+		{
+//#ifdef RADII_DEBUG
+			//			printf("-");
+			//printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]);
+//#endif
+			// if it wasn't a match, back off on the distance we jump
+			g *= 0.7;
+
+		}
+
+#ifdef RADII_DEBUG
+		FLT avg = 0;
+		FLT diffFromAvg[MAX_RADII];
+
+		for (size_t m = 0; m < numRadii; m++)
+		{
+			avg += estimateOut[m];
+		}
+		avg = avg / numRadii;
+
+		for (size_t m = 0; m < numRadii; m++)
+		{
+			diffFromAvg[m] = estimateOut[m] - avg;;
+		}
+		printf("[avg:%f] ", avg);
+
+		for (size_t x = 0; x < numRadii; x++)
+		{
+			printf("%f, ", diffFromAvg[x]);
+			//printf("%f, ", estimateOut[x]);
+		}
+		printf("\n");
+
+
+#endif
+
+
+	}
+	printf(" i=%d ", i);
+}
+
+static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+{
+	FLT estimate[MAX_RADII];
+
+	for (size_t i = 0; i < MAX_RADII; i++)
+	{
+		estimate[i] = 2.38;
+	}
+
+
+	//for (int i=0; i < obj->numSensors; i++)
+	//{
+	//	printf("%d, ", obj->sensor[i].id);
+	//}
+
+	SensorAngles angles[MAX_RADII];
+	PointPair pairs[MAX_POINT_PAIRS];
+
+	size_t pairCount = 0;
+
+	//obj->numSensors = 5; // TODO: HACK!!!!
+
+	for (size_t i = 0; i < obj->numSensors; i++)
+	{
+		angles[i].HorizAngle = obj->sensor[i].theta;
+		angles[i].VertAngle = obj->sensor[i].phi;
+	}
+
+	for (unsigned char i = 0; i < obj->numSensors - 1; i++)
+	{
+		for (unsigned char j = i + 1; j < obj->numSensors; j++)
+		{
+			pairs[pairCount].index1 = i;
+			pairs[pairCount].index2 = j;
+			pairs[pairCount].KnownDistance = distance(obj->sensor[i].point, obj->sensor[j].point);
+			pairCount++;
+		}
+	}
+
+
+	RefineEstimateUsingGradientDescentRadii(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL);
+
+	// we should now have an estimate of the radii.
+
+	//for (int i = 0; i < obj->numSensors; i++)
+	for (int i = 0; i < 1; i++)
+	{
+		printf("radius[%d]: %f\n", i, estimate[i]);
+	}
+
+	// (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+
+	return;
+}
+
+static void QuickPose(SurviveObject *so)
+{
+	OctavioRadiiData * td = so->PoserData;
+
+
+	//for (int i=0; i < so->nr_locations; i++)
+	//{
+	//	FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+	//	FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+	//	//FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+	//	//FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+	//	//printf("%2d: %8.8f, %8.8f   %8.8f, %8.8f   \n", 
+	//	//	i,
+	//	//	x0,
+	//	//	y0,
+	//	//	x1,
+	//	//	y1
+	//	//	);
+	//	printf("%2d: %8.8f, %8.8f   \n", 
+	//		i,
+	//		x0,
+	//		y0
+	//		);
+	//}
+	//printf("\n");
+
+	TrackedObject *to;
+
+	to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+	{
+		int sensorCount = 0;
+
+		for (int i = 0; i < so->nr_locations; i++)
+		{
+			int lh = 0;
+			//printf("%d[%d], ",i,td->hitCount[i][lh][0]);
+
+			int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0))
+				
+				
+			{
+				if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10)
+				{
+					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
+					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
+					to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
+
+
+
+					//printf("%2d: %8.8f, %8.8f   \n", 
+					//	i,
+					//	to->sensor[sensorCount].theta,
+					//	to->sensor[sensorCount].phi
+					//	);
+
+					sensorCount++;
+				}
+			}
+		}
+		//printf("\n");
+		to->numSensors = sensorCount;
+
+		if (sensorCount > 4)
+		{
+			FLT pos[3];
+			FLT orient[4];
+			SolveForLighthouseRadii(pos, orient, to);
+		}
+
+		
+	}
+
+
+	free(to);
+
+}
+
+
+int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
+{
+	PoserType pt = pd->pt;
+	SurviveContext * ctx = so->ctx;
+	OctavioRadiiData * dd = so->PoserData;
+
+	if( !dd )
+	{
+		so->PoserData = dd = malloc( sizeof(OctavioRadiiData) );
+		memset(dd, 0, sizeof(OctavioRadiiData));
+	}
+
+
+	switch( pt )
+	{
+	case POSERDATA_IMU:
+	{
+		PoserDataIMU * imu = (PoserDataIMU*)pd;
+		//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
+		break;
+	}
+	case POSERDATA_LIGHT:
+	{
+		PoserDataLight * l = (PoserDataLight*)pd;
+
+		if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+		{
+			// should never happen.  Famous last words...
+			break;
+		}
+		int axis = l->acode & 0x1;
+
+		//printf("%d ", l->sensor_id);
+
+
+		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+		if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) )
+		{
+			int lastAxis = dd->lastAxis[l->lh];
+	//printf("\n");
+			//if (0 == l->lh)
+			//	printf("or[%d,%d] ", l->lh,lastAxis);
+
+			for (int i=0; i < SENSORS_PER_OBJECT; i++)
+			{
+	//FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+				int index = dd->angleIndex[l->lh][axis];
+				if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0)
+				{
+					//if (0 == l->lh)
+					//	printf("%d ", i);
+
+					dd->hitCount[i][l->lh][axis]++;
+				}
+				else
+				{
+					dd->hitCount[i][l->lh][axis] *= 0.5;
+				}
+			}
+			//if (0 == l->lh)
+			//	printf("\n");
+			//int foo = l->acode & 0x1;
+			//printf("%d", foo);
+
+
+			//if (axis)
+			{
+				if (0 == l->lh && axis) // only once per full cycle...
+				{
+					static unsigned int counter = 1;
+
+					counter++;
+
+					// let's just do this occasionally for now...
+					if (counter % 4 == 0)
+						QuickPose(so);
+				}
+				// axis changed, time to increment the circular buffer index.
+
+
+				dd->angleIndex[l->lh][axis]++;
+				dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+				// and clear out the data.
+				for (int i=0; i < SENSORS_PER_OBJECT; i++)
+				{
+					dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0;
+				}
+
+			}
+			dd->lastAxis[l->lh] = axis;
+		}
+
+		//if (0 == l->lh)
+		//	printf("(%d) ", l->sensor_id);
+
+	//FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+		dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle;
+		break;	}
+	case POSERDATA_FULL_SCENE:
+	{
+		TrackedObject *to;
+
+		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+		to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+		//FLT  lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+		//FLT  angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];  //2 Axes  (Angles in LH space)
+		//FLT  synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+
+		//to->numSensors = so->nr_locations;
+		{
+			int sensorCount = 0;
+
+			for (int i = 0; i < so->nr_locations; i++)
+			{
+				if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0
+				{
+					to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
+					to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
+					to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
+					to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
+					to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
+					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
+					sensorCount++;
+				}
+			}
+
+			to->numSensors = sensorCount;
+
+			Point position;
+			FLT orientation[4];
+
+			SolveForLighthouseRadii(&position, orientation, to);
+		}
+		{
+			int sensorCount = 0;
+			int lh = 1;
+
+			for (int i = 0; i < so->nr_locations; i++)
+			{
+				if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1)
+				{
+					to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
+					to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
+					to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
+					to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
+					to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
+					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
+					sensorCount++;
+				}
+			}
+
+			to->numSensors = sensorCount;
+
+			Point position;
+			FLT orientation[4];
+
+			SolveForLighthouseRadii(&position, orientation, to);
+		}
+		//printf( "Full scene data.\n" );
+		break;
+	}
+	case POSERDATA_DISASSOCIATE:
+	{
+		free( dd );
+		so->PoserData = 0;
+		//printf( "Need to disassociate.\n" );
+		break;
+	}
+	}
+	return 0;
+}
+
+
+REGISTER_LINKTIME( PoserOctavioRadii );
+
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
new file mode 100644
index 0000000..7abf5d0
--- /dev/null
+++ b/src/poser_turveytori.c
@@ -0,0 +1,1642 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <memory.h>
+#include <assert.h>
+#include "linmath.h"
+#include <stddef.h>
+#include <math.h>
+#include <stdint.h>
+#if defined(__FreeBSD__) || defined(__APPLE__)
+#include <stdlib.h>
+#else
+#include <malloc.h> //for alloca
+#endif
+
+
+#define PointToFlts(x) ((FLT*)(x))
+
+typedef struct
+{
+	FLT x;
+	FLT y;
+	FLT z;
+} Point;
+
+void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {}
+void updateHeader(FILE * file) {}
+void writeAxes(FILE * file) {}
+void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {}
+void writePcdHeader(FILE * file) {}
+void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {}
+void markPointWithStar(FILE *file, Point point, unsigned int color) {}
+
+typedef struct
+{
+	Point point; // location of the sensor on the tracked object;
+	Point normal; // unit vector indicating the normal for the sensor
+	double theta; // "horizontal" angular measurement from lighthouse radians
+	double phi; // "vertical" angular measurement from lighthouse in radians.
+} TrackedSensor;
+
+typedef struct
+{
+	size_t numSensors;
+	TrackedSensor sensor[0];
+} TrackedObject;
+
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846264338327
+#endif
+
+#define SQUARED(x) ((x)*(x))
+
+typedef union
+{
+	struct
+	{
+		unsigned char Blue;
+		unsigned char Green;
+		unsigned char Red;
+		unsigned char Alpha;
+	};
+	uint32_t long_value;
+} RGBValue;
+
+static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 };
+static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 };
+static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 };
+
+static const double WORLD_BOUNDS = 100;
+#define MAX_TRACKED_POINTS 40
+
+static const float DefaultPointsPerOuterDiameter = 60;
+
+typedef struct
+{
+	FLT down[3];  // populated by the IMU for posing
+	//Stuff
+
+#define OLD_ANGLES_BUFF_LEN 3
+	FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+	int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+	int lastAxis[NUM_LIGHTHOUSES];
+
+	Point lastLhPos[NUM_LIGHTHOUSES];
+	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+} ToriData;
+
+
+
+
+
+
+
+static FLT distance(Point a, Point b)
+{
+	FLT x = a.x - b.x;
+	FLT y = a.y - b.y;
+	FLT z = a.z - b.z;
+	return FLT_SQRT(x*x + y*y + z*z);
+}
+
+Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
+{
+	Matrix3x3 result;
+	FLT v1[3] = { 0, 0, 1 };
+	FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
+
+	normalize3d(v2, v2);
+
+	rotation_between_vecs_to_m3(&result, v1, v2);
+
+	// Useful for debugging...
+	//FLT v2b[3];
+	//rotate_vec(v2b, v1, result);
+
+	return result;
+}
+
+typedef struct
+{
+	Point a;
+	Point b;
+	FLT angle;
+	FLT tanAngle; // tangent of angle
+	Matrix3x3 rotation;
+	Matrix3x3 invRotation; // inverse of rotation
+	char ai;
+	char bi;
+} PointsAndAngle;
+
+
+Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
+{
+	Point q;
+
+	double pf[3] = { p.x, p.y, p.z };
+	q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x;
+	q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y;
+	q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z;
+
+	return q;
+}
+
+double angleFromPoints(Point p1, Point p2, Point center)
+{
+	Point v1, v2, v1norm, v2norm;
+	v1.x = p1.x - center.x;
+	v1.y = p1.y - center.y;
+	v1.z = p1.z - center.z;
+
+	v2.x = p2.x - center.x;
+	v2.y = p2.y - center.y;
+	v2.z = p2.z - center.z;
+
+	double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
+	v1norm.x = v1.x / v1mag;
+	v1norm.y = v1.y / v1mag;
+	v1norm.z = v1.z / v1mag;
+
+	double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z);
+	v2norm.x = v2.x / v2mag;
+	v2norm.y = v2.y / v2mag;
+	v2norm.z = v2.z / v2mag;
+
+	double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z;
+
+	double angle = acos(res);
+
+	return angle;
+}
+
+Point midpoint(Point a, Point b)
+{
+	Point m;
+	m.x = (a.x + b.x) / 2;
+	m.y = (a.y + b.y) / 2;
+	m.z = (a.z + b.z) / 2;
+
+	return m;
+}
+
+// What we're doing here is:
+// * Given a point in space
+// * And points and a lighthouse angle that implicitly define a torus
+// * for that torus, what is the toroidal angle of the plane that will go through that point in space
+// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space?
+void estimateToroidalAndPoloidalAngleOfPoint(
+	PointsAndAngle *pna,
+	Point point,
+	double *toroidalSin,
+	double *toroidalCos,
+	double *poloidalAngle,
+	double *poloidalSin)
+{
+	// We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from
+	// the tracked object coordinate system into the "easy" or "default" coordinate system of the torus.
+	// Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system.
+	Matrix3x3 rot = pna->invRotation;
+	Point origin;
+	origin.x = 0;
+	origin.y = 0;
+	origin.z = 0;
+
+	Point m = midpoint(pna->a, pna->b);
+
+	// in this new coordinate system, we'll rename all of the points we care about to have an "F" after them
+	// This will be their representation in the "friendly" coordinate system
+	Point pointF;
+
+	// Okay, I lied a little above.  In addition to the rotation matrix that we care about, there was also
+	// a translation that we did to move the origin.  If we're going to get to the "friendly" coordinate system
+	// of the torus, we need to first undo the translation, then undo the rotation.  Below, we're undoing the translation.
+	pointF.x = point.x - m.x;
+	pointF.y = point.y - m.y;
+	pointF.z = point.z - m.z;
+
+	// now we'll undo the rotation part.
+	pointF = RotateAndTranslatePoint(pointF, rot, origin);
+
+	// hooray, now pointF is in our more-friendly coordinate system.  
+
+	// Now, it's time to figure out the toroidal angle to that point.  This should be pretty easy. 
+	// We will "flatten" the z dimension to only look at the x and y values.  Then, we just need to measure the 
+	// angle between a vector to pointF and a vector along the x axis.  
+
+	FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x));
+
+	*toroidalSin = pointF.y / toroidalHyp;
+
+	*toroidalCos = pointF.x / toroidalHyp;
+
+	//*toroidalAngle = atan(pointF.y / pointF.x);
+	//if (pointF.x < 0)
+	//{
+	//	*toroidalAngle += M_PI;
+	//}
+
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001);
+
+	// SCORE!! We've got the toroidal angle.  We're half done!
+
+	// Okay, what next...?  Now, we will need to rotate the torus *again* to make it easy to
+	// figure out the poloidal angle.  We should rotate the entire torus by the toroidal angle
+	// so that the point we're focusin on will lie on the x/z plane.  We then should translate the
+	// torus so that the center of the poloidal circle is at the origin.  At that point, it will
+	// be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a 
+	// vector from the origin to the point.
+
+	// okay, instead of rotating the torus & point by the toroidal angle to get the point on
+	// the xz plane, we're going to take advantage of the radial symmetry of the torus
+	// (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't 
+	// change the torus at all).  Therefore, we'll leave the torus as is, but we'll rotate the point
+	// This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent
+	// this new coordinate system
+
+	Point pointG;
+	pointG.z = pointF.z;
+	pointG.y = 0;
+	pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y));
+
+	// okay, that ended up being easier than I expected.  Now that we have the point on the xZ plane,
+	// our next step will be to shift it down so that the center of the poloidal circle is at the origin.
+	// As you may have noticed, y has now gone to zero, and from here on out, we can basically treat
+	// this as a 2D problem.  I think we're getting close...
+
+	// I stole these lines from the torus generator.  Gonna need the poloidal radius.
+	double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance.
+	double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
+
+	// The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. 
+	// The shift along the X axis will be the toroidal radius.  
+
+	Point pointH;
+	pointH.z = pointG.z;
+	pointH.y = pointG.y;
+	pointH.x = pointG.x - toroidalRadius;
+
+	// Okay, almost there.  If we treat pointH as a vector on the XZ plane, if we get its angle,
+	// that will be the poloidal angle we're looking for.  (crosses fingers)
+
+	FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x));
+
+	*poloidalSin = pointH.z / poloidalHyp;
+
+
+	*poloidalAngle = atan(pointH.z / pointH.x);
+	if (pointH.x < 0)
+	{
+		*poloidalAngle += M_PI;
+	}
+
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+
+
+	// Wow, that ended up being not so much code, but a lot of interesting trig.
+	// can't remember the last time I spent so much time working through each line of code.
+
+	return;
+}
+
+#define MAX_POINT_PAIRS 100
+
+FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
+{
+	FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta));
+	//FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
+
+	return angle;
+}
+
+// This provides a pretty good estimate of the angle above, probably better
+// the further away the lighthouse is.  But, it's not crazy-precise.
+// It's main advantage is speed.
+FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
+{
+	FLT p = (a->phi - b->phi);
+	FLT d = (a->theta - b->theta);
+
+	FLT adjd = FLT_SIN((a->phi + b->phi) / 2);
+	FLT adjP = FLT_SIN((a->theta + b->theta) / 2);
+	FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
+	return pythAngle;
+}
+
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin)
+{
+	Point result;
+
+	FLT distanceBetweenPoints = distance(pna->a, pna->b);
+	Point m = midpoint(pna->a, pna->b);
+	Matrix3x3 rot = pna->rotation;
+
+	FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+	FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
+
+	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
+	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
+	result.z = poloidalRadius*poloidalSin;
+	result = RotateAndTranslatePoint(result, rot, m);
+
+	return result;
+}
+
+FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
+{
+
+	double toroidalSin = 0;
+	double toroidalCos = 0;
+	double poloidalAngle = 0;
+	double poloidalSin = 0;
+
+	estimateToroidalAndPoloidalAngleOfPoint(
+		pna,
+		pointIn,
+		&toroidalSin,
+		&toroidalCos,
+		&poloidalAngle,
+		&poloidalSin);
+
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
+
+	FLT dist = distance(pointIn, torusPoint);
+
+	// This is some voodoo black magic.  This is here to solve the problem that the origin 
+	// (which is near the center of all the tori) erroniously will rank as a good match.
+	// through a lot of empiracle testing on how to compensate for this, the "fudge factor"
+	// below ended up being the best fit.  As simple as it is, I have a strong suspicion
+	// that there's some crazy complex thesis-level math that could be used to derive this
+	// but it works so we'll run with it.
+	// Note that this may be resulting in a skewing of the found location by several millimeters.
+	// it is not clear if this is actually removing existing skew (to get a more accurate value)
+	// or if it is introducing an undesirable skew.
+	double fudge = FLT_SIN((poloidalAngle - M_PI) / 2);
+	dist = dist / fudge;
+
+	return dist;
+}
+
+FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint)
+{
+	FLT fitness;
+
+	FLT resultSum = 0;
+	FLT *fitnesses = alloca(sizeof(FLT) * pnaCount);
+	int i=0, j=0;
+
+	FLT worstFitness = 0;
+
+	for (size_t i = 0; i < pnaCount; i++)
+	{
+		fitness = getPointFitnessForPna(pointIn, &(pna[i]));
+
+		if (worstFitness < fitness)
+		{
+			i = pna[i].ai;
+			j = pna[i].bi;
+			worstFitness = fitness;
+		}
+
+		fitnesses[i] = fitness;
+		if (deubgPrint)
+		{
+			printf("  [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
+		}
+	}
+
+	for (size_t i = 0; i < pnaCount; i++)
+	{
+		// TODO:  This is an UGLY HACK!!!  It is NOT ROBUST and MUST BE BETTER
+		// Right now, we're just throwing away the single worst fitness value
+		// this works frequently, but we REALLY need to do a better job of determing
+		// exactly when we should throw away a bad value.  I'm thinking that decision 
+		// alone could be a master's thesis, so lots of work to be done.
+		// This is just a stupid general approach that helps in a lot of cases,
+		// but is NOT suitable for long term use.
+		//if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j)
+		if (fitnesses[i] != worstFitness)
+			resultSum += SQUARED(fitnesses[i]);
+	}
+	return 1 / FLT_SQRT(resultSum);
+}
+
+// TODO: Use a central point instead of separate "minus" points for each axis.  This will reduce
+// the number of fitness calls by 1/3.
+Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision)
+{
+	Point result;
+
+	FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0);
+
+	Point tmpXplus = pointIn;
+	Point tmpXminus = pointIn;
+	tmpXplus.x = pointIn.x + precision;
+	tmpXminus.x = pointIn.x - precision;
+	result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0);
+
+	Point tmpYplus = pointIn;
+	Point tmpYminus = pointIn;
+	tmpYplus.y = pointIn.y + precision;
+	tmpYminus.y = pointIn.y - precision;
+	result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0);
+
+	Point tmpZplus = pointIn;
+	Point tmpZminus = pointIn;
+	tmpZplus.z = pointIn.z + precision;
+	tmpZminus.z = pointIn.z - precision;
+	result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0);
+
+	return result;
+}
+
+Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude)
+{
+	FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z));
+
+	FLT scale = desiredMagnitude / distanceIn;
+
+	Point result = vectorIn;
+
+	result.x *= scale;
+	result.y *= scale;
+	result.z *= scale;
+
+	return result;
+}
+
+Point getAvgPoints(Point a, Point b)
+{
+	Point result;
+	result.x = (a.x + b.x) / 2;
+	result.y = (a.y + b.y) / 2;
+	result.z = (a.z + b.z) / 2;
+	return result;
+}
+
+
+// This is modifies the basic gradient descent algorithm to better handle the shallow valley case,
+// which appears to be typical of this convergence.  
+static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+	int i = 0;
+	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0);
+	Point lastPoint = initialEstimate;
+
+	// The values below are somewhat magic, and definitely tunable
+	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+	//   bigger values may be faster, especially when the initial guess is wildly off.
+	//   The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+	//   if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+	//   That's fairly unlikely with the lighthouse problem, from expereince.
+	//   The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+	//   to a size that doesn't jump us out of our minima.
+	// The terminal value of g represents how close we want to get to the local minima before we're "done"
+	// The change in value of g for each iteration is intentionally very close to 1.
+	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
+	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
+	for (FLT g = 0.2; g > 0.00001; g *= 0.99)
+	{
+		i++;
+		Point point1 = lastPoint;
+		// let's get 3 iterations of gradient descent here.
+		Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+		Point gradientN1 = getNormalizedAndScaledVector(gradient1, g);
+
+		Point point2;
+		point2.x = point1.x + gradientN1.x;
+		point2.y = point1.y + gradientN1.y;
+		point2.z = point1.z + gradientN1.z;
+
+		Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+		Point gradientN2 = getNormalizedAndScaledVector(gradient2, g);
+
+		Point point3;
+		point3.x = point2.x + gradientN2.x;
+		point3.y = point2.y + gradientN2.y;
+		point3.z = point2.z + gradientN2.z;
+
+		// remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+		// Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+		// gradient descent is kinda horrible here.  Instead, think about the shape that a zig-zagging 
+		// converging gradient descent makes.  Instead of using the gradient as the best indicator of 
+		// the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+		// following *that* vector.  As it turns out, this works *amazingly* well.  
+
+		Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y };
+
+		// The second parameter to this function is very much a tunable parameter.  Different values will result
+		// in a different number of iterations before we get to the minimum.  Numbers between 3-10 seem to work well
+		// It's not clear what would be optimum here.
+		specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4);
+
+		Point point4;
+
+		point4.x = point3.x + specialGradient.x;
+		point4.y = point3.y + specialGradient.y;
+		point4.z = point3.z + specialGradient.z;
+
+		FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+			if (logFile)
+			{
+				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+			}
+
+			lastMatchFitness = newMatchFitness;
+			lastPoint = point4;
+#ifdef TORI_DEBUG
+			printf("+");
+#endif
+		}
+		else
+		{
+#ifdef TORI_DEBUG
+			printf("-");
+#endif
+			g *= 0.7;
+
+		}
+
+		// from empiracle evidence, we're probably "good enough" at this point.
+		// So, even though we could still improve, we're likely to be improving
+		// very slowly, and we should just take what we've got and move on.
+		// This also seems to happen almost only when data is a little more "dirty"
+		// because the tracker is being rotated.  
+		if (i > 120)
+		{
+			//printf("i got big");
+			break;
+		}
+	}
+	printf(" i=%3d ", i);
+
+	return lastPoint;
+}
+
+
+// interesting-- this is one place where we could use any sensors that are only hit by
+// just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
+FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < obj->numSensors; i++)
+	{
+		// first, get the normal of the plane for the horizonal sweep
+		FLT theta = obj->sensor[i].theta;
+		// make two vectors that lie on the plane
+		FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+		FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+		FLT tNormH[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormH, t1H, t2H);
+
+		normalize3d(tNormH, tNormH);
+
+		// Now do the same for the vertical sweep
+
+		// first, get the normal of the plane for the horizonal sweep
+		FLT phi = obj->sensor[i].phi;
+		// make two vectors that lie on the plane
+		FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+		FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+		FLT tNormV[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormV, t1V, t2V);
+
+		normalize3d(tNormV, tNormV);
+
+
+		// First, where is the sensor in the object's reference frame?
+		FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+		// Where is the point, in the reference frame of the lighthouse?
+		// This has two steps, first we translate from the object's location being the
+		// origin to the lighthouse being the origin.
+		// And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+		FLT sensor_in_lh_reference_frame[3];
+		sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+		quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+
+		// now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+		// We need an arbitrary vector from the plane to the point.  
+		// Since the plane goes through the origin, this is trivial.
+		// The sensor point itself is such a vector!
+
+		// And go calculate the distances!
+		// TODO: don't need to ABS these because we square them below.
+		FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+		FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+		
+		fitness += SQUARED(dH);
+		fitness += SQUARED(dV);
+	}
+
+	fitness = FLT_SQRT(fitness);
+
+	return fitness;
+}
+
+FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj)
+{
+	// For this fitness calculator, we're going to use the rotation information to figure out where
+	// we expect to see the tracked object sensors, and we'll do a sum of squares to grade
+	// the quality of the guess formed by the AxisAngle;
+
+	FLT fitness = 0;
+
+	// for each point in the tracked object
+	for (int i=0; i< obj->numSensors; i++)
+	{
+
+
+		
+		// let's see... we need to figure out where this sensor should be in the LH reference frame.
+		FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z};
+
+		// And this puppy needs to be rotated...
+
+		rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]);
+
+		// Now, the vector indicating the position of the sensor, as seen by the lighthouse is:
+		FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)};
+
+		// and the vector we're calculating given the rotation passed in is the same as the sensor location:
+		FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]};
+
+		FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh );
+
+		fitness += SQUARED(angleBetween);
+	}
+
+	return 1/FLT_SQRT(fitness);
+}
+
+// This figures out how far away from the scanned planes each point is, then does a sum of squares
+// for the fitness.
+// 
+// interesting-- this is one place where we could use any sensors that are only hit by
+// just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
+FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < obj->numSensors; i++)
+	{
+		// first, get the normal of the plane for the horizonal sweep
+		FLT theta = obj->sensor[i].theta;
+		// make two vectors that lie on the plane
+		FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+		FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+		FLT tNormH[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormH, t1H, t2H);
+
+		normalize3d(tNormH, tNormH);
+
+		// Now do the same for the vertical sweep
+
+		// first, get the normal of the plane for the horizonal sweep
+		FLT phi = obj->sensor[i].phi;
+		// make two vectors that lie on the plane
+		FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+		FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+		FLT tNormV[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormV, t1V, t2V);
+
+		normalize3d(tNormV, tNormV);
+
+
+		// First, where is the sensor in the object's reference frame?
+		FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+		// Where is the point, in the reference frame of the lighthouse?
+		// This has two steps, first we translate from the object's location being the
+		// origin to the lighthouse being the origin.
+		// And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+		FLT sensor_in_lh_reference_frame[3];
+		sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+		//quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+		rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]);
+
+		// now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+		// We need an arbitrary vector from the plane to the point.  
+		// Since the plane goes through the origin, this is trivial.
+		// The sensor point itself is such a vector!
+
+		// And go calculate the distances!
+		// TODO: don't need to ABS these because we square them below.
+		FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+		FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+		
+		fitness += SQUARED(dH);
+		fitness += SQUARED(dV);
+	}
+
+	fitness = FLT_SQRT(fitness);
+
+	return 1/fitness;
+}
+
+// interesting-- this is one place where we could use any sensors that are only hit by
+// just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
+FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+	FLT fitness = 0;
+	for (size_t i = 0; i < obj->numSensors; i++)
+	{
+		// first, get the normal of the plane for the horizonal sweep
+		FLT theta = obj->sensor[i].theta;
+		// make two vectors that lie on the plane
+		FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+		FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+		FLT tNormH[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormH, t1H, t2H);
+
+		normalize3d(tNormH, tNormH);
+
+		// Now do the same for the vertical sweep
+
+		// first, get the normal of the plane for the horizonal sweep
+		FLT phi = obj->sensor[i].phi;
+		// make two vectors that lie on the plane
+		FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+		FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+		FLT tNormV[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNormV, t1V, t2V);
+
+		normalize3d(tNormV, tNormV);
+
+
+		// First, where is the sensor in the object's reference frame?
+		FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+		// Where is the point, in the reference frame of the lighthouse?
+		// This has two steps, first we translate from the object's location being the
+		// origin to the lighthouse being the origin.
+		// And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+		FLT sensor_in_lh_reference_frame[3];
+		sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+		quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+		//rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]);
+
+		// now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+		// We need an arbitrary vector from the plane to the point.  
+		// Since the plane goes through the origin, this is trivial.
+		// The sensor point itself is such a vector!
+
+		// And go calculate the distances!
+		// TODO: don't need to ABS these because we square them below.
+		FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+		FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+		
+		fitness += SQUARED(dH);
+		fitness += SQUARED(dV);
+	}
+
+	fitness = FLT_SQRT(fitness);
+
+	return 1/fitness;
+}
+
+
+void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision)
+{
+
+	FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj);
+
+	FLT tmp0plus[4];
+	quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0});
+	gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness;
+
+	FLT tmp1plus[4];
+	quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0});
+	gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness;
+
+	FLT tmp2plus[4];
+	quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0});
+	gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness;
+
+	FLT tmp3plus[4];
+	quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision});
+	gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness;
+
+	return;
+}
+
+void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision)
+{
+
+	FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj);
+
+	FLT tmp0plus[4];
+	quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0});
+	gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness;
+
+	FLT tmp1plus[4];
+	quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0});
+	gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness;
+
+	FLT tmp2plus[4];
+	quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0});
+	gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness;
+
+	FLT tmp3plus[4];
+	quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision});
+	gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness;
+
+	return;
+}
+
+//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude)
+//{
+//	quatnormalize(vectorToScale, vectorToScale);
+//	quatscale(vectorToScale, vectorToScale, desiredMagnitude);
+//	return;
+//}
+void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude)
+{
+	quatnormalize(vectorToScale, vectorToScale);
+	quatscale(vectorToScale, vectorToScale, desiredMagnitude);
+	//vectorToScale[3] = desiredMagnitude;
+
+	return;
+}
+
+static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
+{
+	posOut[0] = -lhPoint.x;
+	posOut[1] = -lhPoint.y;
+	posOut[2] = -lhPoint.z;
+	
+	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
+
+	printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+}
+
+static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
+{
+	int i = 0;
+	FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj);
+
+	quatcopy(rotOut, initialEstimate);
+
+	// The values below are somewhat magic, and definitely tunable
+	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+	//   bigger values may be faster, especially when the initial guess is wildly off.
+	//   The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+	//   if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+	//   That's fairly unlikely with the lighthouse problem, from expereince.
+	//   The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+	//   to a size that doesn't jump us out of our minima.
+	// The terminal value of g represents how close we want to get to the local minima before we're "done"
+	// The change in value of g for each iteration is intentionally very close to 1.
+	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
+	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
+	for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99)
+	{
+		i++;
+		FLT point1[4];
+		quatcopy(point1, rotOut);
+		// let's get 3 iterations of gradient descent here.
+		FLT gradient1[4];
+		
+		normalize3d(point1, point1);
+
+		getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000);
+		getNormalizedAndScaledRotationGradient(gradient1,g);
+
+		FLT point2[4];
+		quatadd(point2, gradient1, point1);
+		//quatnormalize(point2,point2);
+
+		normalize3d(point1, point1);
+
+		FLT gradient2[4];
+		getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000);
+		getNormalizedAndScaledRotationGradient(gradient2,g);
+
+		FLT point3[4];
+		quatadd(point3, gradient2, point2);
+
+		normalize3d(point1, point1);
+
+		//quatnormalize(point3,point3);
+
+		// remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+		// Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+		// gradient descent is kinda horrible here.  Instead, think about the shape that a zig-zagging 
+		// converging gradient descent makes.  Instead of using the gradient as the best indicator of 
+		// the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+		// following *that* vector.  As it turns out, this works *amazingly* well.  
+
+		FLT specialGradient[4];
+		quatsub(specialGradient,point3,point1);
+
+		// The second parameter to this function is very much a tunable parameter.  Different values will result
+		// in a different number of iterations before we get to the minimum.  Numbers between 3-10 seem to work well
+		// It's not clear what would be optimum here.
+		getNormalizedAndScaledRotationGradient(specialGradient,g/4);
+
+		FLT point4[4];
+		quatadd(point4, specialGradient, point3);
+		//quatnormalize(point4,point4);
+		normalize3d(point1, point1);
+
+		FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+
+			lastMatchFitness = newMatchFitness;
+			quatcopy(rotOut, point4);
+//#ifdef TORI_DEBUG
+			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
+//#endif
+			g *= 1.02;
+
+		}
+		else
+		{
+//#ifdef TORI_DEBUG
+			//printf("-         , %f\n", point4[3]);
+//#endif
+			g *= 0.7;
+
+		}
+
+		if (i > 998)
+		{
+			//printf("Ri got big");
+			break;
+		}
+	}
+	printf(" Ri=%d ", i);
+}
+static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+{
+	FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]};
+	FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z};
+	
+	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
+	quatrotatevector(objPoint, rotation, objPoint);
+	printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+}
+
+
+
+static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
+{
+	int i = 0;
+	FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj);
+
+	quatcopy(rotOut, initialEstimate);
+
+	// The values below are somewhat magic, and definitely tunable
+	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+	//   bigger values may be faster, especially when the initial guess is wildly off.
+	//   The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+	//   if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+	//   That's fairly unlikely with the lighthouse problem, from expereince.
+	//   The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+	//   to a size that doesn't jump us out of our minima.
+	// The terminal value of g represents how close we want to get to the local minima before we're "done"
+	// The change in value of g for each iteration is intentionally very close to 1.
+	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
+	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
+	for (FLT g = 0.1; g > 0.000000001; g *= 0.99)
+	{
+		i++;
+		FLT point1[4];
+		quatcopy(point1, rotOut);
+		// let's get 3 iterations of gradient descent here.
+		FLT gradient1[4];
+		
+		//normalize3d(point1, point1);
+
+		getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000);
+		getNormalizedAndScaledRotationGradient(gradient1,g);
+
+		FLT point2[4];
+		quatadd(point2, gradient1, point1);
+		quatnormalize(point2,point2);
+
+		//normalize3d(point1, point1);
+
+		FLT gradient2[4];
+		getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000);
+		getNormalizedAndScaledRotationGradient(gradient2,g);
+
+		FLT point3[4];
+		quatadd(point3, gradient2, point2);
+
+		//normalize3d(point1, point1);
+
+		quatnormalize(point3,point3);
+
+		// remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+		// Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+		// gradient descent is kinda horrible here.  Instead, think about the shape that a zig-zagging 
+		// converging gradient descent makes.  Instead of using the gradient as the best indicator of 
+		// the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+		// following *that* vector.  As it turns out, this works *amazingly* well.  
+
+		FLT specialGradient[4];
+		quatsub(specialGradient,point3,point1);
+
+		// The second parameter to this function is very much a tunable parameter.  Different values will result
+		// in a different number of iterations before we get to the minimum.  Numbers between 3-10 seem to work well
+		// It's not clear what would be optimum here.
+		getNormalizedAndScaledRotationGradient(specialGradient,g/4);
+
+		FLT point4[4];
+		quatadd(point4, specialGradient, point3);
+		quatnormalize(point4,point4);
+		//normalize3d(point1, point1);
+
+		FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+
+			lastMatchFitness = newMatchFitness;
+			quatcopy(rotOut, point4);
+//#ifdef TORI_DEBUG
+			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
+//#endif
+			g *= 1.02;
+			printf("+");
+			WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
+		}
+		else
+		{
+//#ifdef TORI_DEBUG
+			//printf("-         , %f\n", point4[3]);
+//#endif
+			g *= 0.7;
+			printf("-");
+		}
+
+
+	}
+	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+}
+
+
+void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
+{
+
+	// Step 1, create initial quaternion for guess.  
+	// This should have the lighthouse directly facing the tracked object.
+	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	FLT theta = atan2(-lh.x, -lh.y);
+	FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2};
+	FLT quat1[4];
+	quatfromaxisangle(quat1, zAxis, theta);
+
+	//quatfrom2vectors(0,0)
+	// not correcting for phi, but that's less important.
+
+
+	// Step 2, optimize the axis/ angle to match the data.
+	RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
+
+
+	//// Step 2, optimize the quaternion to match the data.
+	//RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
+
+	//WhereIsTheTrackedObjectQuaternion(rotOut, lh);
+
+}
+
+
+static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration)
+{
+	ToriData *toriData = so->PoserData;
+
+	//printf("Solving for Lighthouse\n");
+
+	//printf("obj->numSensors = %d;\n", obj->numSensors);
+
+	//for (int i=0; i < obj->numSensors; i++)
+	//{
+	//	printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x);
+	//	printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y);
+	//	printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z);
+	//	printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x);
+	//	printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y);
+	//	printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z);
+	//	printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi);
+	//	printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta);
+	//}
+	PointsAndAngle pna[MAX_POINT_PAIRS];
+
+	volatile size_t sizeNeeded = sizeof(pna);
+
+	Point avgNorm = { 0 };
+
+	FLT smallestAngle = 20.0;
+	FLT largestAngle = 0;
+
+	size_t pnaCount = 0;
+	for (unsigned int i = 0; i < obj->numSensors; i++)
+	{
+		for (unsigned int j = 0; j < i; j++)
+		{
+			if (pnaCount < MAX_POINT_PAIRS)
+			{
+				pna[pnaCount].a = obj->sensor[i].point;
+				pna[pnaCount].b = obj->sensor[j].point;
+
+				pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]);
+				//pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+				pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle);
+
+				if (pna[pnaCount].angle < smallestAngle)
+				{
+					smallestAngle = pna[pnaCount].angle;
+				}
+
+				if (pna[pnaCount].angle > largestAngle)
+				{
+					largestAngle = pna[pnaCount].angle;
+				}
+
+				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
+
+				pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b);
+				pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation);
+				pna[pnaCount].ai = i;
+				pna[pnaCount].bi = j;
+
+
+
+				pnaCount++;
+			}
+		}
+
+		avgNorm.x += obj->sensor[i].normal.x;
+		avgNorm.y += obj->sensor[i].normal.y;
+		avgNorm.z += obj->sensor[i].normal.z;
+	}
+	avgNorm.x = avgNorm.x / obj->numSensors;
+	avgNorm.y = avgNorm.y / obj->numSensors;
+	avgNorm.z = avgNorm.z / obj->numSensors;
+
+	FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z };
+
+
+	FILE *logFile = NULL;
+	if (doLogOutput)
+	{
+		logFile = fopen("pointcloud2.pcd", "wb");
+		writePcdHeader(logFile);
+		writeAxes(logFile);
+	}
+
+
+	// Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
+
+
+	// arbitrarily picking a value of 8 meters out to start from.
+	// intentionally picking the direction of the average normal vector of the sensors that see the lighthouse
+	// since this is least likely to pick the incorrect "mirror" point that would send us 
+	// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
+	Point p1 = getNormalizedAndScaledVector(avgNorm, 8);
+
+	// if the last lighthouse position has been populated (extremely rare it would be 0)
+	if (toriData->lastLhPos[lh].x != 0)
+	{
+		p1.x = toriData->lastLhPos[lh].x;
+		p1.y = toriData->lastLhPos[lh].y;
+		p1.z = toriData->lastLhPos[lh].z;
+	}
+
+	Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
+
+	FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
+
+	FLT a1 = anglebetween3d(pf1, avgNormF);
+
+	if (a1 > M_PI / 2)
+	{
+		Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z };
+		refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
+
+		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
+
+		//FLT a2 = anglebetween3d(pf2, avgNormF);
+
+	}
+
+	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
+
+	FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
+	printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
+	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
+
+	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+
+	if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+	{
+		rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+		rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+		rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+	}
+
+
+	SolveForRotation(rot, obj, refinedEstimateGd);
+	FLT objPos[3];
+
+	{
+		toriData->lastLhRotAxisAngle[lh][0] = rot[0];
+		toriData->lastLhRotAxisAngle[lh][1] = rot[1];
+		toriData->lastLhRotAxisAngle[lh][2] = rot[2];
+		toriData->lastLhRotAxisAngle[lh][3] = rot[3];
+	}
+
+	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
+
+
+	FLT rotQuat[4];
+
+	quatfromaxisangle(rotQuat, rot, rot[3]);
+
+	//{
+		FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
+
+		quatrotatevector(tmpPos, rotQuat, tmpPos);
+	//}
+
+	//static int foo = 0;
+
+	//if (0 == foo)
+	if (setLhCalibration)
+	{ 
+		//foo = 1;
+		if (so->ctx->bsd[lh].PositionSet)
+		{
+			printf("Warning: resetting base station calibration data");
+		}
+
+		FLT invRot[4];
+		quatgetreciprocal(invRot, rotQuat);
+
+		so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x;
+		so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y;
+		so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z;
+		so->ctx->bsd[lh].Pose.Rot[0] = invRot[0];
+		so->ctx->bsd[lh].Pose.Rot[1] = invRot[1];
+		so->ctx->bsd[lh].Pose.Rot[2] = invRot[2];
+		so->ctx->bsd[lh].Pose.Rot[3] = invRot[3];
+		so->ctx->bsd[lh].PositionSet = 1;
+	}
+
+	FLT wcPos[3]; // position in wold coordinates
+
+	quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos);
+
+	FLT newOrientation[4];
+	quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot );
+
+	wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0];
+	wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1];
+	wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2];
+
+	so->OutPose.Pos[0] = wcPos[0];
+	so->OutPose.Pos[1] = wcPos[1];
+	so->OutPose.Pos[2] = wcPos[2];
+
+	so->OutPose.Rot[0] = newOrientation[0];
+	so->OutPose.Rot[1] = newOrientation[1];
+	so->OutPose.Rot[2] = newOrientation[2];
+	so->OutPose.Rot[3] = newOrientation[3];
+
+	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+
+	if (logFile)
+	{
+		updateHeader(logFile);
+		fclose(logFile);
+	}
+
+
+	toriData->lastLhPos[lh].x = refinedEstimateGd.x;
+	toriData->lastLhPos[lh].y = refinedEstimateGd.y;
+	toriData->lastLhPos[lh].z = refinedEstimateGd.z;
+
+	return refinedEstimateGd;
+}
+
+
+
+
+
+
+
+
+static void QuickPose(SurviveObject *so, int lh)
+{
+
+
+	ToriData * td = so->PoserData;
+
+	if (! so->ctx->bsd[lh].PositionSet)
+	{
+		// we don't know where we are!  Augh!!!
+		return;
+	}
+
+	//for (int i=0; i < so->nr_locations; i++)
+	//{
+	//	FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+	//	FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+	//	FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+	//	FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+	//	//printf("%2d: %8.8f, %8.8f   %8.8f, %8.8f   \n", 
+	//	//	i,
+	//	//	x0,
+	//	//	y0,
+	//	//	x1,
+	//	//	y1
+	//	//	);
+	//	printf("%2d: %8.8f, %8.8f   \n", 
+	//		i,
+	//		x0,
+	//		y0
+	//		);
+	//}
+	//printf("\n");
+
+	TrackedObject *to;
+
+	to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+	{
+		int sensorCount = 0;
+
+		//// TODO: remove, for debug purposes only!
+		//FLT downQuat[4];
+		//FLT negZ[3] = { 0,0,-1 };
+		////quatfrom2vectors(downQuat, negZ, td->down);
+		//quatfrom2vectors(downQuat, td->down, negZ);
+		//// end TODO
+
+
+		for (int i = 0; i < so->nr_locations; i++)
+		{
+			int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) 
+			{
+				FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
+				FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
+
+				// TODO: remove these two lines!!!
+				//quatrotatevector(norm, downQuat, norm);
+				//quatrotatevector(point, downQuat, point);
+
+				to->sensor[sensorCount].normal.x = norm[0];
+				to->sensor[sensorCount].normal.y = norm[1];
+				to->sensor[sensorCount].normal.z = norm[2];
+				to->sensor[sensorCount].point.x = point[0];
+				to->sensor[sensorCount].point.y = point[1];
+				to->sensor[sensorCount].point.z = point[2];
+				to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+				to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+
+				sensorCount++;
+			}
+		}
+		to->numSensors = sensorCount;
+
+		if (sensorCount > 4)
+		{
+			FLT pos[3], quat[4];
+
+			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+			printf("!\n");
+		}
+
+		
+	}
+
+
+	free(to);
+
+}
+
+
+
+int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
+{
+	PoserType pt = poserData->pt;
+	SurviveContext * ctx = so->ctx;
+	ToriData * td = so->PoserData;
+
+
+	if (!td)
+	{
+		so->PoserData = td = malloc(sizeof(ToriData));
+		memset(td, 0, sizeof(ToriData));
+	}
+
+	switch( pt )
+	{
+	case POSERDATA_IMU:
+	{
+		PoserDataIMU * tmpImu = (PoserDataIMU*)poserData;
+		
+		// store off data we can use for figuring out what direction is down when doing calibration.
+		//TODO: looks like the data mask isn't getting set correctly.
+		//if (tmpImu->datamask & 1) // accelerometer data is present
+		{
+			td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0];
+			td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1];
+			td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2];
+		}
+		//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] );
+		//printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] );
+		break;
+	}
+	case POSERDATA_LIGHT:
+	{
+		PoserDataLight * l = (PoserDataLight*)poserData;
+
+		if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+		{
+			// should never happen.  Famous last words...
+			break;
+		}
+		int axis = l->acode & 0x1;
+		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+		if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
+		{
+
+
+			if (0 == l->lh && axis) // only once per full cycle...
+			{
+				static unsigned int counter = 1;
+
+				counter++;
+
+				// let's just do this occasionally for now...
+				if (counter % 4 == 0)
+					QuickPose(so, 0);
+			}
+			// axis changed, time to increment the circular buffer index.
+			td->angleIndex[l->lh][axis]++;
+			td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+			// and clear out the data.
+			for (int i=0; i < SENSORS_PER_OBJECT; i++)
+			{
+				td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0;
+			}
+
+			td->lastAxis[l->lh] = axis;
+		}
+
+		td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle;
+		break;
+	}
+	case POSERDATA_FULL_SCENE:
+	{
+		TrackedObject *to;
+
+		PoserDataFullScene * fs = (PoserDataFullScene*)poserData;
+
+		to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+		// if we rotate the internal reference frame of of the tracked object from having -z being arbitrary
+		// to being the down direction as defined by the accelerometer, then when we have come up
+		// with world coordinate system, it will have Z oriented correctly.
+
+		// let's get the quaternion that represents this rotation.
+		FLT downQuat[4];
+		FLT negZ[3] = { 0,0,1 };
+		//quatfrom2vectors(downQuat, negZ, td->down);
+		quatfrom2vectors(downQuat, td->down, negZ);
+
+		{
+			int sensorCount = 0;
+
+
+			for (int i = 0; i < so->nr_locations; i++)
+			{
+				if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0
+				{
+					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
+					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
+
+					//quatrotatevector(norm, downQuat, norm);
+					//quatrotatevector(point, downQuat, point);
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
+					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+					sensorCount++;
+				}
+			}
+			to->numSensors = sensorCount;
+
+			FLT pos[3], quat[4];
+
+			SolveForLighthouse(pos, quat, to, so, 0, 0, 1);
+		}
+		{
+			int sensorCount = 0;
+			int lh = 1;
+
+			for (int i = 0; i < so->nr_locations; i++)
+			{
+				if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) 
+				{
+					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
+					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
+
+					//quatrotatevector(norm, downQuat, norm);
+					//quatrotatevector(point, downQuat, point);
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
+					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					sensorCount++;
+				}
+			}
+
+			to->numSensors = sensorCount;
+
+			FLT pos[3], quat[4];
+
+			SolveForLighthouse(pos, quat, to, so, 0, 1, 1);
+		}
+
+		free(to);
+		//printf( "Full scene data.\n" );
+		break;
+	}
+	case POSERDATA_DISASSOCIATE:
+	{
+		free( so->PoserData );
+		so->PoserData = NULL;
+		//printf( "Need to disassociate.\n" );
+		break;
+	}
+	}
+	return 0;
+}
+
+
+REGISTER_LINKTIME( PoserTurveyTori );
+
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 0eb9446..ae92bad 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -30,6 +30,11 @@ int mkdir(const char *);
 #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
 
 
+	//at stage 1, is when it branches to stage two or stage 7
+	//stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses.
+	//Stage 3 collects a bunch of data for statistical averageing
+	//stage 4 does the calculation for the poses (NOT DONE!)
+	//Stage 5 = System Calibrate.d
 
 
 static void handle_calibration( struct SurviveCalData *cd );
@@ -117,33 +122,51 @@ void survive_cal_install( struct SurviveContext * ctx )
 	cd->stage = 1;
 	cd->ctx = ctx;
 
-	cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" );
-	cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" );
-	cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" );
+	cd->numPoseObjects = 0;
 
-	if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 )
+	const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" );
+	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 );
+	size_t requiredTrackersFound = 0;
+
+	for (int j=0; j < ctx->objs_ct; j++)
 	{
-		SV_ERROR( "Error: cannot find all devices needed for calibration." );
-		free( cd );
-		return;
+		// Add the tracker if we allow all trackers for calibration, or if it's in the list 
+		// of required trackers.
+		int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL;
+
+		if (isRequiredTracker)
+		{
+			requiredTrackersFound++;
+		}
+
+		if (AllowAllTrackersForCal || isRequiredTracker)
+		{
+			if (MAX_DEVICES_TO_CAL > cd->numPoseObjects)
+			{
+				cd->poseobjects[j] = ctx->objs[j];
+				cd->numPoseObjects++;
+
+				SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename);
+			}
+			else
+			{
+				SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename);
+			}
+		}
+
 	}
 
-//XXX TODO MWTourney, work on your code here.
-/*
-	if( !cd->hmd )
-	{
-		cd->hmd = survive_get_so_by_name( ctx, "TR0" );
+	// If we want to mandate that certain devices have been found
 
-		if( !cd->hmd )
+	if (strlen(RequiredTrackersForCal) > 0)
+	{
+		if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4))
 		{
-			SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+			SV_ERROR( "Error: Did not find all devices required for calibration." );
 			free( cd );
 			return;
 		}
-		SV_INFO( "HMD not found, calibrating using Tracker" );
 	}
-*/
-
 
 	const char * DriverName;
 	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
@@ -166,7 +189,7 @@ void survive_cal_install( struct SurviveContext * ctx )
 }
 
 
-void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
@@ -184,8 +207,10 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		//Collecting OOTX data.
 		if( sensor_id < 0 )
 		{
+				//fprintf(stderr, "%s\n", so->codename);
 			int lhid = -sensor_id-1;
-			if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
+			// Take the OOTX data from the first device.  (if using HMD, WM0, WM1 only, this will be HMD)
+			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
 			{
 				uint8_t dbit = (acode & 2)>>1;
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
@@ -193,14 +218,33 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			int i;
 			for( i = 0; i < NUM_LIGHTHOUSES; i++ )
 				if( ctx->bsd[i].OOTXSet == 0 ) break;
-			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
+			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
 		}
 		break;
+	case 3: //Look for light sync lengths.
+	{
+		if( acode >= -2 ) break;
+		else if( acode < -4 ) break;
+		int lh = (-acode) - 3;
+
+		for (int i=0; i < cd->numPoseObjects; i++)
+		{
+			if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+			{
+				sensor_id += i*32;
+			}
+		}
+
+		cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
+		break;
+	}
+
 	}
 	
+
 }
 
-void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
@@ -208,14 +252,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 	if( !cd ) return;
 
 	int sensid = sensor_id;
-	if( strcmp( so->codename, "WM0" ) == 0 )
-		sensid += 32;
-	if( strcmp( so->codename, "WM1" ) == 0 )
-		sensid += 64;
+
+	for (int i=0; i < cd->numPoseObjects; i++)
+	{
+		if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+		{
+			sensid += i*32;
+		}
+	}
 
 	if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
 
-	int lighthouse = acode>>2;
+	int lighthouse = lh;
 	int axis = acode & 1;
 
 	switch( cd->stage )
@@ -259,7 +307,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			int min_peaks = PTS_BEFORE_COMMON;
 			int i, j, k;
 			cd->found_common = 1;
-			for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
+			for( i = 0; i < cd->numPoseObjects; i++ )
+			//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
 			for( j = 0; j < NUM_LIGHTHOUSES; j++ )
 			{
 				int sensors_visible = 0;
@@ -272,6 +321,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 				if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) 
 				{
 					//printf( "Dev %d, LH %d not enough visible points found.\n", i, j );
+					reset_calibration( cd );
 					cd->found_common = 0;
 					return;
 				}
@@ -332,6 +382,8 @@ static void reset_calibration( struct SurviveCalData * cd )
 	cd->found_common = 0;
 	cd->times_found_common = 0;
 	cd->stage = 2;
+
+	memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) );
 }
 
 static void handle_calibration( struct SurviveCalData *cd )
@@ -357,9 +409,45 @@ static void handle_calibration( struct SurviveCalData *cd )
 #else
 	mkdir( "calinfo", 0755 );
 #endif
+	int sen, axis, lh;
+	FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+
+	//Just to get it out of the way early, we'll calculate the sync-pulse-lengths here.
+	FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" );
+
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+	{
+		int count = cd->all_sync_counts[sen][lh];
+		int i;
+		double totaltime;
+
+		totaltime = 0;
+
+		if( count < 20 ) continue;
+		for( i = 0; i < count; i++ )
+		{
+			totaltime += cd->all_sync_times[sen][lh][i];
+		}
+		FLT avg = totaltime/count;
+
+		double stddev = 0.0;
+		for( i = 0; i < count; i++ )
+		{
+			stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg);
+		}
+		stddev /= count;
+
+		fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev );
+	}
+
+	fclose( sync_time_info );
+
+
+
+
 	FILE * hists = fopen( "calinfo/histograms.csv", "w" );
 	FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
-	int sen, axis, lh;
 	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
 	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
 	for( axis = 0; axis < 2; axis++ )
@@ -440,7 +528,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 		FLT stddevlen = 0;
 
 		#define HISTOGRAMSIZE   31
-		#define HISTOGRAMBINANG 0.00001  //TODO: Tune
+		#define HISTOGRAMBINANG ((3.14159)/400000.0)  //TODO: Tune
 
 		int histo[HISTOGRAMSIZE];
 		memset( histo, 0, sizeof( histo ) );
@@ -498,11 +586,11 @@ static void handle_calibration( struct SurviveCalData *cd )
 	int obj;
 
 	//Poses of lighthouses relative to objects.
-	SurvivePose  objphl[POSE_OBJECTS][NUM_LIGHTHOUSES];
+	SurvivePose  objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES];
 
 	FILE * fobjp = fopen( "calinfo/objposes.csv", "w" );
 
-	for( obj = 0; obj < POSE_OBJECTS; obj++ )
+	for( obj = 0; obj < cd->numPoseObjects; obj++ )
 	{
 		int i, j;
 		PoserDataFullScene fsd;
@@ -525,6 +613,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 			fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
 			fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
 			fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}
 
 		int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd );
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 3d62051..ae644d1 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -29,15 +29,19 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri
 //void survive_cal_teardown( struct SurviveContext * ctx );
 
 //Called from survive_default_light_process
-void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
-void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 
-#define MAX_SENSORS_TO_CAL 96
+#define MAX_SENSORS_PER_DEVICE 32
+#define MAX_DEVICES_TO_CAL 3
+#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL)
 
 #define MIN_PTS_BEFORE_CAL 24
-#define DRPTS 128
 
-#define POSE_OBJECTS 3
+
+#define DRPTS 32  //Number of samples required in collection phase.
+
+#define MAX_POSE_OBJECTS 10
 
 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
@@ -54,6 +58,9 @@ struct SurviveCalData
 	int8_t found_common;
 	int8_t times_found_common;
 
+	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS];
+	int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES];
+
 	//For camfind (4+)
 	//Index is calculated with:      int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
 	FLT avgsweeps[MAX_CAL_PT_DAT];
@@ -64,7 +71,9 @@ struct SurviveCalData
 
 	int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
 
-	SurviveObject * poseobjects[POSE_OBJECTS];
+	SurviveObject * poseobjects[MAX_POSE_OBJECTS];
+
+	size_t numPoseObjects;
 
 	PoserCB ConfigPoserFn;
 
diff --git a/src/survive_config.c b/src/survive_config.c
index 0810280..3a83902 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value)
 	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
-	sstrcpy(&(cv->data), value);
+
+	if (NULL != value){
+		sstrcpy(&(cv->data), value);
+	}
+	else {
+		sstrcpy(&(cv->data), "");
+	}
 	cv->type = CONFIG_STRING;
 
 	return value;
@@ -354,12 +360,17 @@ int parse_uint32(char* tag, char** values, uint16_t count) {
 }
 
 void handle_tag_value(char* tag, char** values, uint8_t count) {
-	print_json_value(tag,values,count);
+
+	//Uncomment for more debugging of input configuration.
+	//print_json_value(tag,values,count);
+
 	config_group* cg = cg_stack[cg_stack_head];
 
+	if (NULL != *values){	
 	if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules
 
 	if (parse_floats(tag,values,count) > 0) return;
+	}
 
 	//should probably also handle string arrays
 	config_set_str(cg,tag,values[0]);
diff --git a/src/survive_data.c b/src/survive_data.c
index 4a2cfb6..157650d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -4,21 +4,546 @@
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
+#include <math.h> /* for sqrt */
+
+//#define USE_TURVEYBIGUATOR
+
+#ifdef USE_TURVEYBIGUATOR
+
+static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054,
+	1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211,
+	1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599,
+	1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058,
+	1.9114057255, 1.9130437248, 1.914574735
+};
+
+typedef struct
+{
+	unsigned int sweep_time[SENSORS_PER_OBJECT];
+	uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access
+} lightcaps_sweep_data;
+
+typedef struct
+{
+	int recent_sync_time;
+	int activeLighthouse;
+	int activeSweepStartTime;
+	int activeAcode;
+
+//	int lh_pulse_len[NUM_LIGHTHOUSES];
+	int lh_start_time[NUM_LIGHTHOUSES];
+	int lh_max_pulse_length[NUM_LIGHTHOUSES];
+	int8_t lh_acode[NUM_LIGHTHOUSES];
+	int current_lh; // used knowing which sync pulse we're looking at.
+
+} lightcap2_per_sweep_data;
+
+typedef struct
+{
+	double acode_offset;
+} lightcap2_global_data;
+
+typedef struct
+{
+	lightcaps_sweep_data sweep;
+	lightcap2_per_sweep_data per_sweep;
+	lightcap2_global_data global;
+} lightcap2_data;
+
+
+//static lightcap2_global_data lcgd = { 0 };
+
+int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
+{
+	double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
+
+	int modifiedPulseLen = pulseLen - (int)oldOffset;
+
+	double newOffset = (((pulseLen) + 250) % 500) - 250;
+
+	((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
+
+//fprintf(stderr, "    %f\n", oldOffset);
+#define ACODE_OFFSET 0
+	if (pulseLen < 3250 - ACODE_OFFSET) return 0;
+	if (pulseLen < 3750 - ACODE_OFFSET) return 1;
+	if (pulseLen < 4250 - ACODE_OFFSET) return 2;
+	if (pulseLen < 4750 - ACODE_OFFSET) return 3;
+	if (pulseLen < 5250 - ACODE_OFFSET) return 4;
+	if (pulseLen < 5750 - ACODE_OFFSET) return 5;
+	if (pulseLen < 6250 - ACODE_OFFSET) return 6;
+	return 7;
+}
+
+uint8_t remove_outliers(SurviveObject *so) {
+	return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker.
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	uint32_t sum = 0;
+	uint8_t non_zero_count = 0;
+	uint32_t mean = 0;
+
+	uint16_t* min = NULL;
+	uint16_t* max = NULL;
+	uint8_t found_first = 0;
+
+	//future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab
+
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		sum += lcd->sweep.sweep_len[i];
+		if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count;
+	}
+
+	if (non_zero_count==0) return 0;
+
+	mean = sum/non_zero_count;
+
+	float standard_deviation = 0.0f;
+	sum = 0;
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		uint16_t len = lcd->sweep.sweep_len[i];
+		if (len > 0) {
+			sum += (len - mean)*(len - mean);
+
+			if (found_first==0) {
+				max = min = lcd->sweep.sweep_len + i;
+				found_first=1;
+			} else {
+				if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i;
+				if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i;
+			}
+		}
+	}
+	standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) );
+
+//	printf("%f\n", standard_deviation);
+
+	float tau_test = standard_deviation;
+
+	if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count];
+
+//	uint8_t removed_outliers = 0;
+
+	uint32_t d1 = abs(*min - mean);
+	uint32_t d2 = abs(*max - mean);
+
+	if (d1>d2) {
+		if (d1 > tau_test) {
+			*min = 0;
+			return 1;
+		}
+	}
+	else if (d2>tau_test) {
+		*max = 0;
+		return 1;
+	}
+
+	return 0;
+/*
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		uint16_t len = lcd->sweep.sweep_len[i];
+		if (len == 0) continue;
+
+		if ( abs(len-mean) > tau_test )
+		{
+//			fprintf(stderr, "removing %d\n", len);
+			lcd->sweep.sweep_len[i] = 0;
+			removed_outliers = 1;
+		}
+	}
+*/
+//	return removed_outliers;
+}
+
+void handle_lightcap2_process_sweep_data(SurviveObject *so)
+{
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	while(remove_outliers(so));
+
+	// look at all of the sensors we found, and process the ones that were hit.
+	// TODO: find the sensor(s) with the longest pulse length, and assume 
+	// those are the "highest quality".  Then, reject any pulses that are sufficiently
+	// different from those values, assuming that they are reflections.
+	{
+		unsigned int longest_pulse = 0;
+		unsigned int timestamp_of_longest_pulse = 0;
+		for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+		{
+			if (lcd->sweep.sweep_len[i] > longest_pulse)
+			{
+				longest_pulse = lcd->sweep.sweep_len[i];
+				timestamp_of_longest_pulse = lcd->sweep.sweep_time[i];
+			}
+		}
+
+		int allZero = 1;
+		for (int q=0; q< 32; q++)
+			if (lcd->sweep.sweep_len[q] != 0)
+				allZero=0;
+		//if (!allZero)
+		//	printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5,  lcd->per_sweep.activeLighthouse);
+		for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+		{
+
+			{
+				static int counts[SENSORS_PER_OBJECT][2] = {0};
+
+//				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
+				if (lcd->per_sweep.activeLighthouse > -1 && !allZero)
+				{
+					if (lcd->sweep.sweep_len[i] != 0)
+					{
+						//printf("%d  ", i);
+						//counts[i][lcd->per_sweep.activeAcode & 1] ++;
+					}
+					else
+					{
+						counts[i][lcd->per_sweep.activeAcode & 1] =0;
+					}
+
+					//if (counts[i][0] > 10 && counts[i][1] > 10)
+					//{
+						//printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]);
+					//}
+				}
+			}
+
+
+			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
+			{
+//printf("%4d\n", lcd->sweep.sweep_len[i]);
+				int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
+
+//				if (offset_from < 380000 && offset_from > 70000)
+				{
+					//if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
+					{
+						so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse);
+					}
+				}
+			}
+		}
+		//if (!allZero)
+		//	printf("   ..:..\n");
+
+//		if (!allZero) printf("\n");
+	}
+
+	// clear out sweep data (could probably limit this to only after a "first" sync.  
+	// this is slightly more robust, so doing it here for now.
+	memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data));
+}
+void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
+{
+	//fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length);
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	//static unsigned int recent_sync_time = 0;
+	//static unsigned int recent_sync_count = -1;
+	//static unsigned int activeSweepStartTime;
+
+	int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading
+
+	// Process any sweep data we have
+	handle_lightcap2_process_sweep_data(so);
+
+	int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
+
+
+//	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
+	if (time_since_last_sync < 2400)
+	{
+		lcd->per_sweep.recent_sync_time = le->timestamp;
+		// it's the same sync pulse;
+//		so->sync_set_number = 1;
+		so->recent_sync_time = le->timestamp;
+
+//		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+//		lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+
+		if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) {
+			lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+			lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+			lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
+		}
+
+/*
+		//this stuff should probably be happening on the sweep so that we can filter out erroneous a codes
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
+		{
+			lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
+		}
+		else
+		{
+			//this causes the master lighthouse to be ignored from the HMD
+			lcd->per_sweep.activeLighthouse = -1;
+			lcd->per_sweep.activeSweepStartTime = 0;
+			lcd->per_sweep.activeAcode = 0;
+		}
+		*/
+	}
+	else if (time_since_last_sync < 24000)
+	{
+		lcd->per_sweep.activeLighthouse = -1;
+
+		lcd->per_sweep.recent_sync_time = le->timestamp;
+		// I do believe we are lighthouse B		
+		lcd->per_sweep.current_lh = 1;
+//		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+		lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
+
+/*
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
+		{
+			if (lcd->per_sweep.activeLighthouse != -1)
+			{
+				static int pulseWarningCount=0;
+
+				if (pulseWarningCount < 5)
+				{
+					pulseWarningCount++;
+					// hmm, it appears we got two non-skip pulses at the same time.  That should never happen
+					fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
+				}
+			}
+
+			lcd->per_sweep.activeLighthouse = 1;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
+		}
+		*/
+
+	}
+	else if (time_since_last_sync > 370000)
+	{
+		// XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master
+		//this should probably be fixed. Maybe some kind of timing based guess at which lighthouse.
+
+		// looks like this is the first sync pulse.  Cool!
+
+		// first, send out the sync pulse data for the last round (for OOTX decoding
+		{
+			if (lcd->per_sweep.lh_max_pulse_length[0] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-1,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
+					lcd->per_sweep.lh_max_pulse_length[0],
+					lcd->per_sweep.lh_start_time[0],
+					0,
+					0);
+			}
+			if (lcd->per_sweep.lh_max_pulse_length[1] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-2,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
+					lcd->per_sweep.lh_max_pulse_length[1],
+					lcd->per_sweep.lh_start_time[1],
+					0,
+					1);
+			}
+		}
+
+		//fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n");
+		// initialize here.
+		memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
+		lcd->per_sweep.activeLighthouse = -1; 
+
+		for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) {
+			lcd->per_sweep.lh_acode[i] = -1;
+		}
+
+		lcd->per_sweep.recent_sync_time = le->timestamp;
+		// I do believe we are lighthouse A		
+		lcd->per_sweep.current_lh = 0;
+//		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+		lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
+
+//		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
+
+/*
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
+		{
+			lcd->per_sweep.activeLighthouse = 0;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
+		}
+		*/
+	}
+
+//	printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse );
+}
+
+void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
+{
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	// If we see multiple "hits" on the sweep for a given sensor,
+	// assume that the longest (i.e. strongest signal) is most likely 
+	// the non-reflected signal. 
+
+	//if (le->length < 80)
+	//{
+	//	// this is a low-quality read.  Better to throw it out than to use it.
+	//	//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+	//	return;
+	//}
+	//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+	//fprintf(stderr, ".");
+
+	lcd->per_sweep.activeLighthouse = -1;
+	lcd->per_sweep.activeSweepStartTime = 0;
+	lcd->per_sweep.activeAcode = 0;
+
+	for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) {
+		int acode = lcd->per_sweep.lh_acode[i];
+		if ( (acode>=0) && !(acode >> 2 & 1)) {
+			lcd->per_sweep.activeLighthouse = i;
+			lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i];
+			lcd->per_sweep.activeAcode = acode;
+		}
+	}
+
+	if (lcd->per_sweep.activeLighthouse < 0) {
+		fprintf(stderr, "WARNING: No active lighthouse!\n");
+		fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
+		return;
+	}
+
+	if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
+	{
+		lcd->sweep.sweep_len[le->sensor_id] = le->length;
+		lcd->sweep.sweep_time[le->sensor_id] = le->timestamp;
+	}
+}
+
+void handle_lightcap2( SurviveObject * so, LightcapElement * le )
+{
+	SurviveContext * ctx = so->ctx;
+
+	if (so->disambiguator_data == NULL)
+	{
+		fprintf(stderr, "Initializing Disambiguator Data\n");
+		so->disambiguator_data = malloc(sizeof(lightcap2_data));
+		memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
+	}
+
+	if( le->sensor_id > SENSORS_PER_OBJECT )
+	{
+		return;
+	}
+
+	if (le->length > 6750)
+	{
+		// Should never get a reading so high.  Odd.
+		return;
+	}
+//	if (le->length >= 2750)
+	if (le->length >= 2500)
+	{
+		// Looks like a sync pulse, process it!
+		handle_lightcap2_sync(so, le);
+		return;
+	}
+
+	// must be a sweep pulse, process it!
+	handle_lightcap2_sweep(so, le);
+
+}
+
+
+#endif
+
+
+int32_t decode_acode(uint32_t length, int32_t main_divisor) {
+	//+50 adds a small offset and seems to help always get it right. 
+	//Check the +50 in the future to see how well this works on a variety of hardware.
+	if( !main_divisor ) return -1;
+	int32_t acode = (length+main_divisor+50)/(main_divisor*2);
+	if( acode & 1 ) return -1;
+
+	return (acode>>1) - 6;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////	
+///////The charles disambiguator.  Don't use this, mostly here for debugging.///////////////////////////////////////////////////////	
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////	
+
+
+void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
+{
+	int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+	int32_t acode_array[2] =
+		{
+			decode_acode(so->last_sync_length[0],main_divisor),
+			decode_acode(so->last_sync_length[1],main_divisor)
+		};
+
+
+	int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
+	int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
+
+	//printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
+	if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+	if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
+
+	so->recent_sync_time = so->last_sync_time[1];
+
+	so->did_handle_ootx = 1;
+}
+
 
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
 	SurviveContext * ctx = so->ctx;
-	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
-	//if( so->codename[0] != 'H' )
+#ifdef USE_TURVEYBIGUATOR
+	handle_lightcap2(so,le);
+	return;
 
+#else
+	//printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp );
+
+	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 	if( le->sensor_id > SENSORS_PER_OBJECT )
 	{
 		return;
 	}
 
+#if 0
+	if( so->codename[0] == 'H' )
+	{
+		static int lt;
+		static int last;
+		if( le->length > 1000 )
+		{
+			int dl = le->timestamp - lt;
+			lt = le->timestamp;
+			if( dl > 10000 || dl < -10000 )
+				printf( "+++%s %3d %5d %9d  ", so->codename, le->sensor_id, le->length, dl );
+			if( dl > 100000 ) printf(" \n" );
+		}
+		last=le->length;
+	}
+#endif
+
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
 
@@ -27,6 +552,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	//unified driver.
 	int ssn = so->sync_set_number; //lighthouse number
 	if( ssn < 0 ) ssn = 0;
+#ifdef DEBUG
+	if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); }
+#endif
 	int last_sync_time  =  so->last_sync_time  [ssn];
 	int last_sync_length = so->last_sync_length[ssn];
 	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
@@ -44,17 +572,39 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
-//		printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta);
 
-		so->did_handle_ootx = 0;
+
 
 		if( is_new_pulse )
 		{
 			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
+			int is_pulse_from_same_lh_as_last_sweep;
+			int tp = delta % ( so->timecenter_ticks * 2);
+			is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
+
+			if( !so->did_handle_ootx )
+			{
+				HandleOOTX( ctx, so );
+			}
+			if( !is_master_sync_pulse )
+			{
+				so->did_handle_ootx = 0;
+			}
+
 
-			if( is_master_sync_pulse )
+			if( is_master_sync_pulse )  //Could also be called by slave if no master was seen.
 			{
-				ssn = so->sync_set_number = 0;
+				ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one.
+				if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; }
+				if( ssn != 0 )
+				{
+					//If it's the slave that is repeated, be sure to zero out its sync info.
+					so->last_sync_length[0] = 0;
+				}
+				else
+				{
+					so->last_sync_length[1] = 0;
+				}
 				so->last_sync_time[ssn] = le->timestamp;
 				so->last_sync_length[ssn] = le->length;
 			}
@@ -65,6 +615,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			else
 			{
 				ssn = ++so->sync_set_number;
+
 				if( so->sync_set_number >= NUM_LIGHTHOUSES )
 				{
 					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
@@ -89,9 +640,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				}
 			}
 		}
-	}
 
+#if 0
+		//Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
+		//We don't actually use this anywhere, and I doubt we ever will?  Though, it could be useful at a later time to improve tracking.
+		{
+			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+			int base_station = is_new_pulse;
+			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here.
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here.
+		}
+#endif
+	}
 
+	//Any else- statements below here are 
 
 	//See if this is a valid actual pulse.
 	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
@@ -109,73 +671,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure it fits nicely into a divisible-by-500 time.
 
 		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
-
-		int32_t acode_array[2] =
-			{
-				(so->last_sync_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
-				(so->last_sync_length[1]+main_divisor+50)/(main_divisor*2),	//Check the +50 in the future to see how well this works on a variety of hardware.
-			};
-
-		//XXX: TODO: Capture error count here.
-		if( acode_array[0] & 1 ) return;
-		if( acode_array[1] & 1 ) return;
-
-		acode_array[0] = (acode_array[0]>>1) - 6;
-		acode_array[1] = (acode_array[1]>>1) - 6;
-
-
-		int acode = acode_array[0];
+		int acode = decode_acode(so->last_sync_length[0],main_divisor);
 
 		if( !so->did_handle_ootx )
-		{
-			int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
-			int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
-
-			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] );
-			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] );
-
-			so->recent_sync_time = so->last_sync_time[1];
-
-			//Throw out everything if our sync pulses look like they're bad.
-
-			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
-			int32_t center_2 = so->pulse_synctime_offset;
-			int32_t slack = so->pulse_synctime_slack;
-
-			if( delta1 < center_1 - slack || delta1 > center_1 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-
-			if( delta2 < center_2 - slack || delta2 > center_2 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-
-			so->did_handle_ootx = 1;
-		}
-
-
-		if (acode > 3) {
-			if( ssn == 0 )
-			{
-				//SV_INFO( "Warning: got a slave marker but only got a master sync." );
-				//This happens too frequently.  Consider further examination.
-			}
-			dl = so->last_sync_time[1];
-			tpco = so->last_sync_length[1];
-		}
+			HandleOOTX( ctx, so );
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
-		if( offset_from < 380000 && offset_from > 70000 )
+		if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			int whichlh;
+			if( acode < 0 ) whichlh = 1;
+			else whichlh = !(acode>>2);
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
 		}
 	}
 	else
@@ -183,6 +692,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp );
 		//Runt pulse, or no sync pulses available.
 	}
+#endif
 }
 
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 8dc849a..3af2da9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,16 +6,19 @@
 //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
 //proper angles, then from there perform the rest of the solution. 
 
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 	SurviveContext * ctx = so->ctx;
-	int base_station = acode >> 2;
+	int base_station = lh;
 	int axis = acode & 1;
 	if( ctx->calptr )
 	{
-		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	}
 
+	//We don't use sync times, yet.
+	if( acode < -1 ) return;
+
 	if( base_station > NUM_LIGHTHOUSES ) return;
 
 	//No loner need sync information past this point.
@@ -34,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 #endif
 
 	FLT length_sec = length / (FLT)so->timebase_hz;
-	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
+	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh);
 }
 
 
-void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
 {
 	SurviveContext * ctx = so->ctx;
 	if( ctx->calptr )
 	{
-		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+		survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh );
 	}
 	if( so->PoserFn )
 	{
@@ -54,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 			.timecode = timecode,
 			.length = length,
 			.angle = angle,
+			.lh = lh,
 		};
 		so->PoserFn( so, (PoserData *)&l );
 	}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index cdc319d..9a3cb03 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -22,6 +22,8 @@
 #include <malloc.h> // for alloca
 #endif
 
+#include "json_helpers.h"
+
 #ifdef HIDAPI
 #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32)
 #include <windows.h>
@@ -39,48 +41,60 @@
 struct SurviveViveData;
 
 const short vidpids[] = {
-	0x0bb4, 0x2c87, 0, //The main HTC HMD device
-	0x28de, 0x2000, 0, //Valve lighthouse
+	0x0bb4, 0x2c87, 0, //Valve HMD Button and face proximity sensor
+	0x28de, 0x2000, 0, //Valve HMD IMU & Lighthouse Sensors
 	0x28de, 0x2101, 0, //Valve Watchman
 	0x28de, 0x2101, 1, //Valve Watchman
 	0x28de, 0x2022, 0, //HTC Tracker
+	0x28de, 0x2012, 0, //Valve Watchman, USB connected
 #ifdef HIDAPI
-	0x28de, 0x2000, 1, //Valve lighthouse(B) (only used on HIDAPI, for lightcap)
+	0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
+	0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap)
+	0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
 #endif
 }; //length MAX_USB_INTERFACES*2
 
 const char * devnames[] = {
 	"HMD",
-	"Lighthouse",
+	"HMD IMU & LH",
 	"Watchman 1",
 	"Watchman 2",
 	"Tracker 0",
+	"Wired Watchman 1",
 #ifdef HIDAPI
-	"Lightcap",
+	"HMD Lightcap",
+	"Tracker 0 Lightcap",
+	"Wired Watchman 1 Lightcap",
 #endif
 }; //length MAX_USB_INTERFACES
 
 
 #define USB_DEV_HMD			0
-#define USB_DEV_LIGHTHOUSE	1
+#define USB_DEV_HMD_IMU_LH	1
 #define USB_DEV_WATCHMAN1	2
 #define USB_DEV_WATCHMAN2	3
 #define USB_DEV_TRACKER0	4
+#define USB_DEV_W_WATCHMAN1	5 // Wired Watchman attached via USB
 
 #ifdef HIDAPI
-#define USB_DEV_LIGHTHOUSEB 5
-#define MAX_USB_DEVS		6
+#define USB_DEV_HMD_IMU_LHB 6
+#define USB_DEV_TRACKER0_LIGHTCAP 7
+#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8
+#define MAX_USB_DEVS		9
 #else
-#define MAX_USB_DEVS		5
+#define MAX_USB_DEVS		6
 #endif
 
 #define USB_IF_HMD			0
-#define USB_IF_LIGHTHOUSE 	1
+#define USB_IF_HMD_IMU_LH 	1
 #define USB_IF_WATCHMAN1	2
 #define USB_IF_WATCHMAN2	3
 #define USB_IF_TRACKER0		4
-#define USB_IF_LIGHTCAP		5
-#define MAX_INTERFACES		6
+#define USB_IF_W_WATCHMAN1	5
+#define USB_IF_LIGHTCAP		6
+#define USB_IF_TRACKER0_LIGHTCAP		7
+#define USB_IF_W_WATCHMAN1_LIGHTCAP		8
+#define MAX_INTERFACES		9
 
 typedef struct SurviveUSBInterface SurviveUSBInterface;
 typedef struct SurviveViveData SurviveViveData;
@@ -133,10 +147,10 @@ void survive_data_cb( SurviveUSBInterface * si );
 
 //USB Subsystem 
 void survive_usb_close( SurviveContext * t );
-int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 );
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 );
 int survive_usb_poll( SurviveContext * ctx );
 int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic );
-int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
+int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
 
 #ifdef HIDAPI
 void * HAPIReceiver( void * v )
@@ -169,8 +183,8 @@ void * HAPIReceiver( void * v )
 #else
 static void handle_transfer(struct libusb_transfer* transfer)
 {
-	struct SurviveUSBInterface * iface = transfer->user_data;
-	struct SurviveContext * ctx = iface->ctx;
+	SurviveUSBInterface * iface = transfer->user_data;
+	SurviveContext * ctx = iface->ctx;
 
 	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
 	{
@@ -299,9 +313,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac
 	return -1;
 }
 
-int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 )
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 )
 {
-	struct SurviveContext * ctx = sv->ctx;
+	SurviveContext * ctx = sv->ctx;
 #ifdef HIDAPI
 	if( !GlobalRXUSBMutx )
 	{
@@ -332,7 +346,8 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
 			if (cur_dev->vendor_id == vendor_id &&
 				cur_dev->product_id == product_id)
 			{
-				if( menum == enumid )
+				if( cur_dev->interface_number == enumid ||
+					cur_dev->interface_number == -1 && menum == enumid)
 				{
 					path_to_open = cur_dev->path;
 					break;
@@ -358,6 +373,7 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
 		wchar_t wstr[255];
 
 		res = hid_get_serial_number_string(handle, wstr, 255);
+		printf("Found %s. ", devnames[i]);
 		wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d  (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle);
 		
 		sv->udev[i] = handle;
@@ -459,15 +475,23 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
 #endif
 
 	if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD,        sv->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
-	if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
+	if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
 	if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1,  sv->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
 	if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; }
 	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; }
+	if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; }
 #ifdef HIDAPI
 	//Tricky: use other interface for actual lightcap.  XXX THIS IS NOT YET RIGHT!!!
-	if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+	if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+
+	// This is a HACK!  But it works.  Need to investigate further
+	sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64;
+	if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; }
+	if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
 #else
-	if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+	if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
+	if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
 #endif
 	SV_INFO( "All enumerated devices attached." );
 
@@ -477,10 +501,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
 	return 0;
 }
 
-int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
+int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
 {
 	int r;
-	struct SurviveViveData * sv = drv;
+	SurviveViveData * sv = drv;
 	printf( "*CALLING %p %p\n", ctx, sv );
 
 	//XXX TODO: Handle haptics, etc.
@@ -497,27 +521,100 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c
 			if( r != sizeof( vive_magic_power_on ) ) return 5;
 		}
 
-		if (sv->udev[USB_DEV_LIGHTHOUSE])
+		if (sv->udev[USB_DEV_HMD_IMU_LH])
+		{
+			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+			r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+		}
+
+		if (sv->udev[USB_DEV_W_WATCHMAN1])
+		{
+			static uint8_t vive_magic_power_on[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+			if( r != sizeof( vive_magic_power_on ) ) return 5;
+		}
+
+#ifdef HIDAPI
+		if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP])
+		{
+			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+		}
+
+#else
+		if (sv->udev[USB_DEV_W_WATCHMAN1])
+		{
+			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+		}
+
+#endif
+
+		if (sv->udev[USB_DEV_TRACKER0])
+		{
+			static uint8_t vive_magic_power_on[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+			if( r != sizeof( vive_magic_power_on ) ) return 5;
+		}
+//#ifdef HIDAPI
+//		if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP])
+//		{
+//			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+//			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+//			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+//
+//			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+//			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+//			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+//		}
+//#else		
+		if (sv->udev[USB_DEV_TRACKER0])
 		{
 			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
-			r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
 			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
 
 			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
-			r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
 			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
 		}
 
+//#endif
+
 #if 0
-		for( i = 0; i < 256; i++ )
+		for( int i = 0; i < 256; i++ )
 		{
 			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
-			r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+			//r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
 			SV_INFO( "UCR: %d", r );
 			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
 			OGUSleep( 1000 );
 		}
 #endif
+
+		//if (sv->udev[USB_DEV_TRACKER0])
+		//{
+		//	static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
+		//	r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+		//	if( r != sizeof( vive_magic_power_on ) ) return 5;
+		//}
+
 		SV_INFO( "Powered unit on." );
 	}
 	else
@@ -553,7 +650,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c
 	return 0;
 }
 
-void survive_vive_usb_close( struct SurviveViveData * sv )
+void survive_vive_usb_close( SurviveViveData * sv )
 {
 	int i;
 #ifdef HIDAPI
@@ -578,18 +675,19 @@ void survive_vive_usb_close( struct SurviveViveData * sv )
 #endif
 }
 
-int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
+int survive_vive_usb_poll( SurviveContext * ctx, void * v )
 {
 #ifdef HIDAPI
 	OGUnlockMutex( GlobalRXUSBMutx );
 	OGUSleep( 100 );
 	OGUnlockMutex( GlobalRXUSBMutx );
+	return 0;
 #else
-	struct SurviveViveData * sv = v;
+	SurviveViveData * sv = v;
 	int r = libusb_handle_events( sv->usbctx );
 	if( r )
 	{
-		struct SurviveContext * ctx = sv->ctx;
+		SurviveContext * ctx = sv->ctx;
 		SV_ERROR( "Libusb poll failed." );
 	}
 	return r;
@@ -598,9 +696,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
 }
 
 
-int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic )
+int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic )
 {
-	struct SurviveContext * ctx = sv->ctx;
+	SurviveContext * ctx = sv->ctx;
 	int ret, count = 0, size = 0;
 	uint8_t cfgbuff[64];
 	uint8_t compressed_data[8192];
@@ -629,11 +727,9 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno,
 	
 		#else
 		//Switch mode to pull config?
-		SV_INFO( "XXX TODO See if this can be update_feature_report" );
 		for( k = 0; k < 10; k++ )
 		{
-			int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
-			0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 );
+			update_feature_report( dev, iface, cfgbuff_send, 64 );
 			OGUSleep(1000);
 		}
 		#endif
@@ -643,6 +739,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno,
 		OGUSleep(1000);
 	}
 
+	// Send Report 16 to prepare the device for reading config info
 	memset( cfgbuff, 0, sizeof( cfgbuff ) );
 	cfgbuff[0] = 0x10;
 	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
@@ -652,6 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno,
 	}
 
 
+	// Now do a bunch of Report 17 until there are no bytes left
 	cfgbuff[1] = 0xaa;
 	cfgbuff[0] = 0x11;
 	do
@@ -716,7 +814,36 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno,
 #define POP2  (*(((uint16_t*)((readdata+=2)-2))))
 #define POP4  (*(((uint32_t*)((readdata+=4)-4))))
 
-static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
+void calibrate_acc(SurviveObject* so, FLT* agm) {
+	if (so->acc_bias != NULL) {
+		agm[0] -= so->acc_bias[0];
+		agm[1] -= so->acc_bias[1];
+		agm[2] -= so->acc_bias[2];
+	}
+
+	if (so->acc_scale != NULL) {
+		agm[0] *= so->acc_scale[0];
+		agm[1] *= so->acc_scale[1];
+		agm[2] *= so->acc_scale[2];
+	}
+}
+
+void calibrate_gyro(SurviveObject* so, FLT* agm) {
+	if (so->gyro_bias != NULL) {
+		agm[0] -= so->gyro_bias[0];
+		agm[1] -= so->gyro_bias[1];
+		agm[2] -= so->gyro_bias[2];
+	}
+
+	if (so->gyro_scale != NULL) {
+		agm[0] *= so->gyro_scale[0];
+		agm[1] *= so->gyro_scale[1];
+		agm[2] *= so->gyro_scale[2];
+	}
+}
+
+
+static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 {
 
 	uint8_t startread[29];
@@ -739,7 +866,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	int propset = 0;
 	int doimu = 0;
 
-
 	if( (type & 0xf0) == 0xf0 )
 	{
 		propset |= 4;
@@ -794,6 +920,21 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 						readdata[4], readdata[5], readdata[6],
 						0,0,0 };
 
+//		if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]);
+		calibrate_acc(w, agm);
+
+		//I don't understand where these numbers come from but the data from the WMD seems to max out at 255...
+		agm[0]*=(1.0f/255.0f);
+		agm[1]*=(1.0f/255.0f);
+		agm[2]*=(1.0f/255.0f);
+
+		calibrate_gyro(w, agm+3);
+
+		//I don't understand where these numbers come from but the data from the WMD seems to max out at 255...
+		agm[3]*=(1.0f/255.0f);
+		agm[4]*=(1.0f/255.0f);
+		agm[5]*=(1.0f/255.0f);
+
 		w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
 
 		int16_t * k = (int16_t *)readdata+1;
@@ -815,10 +956,9 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		*readdata = type; //Put 'type' back on stack.
 		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
 
-//#define DEBUG_WATCHMAN
 #ifdef DEBUG_WATCHMAN
 		printf( "_%s ", w->codename);
-		for( i = 0; i < qty; i++ )
+		for(int i = 0; i < qty; i++ )
 		{
 			printf( "%02x ", readdata[i] );
 		}
@@ -883,11 +1023,12 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		LightcapElement les[10];
 		int lese = 0; //les's end
 
+
 		//Second, go through all LEDs and extract the lightevent from them. 
 		{
 			uint8_t *marked;
 			marked = alloca(nrtime);
-			memset( marked, 0, sizeof( marked ) );
+			memset( marked, 0, nrtime );
 			int i, parpl = 0;
 			timecount--;
 			int timepl = 0;
@@ -900,8 +1041,20 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 				led >>= 3;
 
 				while( marked[timepl] ) timepl++;
+
+#ifdef DEBUG_WATCHMAN
+				int i;
+				printf( "TP %d   TC: %d : ", timepl, timecount );
+				for( i = 0; i < nrtime; i++ )
+				{
+					printf( "%d", marked[i] );
+				}
+				printf( "\n" );
+#endif
+
 				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
 				uint32_t endtime = times[timepl++];
+
 				int end = timepl + adv;
 				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
 				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
@@ -945,12 +1098,11 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	end:
 		{
 			SurviveContext * ctx = w->ctx;
-			SV_INFO( "Light decoding fault: %d\n", fault );
+			SV_INFO( "Light decoding fault: %d", fault );
 		}
 	}
 }
 
-
 void survive_data_cb( SurviveUSBInterface * si )
 {
 	int size = si->actual_len;
@@ -982,7 +1134,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 	{
 	case USB_IF_HMD:
 	{
-		struct SurviveObject * headset = obj;
+		SurviveObject * headset = obj;
 		readdata+=2;
 		headset->buttonmask = POP1;		//Lens
 		headset->axis2 = POP2;			//Lens Separation
@@ -996,7 +1148,9 @@ void survive_data_cb( SurviveUSBInterface * si )
 		headset->ison = 1;
 		break;
 	}
-	case USB_IF_LIGHTHOUSE:
+	case USB_IF_HMD_IMU_LH:
+	case USB_IF_W_WATCHMAN1:
+	case USB_IF_TRACKER0:
 	{
 		int i;
 		//printf( "%d -> ", size );
@@ -1018,6 +1172,23 @@ void survive_data_cb( SurviveUSBInterface * si )
 								acceldata[3], acceldata[4], acceldata[5],
 								0,0,0 };
 
+				agm[0]*=(float)(1./8192.0);
+				agm[1]*=(float)(1./8192.0);
+				agm[2]*=(float)(1./8192.0);
+				calibrate_acc(obj, agm);
+
+				//1G for accelerometer, from MPU6500 datasheet
+				//this can change if the firmware changes the sensitivity.
+
+
+				agm[3]*=(float)((1./32.768)*(3.14159/180.));
+				agm[4]*=(float)((1./32.768)*(3.14159/180.));
+				agm[5]*=(float)((1./32.768)*(3.14159/180.));
+				calibrate_gyro(obj, agm+3);
+
+				//65.5 deg/s for gyroscope, from MPU6500 datasheet
+				//1000 deg/s for gyroscope, from MPU6500 datasheet
+
 				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}
@@ -1048,22 +1219,6 @@ void survive_data_cb( SurviveUSBInterface * si )
 		}
 		break;
 	}
-	case USB_IF_TRACKER0:
-	{
-		SurviveObject * w = obj;
-		if( id == 32 )
-		{
-			// TODO: Looks like this will need to be handle_tracker, since
-			// it appears the interface is sufficiently different.
-			// More work needd to reverse engineer it.
-			handle_watchman( w, readdata);
-		}
-		else
-		{
-			SV_INFO( "Unknown tracker code %d\n", id );
-		}
-		break;
-	}
 	case USB_IF_LIGHTCAP:
 	{
 		int i;
@@ -1073,7 +1228,25 @@ void survive_data_cb( SurviveUSBInterface * si )
 			le.sensor_id = POP1;
 			le.length = POP2;
 			le.timestamp = POP4;
-			if( le.sensor_id == 0xff ) break;
+			if( le.sensor_id > 0xfd ) continue;
+			handle_lightcap( obj, &le );
+		}		
+		break;
+	}
+	case USB_IF_W_WATCHMAN1_LIGHTCAP:
+	case USB_IF_TRACKER0_LIGHTCAP:
+	{
+		int i=0;
+		for( i = 0; i < 7; i++ )
+		{
+			unsigned short *sensorId = (unsigned short *)readdata;
+			unsigned short *length = (unsigned short *)(&(readdata[2]));
+			unsigned long *time = (unsigned long *)(&(readdata[4]));
+			LightcapElement le;
+			le.sensor_id = (uint8_t)POP2;
+			le.length = POP2;
+			le.timestamp = POP4;
+			if( le.sensor_id > 0xfd ) continue;  //
 			handle_lightcap( obj, &le );
 		}		
 		break;
@@ -1199,6 +1372,40 @@ printf( "Loading config: %d\n", len );
 					break;
 				}
 			}
+
+
+			if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->acc_bias = values;
+					so->acc_bias[0] *= .125; //XXX Wat?  Observed by CNL.  Biasing by more than this seems to hose things.
+					so->acc_bias[1] *= .125;
+					so->acc_bias[2] *= .125;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->acc_scale = values;
+				}
+			}
+
+			if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->gyro_bias = values;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->gyro_scale = values;
+				}
+			}
 		}
 	}
 	else
@@ -1238,6 +1445,12 @@ int survive_vive_close( SurviveContext * ctx, void * driver )
 	return 0;
 }
 
+void init_SurviveObject(SurviveObject* so) {
+	so->acc_scale = NULL;
+	so->acc_bias = NULL;
+	so->gyro_scale = NULL;
+	so->gyro_bias = NULL;
+}
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
@@ -1246,8 +1459,15 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) );
 	SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) );
 	SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) );
+	SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) );
 	SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
 
+	init_SurviveObject(hmd);
+	init_SurviveObject(wm0);
+	init_SurviveObject(wm1);
+	init_SurviveObject(tr0);
+	init_SurviveObject(ww0);
+
 	sv->ctx = ctx;
 	
 	#ifdef _WIN32
@@ -1271,32 +1491,50 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	tr0->ctx = ctx;
 	memcpy( tr0->codename, "TR0", 4 );
 	memcpy( tr0->drivername, "HTC", 4 );
+	ww0->ctx = ctx;
+	memcpy( ww0->codename, "WW0", 4 );
+	memcpy( ww0->drivername, "HTC", 4 );
 
 	//USB must happen last.
-	if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0) )
+	if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) )
 	{
 		//TODO: Cleanup any libUSB stuff sitting around.
 		goto fail_gracefully;
 	}
 
 	//Next, pull out the config stuff.
-	if( sv->udev[USB_DEV_HMD]       && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); }
+	if( sv->udev[USB_DEV_HMD_IMU_LH]       && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); }
 	if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); }
 	if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); }
-	if( sv->udev[USB_DEV_TRACKER0]  && LoadConfig( sv, tr0, 4, 0, 1 )) { SV_INFO( "Tracker 0 config issue." ); }
+	if( sv->udev[USB_DEV_TRACKER0]  && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); }
+	if( sv->udev[USB_DEV_W_WATCHMAN1]  && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); }
 
 	hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000;
+	tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz;
+
 	hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000;
+	tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks;
+
 	hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200;
+	tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync;
+
 	hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000;
+	tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time;
+
 	hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800;
+	tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep;
 
 	hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000;
+	tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset;
+
 	hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000;
+	tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack;
 
 	hmd->timecenter_ticks = hmd->timebase_hz / 240;
 	wm0->timecenter_ticks = wm0->timebase_hz / 240;
 	wm1->timecenter_ticks = wm1->timebase_hz / 240;
+	tr0->timecenter_ticks = tr0->timebase_hz / 240;
+	ww0->timecenter_ticks = ww0->timebase_hz / 240;
 /*
 	int i;
 	int locs = hmd->nr_locations;
@@ -1320,10 +1558,11 @@ int DriverRegHTCVive( SurviveContext * ctx )
 */
 
 	//Add the drivers.
-	if( sv->udev[USB_DEV_HMD]       ) { survive_add_object( ctx, hmd ); }
+	if( sv->udev[USB_DEV_HMD_IMU_LH]       ) { survive_add_object( ctx, hmd ); }
 	if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
 	if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); }
 	if( sv->udev[USB_DEV_TRACKER0]  ) { survive_add_object( ctx, tr0 ); }
+	if( sv->udev[USB_DEV_W_WATCHMAN1]  ) { survive_add_object( ctx, ww0 ); }
 
 	survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
 
@@ -1333,6 +1572,7 @@ fail_gracefully:
 	free( wm0 );
 	free( wm1 );
 	free( tr0 );
+	free( ww0 );
 	survive_vive_usb_close( sv );
 	free( sv );
 	return -1;