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

24 files changed, 3394 insertions, 140 deletions

diff --git a/calibrate.c b/calibrate.c
index f27131a..abf592a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -51,36 +51,36 @@ int bufferpts[32*2*3][2];
 char buffermts[32*128*3];
 int buffertimeto[32*3][2];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 //	if( timeinsweep < 0 ) return;
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	if( sensor_id < 0 ) return;
 	if( acode < 0 ) return;
 //return;
 	int jumpoffset = sensor_id;
-	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32;
 	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
 
 
-	if( acode == 0 || acode == 2 ) //data = 0
+	if( acode % 2 == 0 && lh == 0) //data = 0
 	{
 		bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500;
 		buffertimeto[jumpoffset][0] = 0;
 	}
-	if( acode == 1 || acode == 3 ) //data = 1
+	if(  acode % 2 == 1 && lh == 0 ) //data = 1
 	{
 		bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500;
 		buffertimeto[jumpoffset][0] = 0;
 	}
 
 
-	if( acode == 4 || acode == 6 ) //data = 0
+	if( acode % 2 == 0 && lh == 1 ) //data = 0
 	{
 		bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500;
 		buffertimeto[jumpoffset][1] = 0;
 	}
-	if( acode == 5 || acode == 7 ) //data = 1
+	if( acode % 2 == 1 && lh == 1 ) //data = 1
 	{
 		bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500;
 		buffertimeto[jumpoffset][1] = 0;
@@ -99,9 +99,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3
 }
 
 
-void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
 {
-	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh );
 }
 
 char* sensor_name[32];
diff --git a/calibrate_client.c b/calibrate_client.c
index 1a2ee41..abfbabc 100644
--- a/calibrate_client.c
+++ b/calibrate_client.c
@@ -43,9 +43,9 @@ int bufferpts[32*2*3];
 char buffermts[32*128*3];
 int buffertimeto[32*3];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 
 	if( acode == -1 ) return;
 //return;
@@ -90,9 +90,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3
 }
 
 
-void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
 {
-	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh );
 }
 
 
@@ -160,7 +160,7 @@ int main()
 
 //	config_save("config.json");
 */
-	
+
 	ctx = survive_init( 1 );
 
 	survive_install_light_fn( ctx,  my_light_process );
@@ -219,9 +219,12 @@ int main()
 					so = wm0;
 				if( strcmp( dev, "WM1" ) == 0 )
 					so = wm1;
+				uint32_t lh = 0;
+				if (lineptr[0] == 'r')
+					lh = 1;
 
 				if( so )
-					my_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+					my_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh );
 
 				break;
 			}
diff --git a/data_recorder.c b/data_recorder.c
index 46f3427..04a219a 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -43,9 +43,9 @@ int bufferpts[32*2*3];
 char buffermts[32*128*3];
 int buffertimeto[32*3];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 
 	if( acode == -1 ) return;
 //return;
diff --git a/dave/AffineSolve b/dave/AffineSolve
index 7d51b34..bd93cbd 100755
--- a/dave/AffineSolve
+++ b/dave/AffineSolve
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index cf11e0c..497b009 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -32,7 +32,8 @@ typedef struct
 {
 	PoserType pt;
 	int sensor_id;
-	int acode;			//OOTX Code associated with this sweep. base_station = acode >> 2;  axis = acode & 1;
+	int acode;			//OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep
+	int lh;             //Lighthouse making this sweep
 	uint32_t timecode;  //In object-local ticks.
 	FLT length;			//In seconds
 	FLT angle;			//In radians from center of lighthouse.
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index e04586c..278bbca 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -32,9 +32,9 @@ struct SurviveObject
 	PoserCB PoserFn;
 
 	//Device-specific information about the location of the sensors.  This data will be used by the poser.
-	int8_t nr_locations;
-	FLT * sensor_locations;
-	FLT * sensor_normals;
+	int8_t nr_locations; // sensor count
+	FLT * sensor_locations; // size is nr_locations*3.  Contains x,y,z values for each sensor
+	FLT * sensor_normals;// size is nrlocations*3.  cointains normal vector for each sensor
 
 	//Timing sensitive data (mostly for disambiguation)
 	int32_t timebase_hz;		//48,000,000 for normal vive hardware.  (checked)
@@ -47,6 +47,7 @@ struct SurviveObject
 	int32_t pulse_synctime_slack; //5,000 for normal vive hardware.    (guessed)
 
 	//Flood info, for calculating which laser is currently sweeping.
+	void * disambiguator_data;
 	int8_t   oldcode;
 	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault. 
 	int8_t   did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
@@ -129,9 +130,9 @@ void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if
 
 //Call these from your callback if overridden.  
 //Accept higher-level data.
-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);
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
-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 );
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 1600e11..bfd0b1d 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -28,9 +28,9 @@ typedef struct BaseStationData BaseStationData;
 typedef struct SurviveCalData SurviveCalData;   //XXX Warning: This may be removed.  Check at a later time for its defunctness.
 
 typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
-typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
-typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
 
 
 //Device drivers (prefix your drivers with "DriverReg") i.e.
diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c
index c5da925..b1c1eb0 100644
--- a/redist/CNFGWinDriver.c
+++ b/redist/CNFGWinDriver.c
@@ -232,7 +232,7 @@ void CNFGHandleInput()
 		case WM_MBUTTONUP:		HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break;
 		case WM_KEYDOWN:
 		case WM_KEYUP:
-			HandleKey( tolower( msg.wParam ), (msg.message==WM_KEYDOWN) );
+			HandleKey( tolower( (int)(msg.wParam) ), (msg.message==WM_KEYDOWN) );
 			break;
 		default:
 			DispatchMessage(&msg);
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 0267932..29d48bd 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -117,7 +117,8 @@ char* load_file_to_mem(const char* path) {
 	fseek( f, 0, SEEK_END );
 	int len = ftell( f );
 	fseek( f, 0, SEEK_SET );
-	char * JSON_STRING = malloc( len );
+	char * JSON_STRING = malloc( len + 1);
+	memset(JSON_STRING,0,len+1); 
 	fread( JSON_STRING, len, 1, f );
 	fclose( f );
 	return JSON_STRING;
@@ -173,7 +174,7 @@ void json_load_file(const char* path) {
 
 	int16_t children = -1;
 
-	for (i=0; i<(int)items; i+=2)
+	for (i=0; i<(unsigned int)items; i+=2)
 	{
 		//increment i on each successful tag + values combination, not individual tokens
 		jsmntok_t* tag_t = tokens+i;
diff --git a/redist/linmath.c b/redist/linmath.c
index eefcd5f..caff1de 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -82,6 +82,28 @@ FLT anglebetween3d( FLT * a, FLT * b )
 	return FLT_ACOS(dot);
 }
 
+// algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/
+void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle)
+{
+	// TODO: this really should be external.
+	normalize3d(axis, axis);
+
+	FLT s = FLT_SIN(angle);
+	FLT c = FLT_COS(angle);
+
+	FLT u=axis[0];
+	FLT v=axis[1];
+	FLT w=axis[2];
+
+	FLT x=invec3[0];
+	FLT y=invec3[1];
+	FLT z=invec3[2];
+
+	outvec3[0] = u*(u*x + v*y + w*z)*(1-c)   +   x*c   +   (-w*y + v*z)*s;
+	outvec3[1] = v*(u*x + v*y + w*z)*(1-c)   +   y*c   +   ( w*x - u*z)*s;
+	outvec3[2] = w*(u*x + v*y + w*z)*(1-c)   +   z*c   +   (-v*x + u*y)*s;
+}
+
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
@@ -215,7 +237,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 )
 		q[1] = (matrix44[9] - matrix44[6]) / S;
 		q[2] = (matrix44[2] - matrix44[8]) / S;
 		q[3] = (matrix44[4] - matrix44[1]) / S;
-	} else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) {
+	} else if ((matrix44[0] > matrix44[5])&&(matrix44[0] > matrix44[10])) {
 		FLT S = FLT_SQRT(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2.; // S=4*qx
 		q[0] = (matrix44[9] - matrix44[6]) / S;
 		q[1] = 0.25f * S;
diff --git a/redist/linmath.h b/redist/linmath.h
index 4e0cb77..6f0bf60 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -70,6 +70,7 @@ FLT magnitude3d(const FLT * a );
 
 FLT anglebetween3d( FLT * a, FLT * b );
 
+void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
 //Quaternion things...
 
 void quatsetnone( FLT * q );
diff --git a/redist/svd.h b/redist/svd.h
new file mode 100644
index 0000000..41708da
--- /dev/null
+++ b/redist/svd.h
@@ -0,0 +1,450 @@
+/**************************************************************************
+**
+**  svd3
+**
+** Quick singular value decomposition as described by:
+** A. McAdams, A. Selle, R. Tamstorf, J. Teran and E. Sifakis,
+** "Computing the Singular Value Decomposition of 3x3 matrices
+** with minimal branching and elementary floating point operations",
+**  University of Wisconsin - Madison technical report TR1690, May 2011
+**
+**	OPTIMIZED CPU VERSION
+** 	Implementation by: Eric Jang
+**
+**  13 Apr 2014
+**
+** This file originally retrieved from:
+** https://github.com/ericjang/svd3/blob/master/svd3.h  3/26/2017
+**
+** Original licesnse is MIT per:
+** https://github.com/ericjang/svd3/blob/master/LICENSE.md
+**
+** Ported from C++ to C by Mike Turvey.  All modifications also released 
+** under an MIT license.
+**************************************************************************/
+
+
+#ifndef SVD3_H
+#define SVD3_H
+
+#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3;
+#define _cstar 0.923879532 // cos(pi/8)
+#define _sstar 0.3826834323 // sin(p/8)
+#define EPSILON 1e-6
+
+#include <math.h>
+
+/* This is a novel and fast routine for the reciprocal square root of an
+IEEE float (single precision).
+http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf
+http://playstation2-linux.com/download/p2lsd/fastrsqrt.pdf
+http://www.beyond3d.com/content/articles/8/
+*/
+inline float rsqrt(float x) {
+	// int ihalf = *(int *)&x - 0x00800000; // Alternative to next line,
+	// float xhalf = *(float *)&ihalf;      // for sufficiently large nos.
+	float xhalf = 0.5f*x;
+	int i = *(int *)&x;          // View x as an int.
+								 // i = 0x5f3759df - (i >> 1);   // Initial guess (traditional).
+	i = 0x5f375a82 - (i >> 1);   // Initial guess (slightly better).
+	x = *(float *)&i;            // View i as float.
+	x = x*(1.5f - xhalf*x*x);    // Newton step.
+								 // x = x*(1.5008908 - xhalf*x*x);  // Newton step for a balanced error.
+	return x;
+}
+
+/* This is rsqrt with an additional step of the Newton iteration, for
+increased accuracy. The constant 0x5f37599e makes the relative error
+range from 0 to -0.00000463.
+You can't balance the error by adjusting the constant. */
+inline float rsqrt1(float x) {
+	float xhalf = 0.5f*x;
+	int i = *(int *)&x;          // View x as an int.
+	i = 0x5f37599e - (i >> 1);   // Initial guess.
+	x = *(float *)&i;            // View i as float.
+	x = x*(1.5f - xhalf*x*x);    // Newton step.
+	x = x*(1.5f - xhalf*x*x);    // Newton step again.
+	return x;
+}
+
+inline float accurateSqrt(float x)
+{
+	return x * rsqrt1(x);
+}
+
+inline void condSwap(bool c, float *X, float *Y)
+{
+	// used in step 2
+	float Z = *X;
+	*X = c ? *Y : *X;
+	*Y = c ? Z : *Y;
+}
+
+inline void condNegSwap(bool c, float *X, float *Y)
+{
+	// used in step 2 and 3
+	float Z = -*X;
+	*X = c ? *Y : *X;
+	*Y = c ? Z : *Y;
+}
+
+// matrix multiplication M = A * B
+inline void multAB(float a11, float a12, float a13,
+	float a21, float a22, float a23,
+	float a31, float a32, float a33,
+	//
+	float b11, float b12, float b13,
+	float b21, float b22, float b23,
+	float b31, float b32, float b33,
+	//
+	float *m11, float *m12, float *m13,
+	float *m21, float *m22, float *m23,
+	float *m31, float *m32, float *m33)
+{
+
+	*m11 = a11*b11 + a12*b21 + a13*b31; *m12 = a11*b12 + a12*b22 + a13*b32; *m13 = a11*b13 + a12*b23 + a13*b33;
+	*m21 = a21*b11 + a22*b21 + a23*b31; *m22 = a21*b12 + a22*b22 + a23*b32; *m23 = a21*b13 + a22*b23 + a23*b33;
+	*m31 = a31*b11 + a32*b21 + a33*b31; *m32 = a31*b12 + a32*b22 + a33*b32; *m33 = a31*b13 + a32*b23 + a33*b33;
+}
+
+// matrix multiplication M = Transpose[A] * B
+inline void multAtB(float a11, float a12, float a13,
+	float a21, float a22, float a23,
+	float a31, float a32, float a33,
+	//
+	float b11, float b12, float b13,
+	float b21, float b22, float b23,
+	float b31, float b32, float b33,
+	//
+	float *m11, float *m12, float *m13,
+	float *m21, float *m22, float *m23,
+	float *m31, float *m32, float *m33)
+{
+	*m11 = a11*b11 + a21*b21 + a31*b31; *m12 = a11*b12 + a21*b22 + a31*b32; *m13 = a11*b13 + a21*b23 + a31*b33;
+	*m21 = a12*b11 + a22*b21 + a32*b31; *m22 = a12*b12 + a22*b22 + a32*b32; *m23 = a12*b13 + a22*b23 + a32*b33;
+	*m31 = a13*b11 + a23*b21 + a33*b31; *m32 = a13*b12 + a23*b22 + a33*b32; *m33 = a13*b13 + a23*b23 + a33*b33;
+}
+
+inline void quatToMat3(const float * qV,
+	float *m11, float *m12, float *m13,
+	float *m21, float *m22, float *m23,
+	float *m31, float *m32, float *m33
+)
+{
+	float w = qV[3];
+	float x = qV[0];
+	float y = qV[1];
+	float z = qV[2];
+
+	float qxx = x*x;
+	float qyy = y*y;
+	float qzz = z*z;
+	float qxz = x*z;
+	float qxy = x*y;
+	float qyz = y*z;
+	float qwx = w*x;
+	float qwy = w*y;
+	float qwz = w*z;
+
+	*m11 = 1 - 2 * (qyy + qzz); *m12 = 2 * (qxy - qwz); *m13 = 2 * (qxz + qwy);
+	*m21 = 2 * (qxy + qwz); *m22 = 1 - 2 * (qxx + qzz); *m23 = 2 * (qyz - qwx);
+	*m31 = 2 * (qxz - qwy); *m32 = 2 * (qyz + qwx); *m33 = 1 - 2 * (qxx + qyy);
+}
+
+inline void approximateGivensQuaternion(float a11, float a12, float a22, float *ch, float *sh)
+{
+	/*
+	* Given givens angle computed by approximateGivensAngles,
+	* compute the corresponding rotation quaternion.
+	*/
+	*ch = 2 * (a11 - a22);
+	*sh = a12;
+	bool b = _gamma* (*sh)*(*sh) < (*ch)*(*ch);
+	// fast rsqrt function suffices
+	// rsqrt2 (https://code.google.com/p/lppython/source/browse/algorithm/HDcode/newCode/rsqrt.c?r=26)
+	// is even faster but results in too much error
+	float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh));
+	*ch = b ? w*(*ch) : (float)_cstar;
+	*sh = b ? w*(*sh) : (float)_sstar;
+}
+
+inline void jacobiConjugation(const int x, const int y, const int z,
+	float *s11,
+	float *s21, float *s22,
+	float *s31, float *s32, float *s33,
+	float * qV)
+{
+	float ch, sh;
+	approximateGivensQuaternion(*s11, *s21, *s22, &ch, &sh);
+
+	float scale = ch*ch + sh*sh;
+	float a = (ch*ch - sh*sh) / scale;
+	float b = (2 * sh*ch) / scale;
+
+	// make temp copy of S
+	float _s11 = *s11;
+	float _s21 = *s21; float _s22 = *s22;
+	float _s31 = *s31; float _s32 = *s32; float _s33 = *s33;
+
+	// perform conjugation S = Q'*S*Q
+	// Q already implicitly solved from a, b
+	*s11 = a*(a*_s11 + b*_s21) + b*(a*_s21 + b*_s22);
+	*s21 = a*(-b*_s11 + a*_s21) + b*(-b*_s21 + a*_s22);	*s22 = -b*(-b*_s11 + a*_s21) + a*(-b*_s21 + a*_s22);
+	*s31 = a*_s31 + b*_s32;								*s32 = -b*_s31 + a*_s32; *s33 = _s33;
+
+	// update cumulative rotation qV
+	float tmp[3];
+	tmp[0] = qV[0] * sh;
+	tmp[1] = qV[1] * sh;
+	tmp[2] = qV[2] * sh;
+	sh *= qV[3];
+
+	qV[0] *= ch;
+	qV[1] *= ch;
+	qV[2] *= ch;
+	qV[3] *= ch;
+
+	// (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1))
+	// for (p,q) = ((0,1),(1,2),(0,2))
+	qV[z] += sh;
+	qV[3] -= tmp[z]; // w
+	qV[x] += tmp[y];
+	qV[y] -= tmp[x];
+
+	// re-arrange matrix for next iteration
+	_s11 = *s22;
+	_s21 = *s32; _s22 = *s33;
+	_s31 = *s21; _s32 = *s31; _s33 = *s11;
+	*s11 = _s11;
+	*s21 = _s21; *s22 = _s22;
+	*s31 = _s31; *s32 = _s32; *s33 = _s33;
+
+}
+
+inline float dist2(float x, float y, float z)
+{
+	return x*x + y*y + z*z;
+}
+
+// finds transformation that diagonalizes a symmetric matrix
+inline void jacobiEigenanlysis( // symmetric matrix
+	float *s11,
+	float *s21, float *s22,
+	float *s31, float *s32, float *s33,
+	// quaternion representation of V
+	float * qV)
+{
+	qV[3] = 1; qV[0] = 0; qV[1] = 0; qV[2] = 0; // follow same indexing convention as GLM
+	for (int i = 0; i<4; i++)
+	{
+		// we wish to eliminate the maximum off-diagonal element
+		// on every iteration, but cycling over all 3 possible rotations
+		// in fixed order (p,q) = (1,2) , (2,3), (1,3) still retains
+		//  asymptotic convergence
+		jacobiConjugation(0, 1, 2, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,1
+		jacobiConjugation(1, 2, 0, s11, s21, s22, s31, s32, s33, qV); // p,q = 1,2
+		jacobiConjugation(2, 0, 1, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,2
+	}
+}
+
+
+inline void sortSingularValues(// matrix that we want to decompose
+	float *b11, float *b12, float *b13,
+	float *b21, float *b22, float *b23,
+	float *b31, float *b32, float *b33,
+	// sort V simultaneously
+	float *v11, float *v12, float *v13,
+	float *v21, float *v22, float *v23,
+	float *v31, float *v32, float *v33)
+{
+	float rho1 = dist2(*b11, *b21, *b31);
+	float rho2 = dist2(*b12, *b22, *b32);
+	float rho3 = dist2(*b13, *b23, *b33);
+	bool c;
+	c = rho1 < rho2;
+	condNegSwap(c, b11, b12); condNegSwap(c, v11, v12);
+	condNegSwap(c, b21, b22); condNegSwap(c, v21, v22);
+	condNegSwap(c, b31, b32); condNegSwap(c, v31, v32);
+	condSwap(c, &rho1, &rho2);
+	c = rho1 < rho3;
+	condNegSwap(c, b11, b13); condNegSwap(c, v11, v13);
+	condNegSwap(c, b21, b23); condNegSwap(c, v21, v23);
+	condNegSwap(c, b31, b33); condNegSwap(c, v31, v33);
+	condSwap(c, &rho1, &rho3);
+	c = rho2 < rho3;
+	condNegSwap(c, b12, b13); condNegSwap(c, v12, v13);
+	condNegSwap(c, b22, b23); condNegSwap(c, v22, v23);
+	condNegSwap(c, b32, b33); condNegSwap(c, v32, v33);
+}
+
+
+void QRGivensQuaternion(float a1, float a2, float *ch, float *sh)
+{
+	// a1 = pivot point on diagonal
+	// a2 = lower triangular entry we want to annihilate
+	float epsilon = (float)EPSILON;
+	float rho = accurateSqrt(a1*a1 + a2*a2);
+
+	*sh = rho > epsilon ? a2 : 0;
+	*ch = fabsf(a1) + fmaxf(rho, epsilon);
+	bool b = a1 < 0;
+	condSwap(b, sh, ch);
+	float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh));
+	*ch *= w;
+	*sh *= w;
+}
+
+
+inline void QRDecomposition(// matrix that we want to decompose
+	float b11, float b12, float b13,
+	float b21, float b22, float b23,
+	float b31, float b32, float b33,
+	// output Q
+	float *q11, float *q12, float *q13,
+	float *q21, float *q22, float *q23,
+	float *q31, float *q32, float *q33,
+	// output R
+	float *r11, float *r12, float *r13,
+	float *r21, float *r22, float *r23,
+	float *r31, float *r32, float *r33)
+{
+	float ch1, sh1, ch2, sh2, ch3, sh3;
+	float a, b;
+
+	// first givens rotation (ch,0,0,sh)
+	QRGivensQuaternion(b11, b21, &ch1, &sh1);
+	a = 1 - 2 * sh1*sh1;
+	b = 2 * ch1*sh1;
+	// apply B = Q' * B
+	*r11 = a*b11 + b*b21;  *r12 = a*b12 + b*b22;  *r13 = a*b13 + b*b23;
+	*r21 = -b*b11 + a*b21; *r22 = -b*b12 + a*b22; *r23 = -b*b13 + a*b23;
+	*r31 = b31;          *r32 = b32;          *r33 = b33;
+
+	// second givens rotation (ch,0,-sh,0)
+	QRGivensQuaternion(*r11, *r31, &ch2, &sh2);
+	a = 1 - 2 * sh2*sh2;
+	b = 2 * ch2*sh2;
+	// apply B = Q' * B;
+	b11 = a*(*r11) + b*(*r31);  b12 = a*(*r12) + b*(*r32);  b13 = a*(*r13) + b*(*r33);
+	b21 = *r21;           b22 = *r22;           b23 = *r23;
+	b31 = -b*(*r11) + a*(*r31); b32 = -b*(*r12) + a*(*r32); b33 = -b*(*r13) + a*(*r33);
+
+	// third givens rotation (ch,sh,0,0)
+	QRGivensQuaternion(b22, b32, &ch3, &sh3);
+	a = 1 - 2 * sh3*sh3;
+	b = 2 * ch3*sh3;
+	// R is now set to desired value
+	*r11 = b11;               *r12 = b12;             *r13 = b13;
+	*r21 = a*b21 + b*b31;     *r22 = a*b22 + b*b32;   *r23 = a*b23 + b*b33;
+	*r31 = -b*b21 + a*b31;    *r32 = -b*b22 + a*b32;  *r33 = -b*b23 + a*b33;
+
+	// construct the cumulative rotation Q=Q1 * Q2 * Q3
+	// the number of floating point operations for three quaternion multiplications
+	// is more or less comparable to the explicit form of the joined matrix.
+	// certainly more memory-efficient!
+	float sh12 = sh1*sh1;
+	float sh22 = sh2*sh2;
+	float sh32 = sh3*sh3;
+
+	*q11 = (-1 + 2 * sh12)*(-1 + 2 * sh22);
+	*q12 = 4 * ch2*ch3*(-1 + 2 * sh12)*sh2*sh3 + 2 * ch1*sh1*(-1 + 2 * sh32);
+	*q13 = 4 * ch1*ch3*sh1*sh3 - 2 * ch2*(-1 + 2 * sh12)*sh2*(-1 + 2 * sh32);
+
+	*q21 = 2 * ch1*sh1*(1 - 2 * sh22);
+	*q22 = -8 * ch1*ch2*ch3*sh1*sh2*sh3 + (-1 + 2 * sh12)*(-1 + 2 * sh32);
+	*q23 = -2 * ch3*sh3 + 4 * sh1*(ch3*sh1*sh3 + ch1*ch2*sh2*(-1 + 2 * sh32));
+
+	*q31 = 2 * ch2*sh2;
+	*q32 = 2 * ch3*(1 - 2 * sh22)*sh3;
+	*q33 = (-1 + 2 * sh22)*(-1 + 2 * sh32);
+}
+
+void svd(// input A
+	float a11, float a12, float a13,
+	float a21, float a22, float a23,
+	float a31, float a32, float a33,
+	// output U
+	float *u11, float *u12, float *u13,
+	float *u21, float *u22, float *u23,
+	float *u31, float *u32, float *u33,
+	// output S
+	float *s11, float *s12, float *s13,
+	float *s21, float *s22, float *s23,
+	float *s31, float *s32, float *s33,
+	// output V
+	float *v11, float *v12, float *v13,
+	float *v21, float *v22, float *v23,
+	float *v31, float *v32, float *v33)
+{
+	// normal equations matrix
+	float ATA11, ATA12, ATA13;
+	float ATA21, ATA22, ATA23;
+	float ATA31, ATA32, ATA33;
+
+	multAtB(a11, a12, a13, a21, a22, a23, a31, a32, a33,
+		a11, a12, a13, a21, a22, a23, a31, a32, a33,
+		&ATA11, &ATA12, &ATA13, &ATA21, &ATA22, &ATA23, &ATA31, &ATA32, &ATA33);
+
+	// symmetric eigenalysis
+	float qV[4];
+	jacobiEigenanlysis(&ATA11, &ATA21, &ATA22, &ATA31, &ATA32, &ATA33, qV);
+	quatToMat3(qV, v11, v12, v13, v21, v22, v23, v31, v32, v33);
+
+	float b11, b12, b13;
+	float b21, b22, b23;
+	float b31, b32, b33;
+	multAB(a11, a12, a13, a21, a22, a23, a31, a32, a33,
+		*v11, *v12, *v13, *v21, *v22, *v23, *v31, *v32, *v33,
+		&b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33);
+
+	// sort singular values and find V
+	sortSingularValues(&b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33,
+		v11, v12, v13, v21, v22, v23, v31, v32, v33);
+
+	// QR decomposition
+	QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33,
+		u11, u12, u13, u21, u22, u23, u31, u32, u33,
+		s11, s12, s13, s21, s22, s23, s31, s32, s33
+	);
+}
+
+/// polar decomposition can be reconstructed trivially from SVD result
+// A = UP
+void pd(float a11, float a12, float a13,
+	float a21, float a22, float a23,
+	float a31, float a32, float a33,
+	// output U
+	float *u11, float *u12, float *u13,
+	float *u21, float *u22, float *u23,
+	float *u31, float *u32, float *u33,
+	// output P
+	float *p11, float *p12, float *p13,
+	float *p21, float *p22, float *p23,
+	float *p31, float *p32, float *p33)
+{
+	float w11, w12, w13, w21, w22, w23, w31, w32, w33;
+	float s11, s12, s13, s21, s22, s23, s31, s32, s33;
+	float v11, v12, v13, v21, v22, v23, v31, v32, v33;
+
+	svd(a11, a12, a13, a21, a22, a23, a31, a32, a33,
+		&w11, &w12, &w13, &w21, &w22, &w23, &w31, &w32, &w33,
+		&s11, &s12, &s13, &s21, &s22, &s23, &s31, &s32, &s33,
+		&v11, &v12, &v13, &v21, &v22, &v23, &v31, &v32, &v33);
+
+	// P = VSV'
+	float t11, t12, t13, t21, t22, t23, t31, t32, t33;
+	multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33,
+		s11, s12, s13, s21, s22, s23, s31, s32, s33,
+		&t11, &t12, &t13, &t21, &t22, &t23, &t31, &t32, &t33);
+
+	multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33,
+		v11, v21, v31, v12, v22, v32, v13, v23, v33,
+		p11, p12, p13, p21, p22, p23, p31, p32, p33);
+
+	// U = WV'
+	multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33,
+		v11, v21, v31, v12, v22, v32, v13, v23, v33,
+		u11, u12, u13, u21, u22, u23, u31, u32, u33);
+}
+
+#endif
\ No newline at end of file
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 80f65a9..906b21e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -456,41 +456,73 @@ PRINT(ab,2,1);
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-
-	PRINT_MAT(T,4,4);
-matrix44transpose(T2, T);
-//matrix44copy(T2,T);
-	cross3d( &T2[0][0], &T2[1][0], &T2[2][0] );
-	cross3d( &T2[2][0], &T2[0][0], &T2[1][0] );
-	normalize3d( &T2[0][0], &T2[0][0] );
-	normalize3d( &T2[1][0], &T2[1][0] );
-	normalize3d( &T2[2][0], &T2[2][0] );
-//matrix44copy(T2,T);
-matrix44transpose(T, T2);
 	PRINT_MAT(T,4,4);
 
+#if 1
+//	matrix44transpose(T2, T);  //Transpose so we are
+	matrix44copy(T2,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(T,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
 
-	//memcpy(T2,T,16*sizeof(float));
-//	matrix44copy(T2,T);
+	//matrix44copy(T2,T);
 	matrix44transpose(T2,T);
+
 	quatfrommatrix( quat, &T2[0][0] );
+	printf( "QM: %f\n", quatmagnitude( quat ) );
 	quatnormalize(quatNorm,quat);
 	quattoeuler(euler,quatNorm);
-	quattomatrix( T2, 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);
+//	PRINT(T,4,4);
 
 //	matrix44copy(temp,T2);
 	matrix44transpose(temp,T2);
 
-	memcpy(T2,temp,16*sizeof(float));
+
+//	matrix44transpose(T2, temp);
+//	memcpy(T2,temp,16*sizeof(float));
 	for (i=0; i<3; i++) {
 		for (j=0; j<3; j++) {
-			T[i][j] = T2[j][i];
+			T[i][j] = temp[i][j];
 		}
 	}
-	PRINT(T2,4,4);
+
+/*	PRINT(T2,4,4); */
+#endif
+
+	T[1][0]*=-1;
+	T[1][1]*=-1;
+	T[1][2]*=-1;
 
 
 /*
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..c251040
--- /dev/null
+++ b/src/poser_turveytori.c
@@ -0,0 +1,1604 @@
+#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];
+} 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;
+
+	Point tmpXplus = pointIn;
+	Point tmpXminus = pointIn;
+	tmpXplus.x = pointIn.x + precision;
+	tmpXminus.x = pointIn.x - precision;
+	result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0);
+
+	Point tmpYplus = pointIn;
+	Point tmpYminus = pointIn;
+	tmpYplus.y = pointIn.y + precision;
+	tmpYminus.y = pointIn.y - precision;
+	result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0);
+
+	Point tmpZplus = pointIn;
+	Point tmpZminus = pointIn;
+	tmpZplus.z = pointIn.z + precision;
+	tmpZminus.z = pointIn.z - precision;
+	result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - 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)
+{
+	//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);
+
+	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!
+	SolveForRotation(rot, obj, refinedEstimateGd);
+	FLT objPos[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);
+	}
+
+	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 % 2 == 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 04931cc..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,7 +218,7 @@ 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.
@@ -202,10 +227,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		else if( acode < -4 ) break;
 		int lh = (-acode) - 3;
 
-		if( strcmp( so->codename, "WM0" ) == 0 )
-			sensor_id += 32;
-		if( strcmp( so->codename, "WM1" ) == 0 )
-			sensor_id += 64;
+		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;
@@ -213,9 +241,10 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 
 	}
 	
+
 }
 
-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;
@@ -223,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 )
@@ -274,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;
@@ -552,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;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 45b77f6..ae644d1 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -29,17 +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 32  //Number of samples required in collection phase.
 
-#define POSE_OBJECTS 3
+#define MAX_POSE_OBJECTS 10
 
 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
@@ -69,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..005cfaf 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;
@@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) {
 	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 0873f7f..3f16c7c 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,6 +5,311 @@
 #include <stdint.h>
 #include <string.h>
 
+typedef struct
+{
+	unsigned int sweep_time[SENSORS_PER_OBJECT];
+	unsigned int sweep_len[SENSORS_PER_OBJECT];
+} 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 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;
+}
+void handle_lightcap2_process_sweep_data(SurviveObject *so)
+{
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	// 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->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
+			{
+
+				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");
+
+	}
+	// 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);
+
+	// 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 (!(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
+		{
+			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.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;
+
+		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)
+	{
+		// 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_pulse_len[0] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-1,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]),
+					lcd->per_sweep.lh_pulse_len[0],
+					lcd->per_sweep.lh_start_time[0],
+					0,
+					0);
+			}
+			if (lcd->per_sweep.lh_pulse_len[1] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-2,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]),
+					lcd->per_sweep.lh_pulse_len[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; 
+
+
+
+		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;
+
+		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;
+		}
+	}
+}
+
+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, ".");
+
+	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)
+	{
+		// 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);
+
+}
 
 int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	//+50 adds a small offset and seems to help always get it right. 
@@ -19,7 +324,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 //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;	
+	SurviveContext * ctx = so->ctx;
+	handle_lightcap2(so,le);
+	return;
+
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 	if( le->sensor_id > SENSORS_PER_OBJECT )
@@ -120,7 +428,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			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 );
-			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
 		}
 	}
 
@@ -160,8 +468,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			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] );
+			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 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];
 
@@ -204,7 +512,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number );
 		}
 	}
 	else
diff --git a/src/survive_process.c b/src/survive_process.c
index 9295638..3af2da9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,14 +6,14 @@
 //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.
@@ -37,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 )
 	{
@@ -57,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 f6465b2..5224728 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -47,6 +47,7 @@ const short vidpids[] = {
 	0x28de, 0x2012, 0, //Valve Watchman, USB connected
 #ifdef HIDAPI
 	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
@@ -60,35 +61,38 @@ const char * devnames[] = {
 	"Wired Watchman 1",
 #ifdef HIDAPI
 	"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 6
-#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7
-#define MAX_USB_DEVS		8
+#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		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_W_WATCHMAN1	5
 #define USB_IF_LIGHTCAP		6
-#define USB_IF_W_WATCHMAN1_LIGHTCAP		7
-#define MAX_INTERFACES		8
+#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;
@@ -340,7 +344,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 			if (cur_dev->vendor_id == vendor_id &&
 				cur_dev->product_id == product_id)
 			{
-				if( menum == enumid )
+				if( cur_dev->interface_number == enumid )
 				{
 					path_to_open = cur_dev->path;
 					break;
@@ -467,17 +471,22 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 #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_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." );
@@ -508,17 +517,37 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 			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_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			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_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			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 };
@@ -530,23 +559,57 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 			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_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_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+		}
+
+//#endif
+
 #if 0
 		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;
-		}
+		//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." );
 	}
@@ -771,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
-	int i;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
@@ -848,10 +910,9 @@ static void handle_watchman( 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] );
 		}
@@ -1042,8 +1103,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 );
@@ -1095,22 +1157,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_wired_watchman( w, readdata, size);
-		}
-		else
-		{
-			SV_INFO( "Unknown tracker code %d\n", id );
-		}
-		break;
-	}
 	case USB_IF_LIGHTCAP:
 	{
 		int i;
@@ -1126,6 +1172,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		break;
 	}
 	case USB_IF_W_WATCHMAN1_LIGHTCAP:
+	case USB_IF_TRACKER0_LIGHTCAP:
 	{
 		int i=0;
 		for( i = 0; i < 7; i++ )
@@ -1134,7 +1181,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 			unsigned short *length = (unsigned short *)(&(readdata[2]));
 			unsigned long *time = (unsigned long *)(&(readdata[4]));
 			LightcapElement le;
-			le.sensor_id = POP2;
+			le.sensor_id = (uint8_t)POP2;
 			le.length = POP2;
 			le.timestamp = POP4;
 			if( le.sensor_id == 0xff ) break;
@@ -1348,7 +1395,7 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	}
 
 	//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, 0 )) { SV_INFO( "Tracker 0 config issue." ); }
@@ -1403,7 +1450,7 @@ 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 ); }
diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json
new file mode 100644
index 0000000..e12594a
--- /dev/null
+++ b/useful_files/sample.config.json
@@ -0,0 +1,8 @@
+"_Comment0":"This file must be named config.json and placed in the same directory as the executable",
+"_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori",
+"DefaultPoser":"PoserTurveyTori",
+"ConfigPoser":"PoserTurveyTori",
+"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass.  Valid options are: HMD,WM0,WM1,TR0,WW0",
+"RequiredTrackersForCal":"",
+"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration.  Otherwise only required trackers will be used.",
+"AllowAllTrackersForCal":"1",
diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj
index 643cff5..c794382 100644
--- a/winbuild/libsurvive/libsurvive.vcxproj
+++ b/winbuild/libsurvive/libsurvive.vcxproj
@@ -153,6 +153,8 @@
     <ClCompile Include="..\..\src\poser_charlesslow.c" />
     <ClCompile Include="..\..\src\poser_daveortho.c" />
     <ClCompile Include="..\..\src\poser_dummy.c" />
+    <ClCompile Include="..\..\src\poser_octavioradii.c" />
+    <ClCompile Include="..\..\src\poser_turveytori.c" />
     <ClCompile Include="..\..\src\survive.c" />
     <ClCompile Include="..\..\src\survive_cal.c" />
     <ClCompile Include="..\..\src\survive_config.c" />
diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters
index 0bb9d1b..e7d44e2 100644
--- a/winbuild/libsurvive/libsurvive.vcxproj.filters
+++ b/winbuild/libsurvive/libsurvive.vcxproj.filters
@@ -84,6 +84,12 @@
     <ClCompile Include="..\..\redist\CNFGWinDriver.c">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="..\..\src\poser_turveytori.c">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\src\poser_octavioradii.c">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\..\src\ootx_decoder.h">