3 files changed, 141 insertions, 72 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index c251040..7abf5d0 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -83,6 +83,9 @@ typedef struct
 	FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
 	int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
 	int lastAxis[NUM_LIGHTHOUSES];
+
+	Point lastLhPos[NUM_LIGHTHOUSES];
+	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
 } ToriData;
 
 
@@ -433,23 +436,25 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci
 {
 	Point result;
 
+	FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0);
+
 	Point tmpXplus = pointIn;
 	Point tmpXminus = pointIn;
 	tmpXplus.x = pointIn.x + precision;
 	tmpXminus.x = pointIn.x - precision;
-	result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0);
+	result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0);
 
 	Point tmpYplus = pointIn;
 	Point tmpYminus = pointIn;
 	tmpYplus.y = pointIn.y + precision;
 	tmpYminus.y = pointIn.y - precision;
-	result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0);
+	result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0);
 
 	Point tmpZplus = pointIn;
 	Point tmpZminus = pointIn;
 	tmpZplus.z = pointIn.z + precision;
 	tmpZminus.z = pointIn.z - precision;
-	result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0);
+	result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0);
 
 	return result;
 }
@@ -1144,6 +1149,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration)
 {
+	ToriData *toriData = so->PoserData;
+
 	//printf("Solving for Lighthouse\n");
 
 	//printf("obj->numSensors = %d;\n", obj->numSensors);
@@ -1234,6 +1241,14 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
 	Point p1 = getNormalizedAndScaledVector(avgNorm, 8);
 
+	// if the last lighthouse position has been populated (extremely rare it would be 0)
+	if (toriData->lastLhPos[lh].x != 0)
+	{
+		p1.x = toriData->lastLhPos[lh].x;
+		p1.y = toriData->lastLhPos[lh].y;
+		p1.z = toriData->lastLhPos[lh].z;
+	}
+
 	Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
 
 	FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
@@ -1258,11 +1273,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
 	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+
+	if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+	{
+		rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+		rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+		rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+	}
+
+
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
 
+	{
+		toriData->lastLhRotAxisAngle[lh][0] = rot[0];
+		toriData->lastLhRotAxisAngle[lh][1] = rot[1];
+		toriData->lastLhRotAxisAngle[lh][2] = rot[2];
+		toriData->lastLhRotAxisAngle[lh][3] = rot[3];
+	}
+
 	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
 
+
 	FLT rotQuat[4];
 
 	quatfromaxisangle(rotQuat, rot, rot[3]);
@@ -1325,6 +1358,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		fclose(logFile);
 	}
 
+
+	toriData->lastLhPos[lh].x = refinedEstimateGd.x;
+	toriData->lastLhPos[lh].y = refinedEstimateGd.y;
+	toriData->lastLhPos[lh].z = refinedEstimateGd.z;
+
 	return refinedEstimateGd;
 }
 
@@ -1482,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 				counter++;
 
 				// let's just do this occasionally for now...
-				if (counter % 2 == 0)
+				if (counter % 4 == 0)
 					QuickPose(so, 0);
 			}
 			// axis changed, time to increment the circular buffer index.
diff --git a/src/survive_data.c b/src/survive_data.c
index 22ce8c2..df8df8e 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -4,9 +4,9 @@
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
-#include <math.h>
+#include <math.h> /* for sqrt */
 
-#define USE_TURVEYBIGUATOR
+//#define USE_TURVEYBIGUATOR
 
 #ifdef USE_TURVEYBIGUATOR
 
@@ -76,6 +76,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 }
 
 uint8_t remove_outliers(SurviveObject *so) {
+	return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker.
 	lightcap2_data *lcd = so->disambiguator_data;
 
 	uint32_t sum = 0;
@@ -470,13 +471,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * 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. 
 	//Check the +50 in the future to see how well this works on a variety of hardware.
-
+	if( !main_divisor ) return -1;
 	int32_t acode = (length+main_divisor+50)/(main_divisor*2);
 	if( acode & 1 ) return -1;
 
 	return (acode>>1) - 6;
 }
 
+
+void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
+{
+	int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+	int32_t acode_array[2] =
+		{
+			decode_acode(so->last_sync_length[0],main_divisor),
+			decode_acode(so->last_sync_length[1],main_divisor)
+		};
+
+
+	int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
+	int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
+
+	//printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
+	if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+	if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
+
+	so->recent_sync_time = so->last_sync_time[1];
+
+/*
+	//Throw out everything if our sync pulses look like they're bad.
+	//This actually doesn't seem to hold anymore, now that we're looking for multiple LHs.
+	int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+	int32_t center_2 = so->pulse_synctime_offset;
+	int32_t slack = so->pulse_synctime_slack;
+
+	if( delta1 < center_1 - slack || delta1 > center_1 + slack )
+	{
+		//XXX: TODO: Count faults.
+		so->sync_set_number = -1;
+		return;
+	}
+
+	if( delta2 < center_2 - slack || delta2 > center_2 + slack )
+	{
+		//XXX: TODO: Count faults.
+		so->sync_set_number = -1;
+		return;
+	}
+*/
+	so->did_handle_ootx = 1;
+}
+		
+
 //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 )
 {
@@ -487,6 +534,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	return;
 
 #else
+	//printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp );
 
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
@@ -520,6 +568,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	//unified driver.
 	int ssn = so->sync_set_number; //lighthouse number
 	if( ssn < 0 ) ssn = 0;
+#ifdef DEBUG
+	if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); }
+#endif
 	int last_sync_time  =  so->last_sync_time  [ssn];
 	int last_sync_length = so->last_sync_length[ssn];
 	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
@@ -537,15 +588,37 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
+		//TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync
+		//information to the rest of libsurvive.  This could be turned into a function and combined with the code below.
+		if( !so->did_handle_ootx && is_new_pulse )
+		{
+			HandleOOTX( ctx, so );
+		}
 		so->did_handle_ootx = 0;
 
+
+		//printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
+
 		if( is_new_pulse )
 		{
 			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
+			int is_pulse_from_same_lh_as_last_sweep;
+			int tp = delta % ( so->timecenter_ticks * 2);
+			is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
 
-			if( is_master_sync_pulse )
+			if( is_master_sync_pulse )  //Could also be called by slave if no master was seen.
 			{
-				ssn = so->sync_set_number = 0;
+				ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one.
+				if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; }
+				if( ssn != 0 )
+				{
+					//If it's the slave that is repeated, be sure to zero out its sync info.
+					so->last_sync_length[0] = 0;
+				}
+				else
+				{
+					so->last_sync_length[1] = 0;
+				}
 				so->last_sync_time[ssn] = le->timestamp;
 				so->last_sync_length[ssn] = le->length;
 			}
@@ -582,15 +655,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			}
 		}
 
-		//Extra tidbit for storing length-of-sync-pulses.
+#if 0
+		//Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
+		//We don't actually use this anywhere, and I doubt we ever will?  Though, it could be useful at a later time to improve tracking.
 		{
 			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
 			int base_station = is_new_pulse;
-			//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
-			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
+			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here.
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here.
 		}
+#endif
 	}
 
+	//Any else- statements below here are 
+
 	//See if this is a valid actual pulse.
 	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
@@ -607,68 +685,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure it fits nicely into a divisible-by-500 time.
 
 		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
-
-		int32_t acode_array[2] =
-			{
-				decode_acode(so->last_sync_length[0],main_divisor),
-				decode_acode(so->last_sync_length[1],main_divisor)
-			};
-
-		//XXX: TODO: Capture error count here.
-		if( acode_array[0] < 0 ) return;
-		if( acode_array[1] < 0 ) return;
-
-		int acode = acode_array[0];
+		int acode = decode_acode(so->last_sync_length[0],main_divisor);
 
 		if( !so->did_handle_ootx )
-		{
-			int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
-			int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
-
-			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 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];
-
-			//Throw out everything if our sync pulses look like they're bad.
-
-			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
-			int32_t center_2 = so->pulse_synctime_offset;
-			int32_t slack = so->pulse_synctime_slack;
-
-			if( delta1 < center_1 - slack || delta1 > center_1 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-
-			if( delta2 < center_2 - slack || delta2 > center_2 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-
-			so->did_handle_ootx = 1;
-		}
-
-		if (acode > 3) {
-			if( ssn == 0 )
-			{
-				//SV_INFO( "Warning: got a slave marker but only got a master sync." );
-				//This happens too frequently.  Consider further examination.
-			}
-			dl = so->last_sync_time[1];
-			tpco = so->last_sync_length[1];
-		}
+			HandleOOTX( ctx, so );
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
-		if( offset_from < 380000 && offset_from > 70000 )
+		if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) );
+			int whichlh;
+			if( acode < 0 ) whichlh = 1;
+			else whichlh = !(acode>>2);
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
 		}
 	}
 	else
@@ -677,7 +707,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Runt pulse, or no sync pulses available.
 	}
 #endif
-
 }
 
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 8e22ea1..030db8a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -54,7 +54,7 @@ const short vidpids[] = {
 
 const char * devnames[] = {
 	"HMD",
-	"Lighthouse",
+	"HMD IMU & LH",
 	"Watchman 1",
 	"Watchman 2",
 	"Tracker 0",
@@ -344,7 +344,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 			if (cur_dev->vendor_id == vendor_id &&
 				cur_dev->product_id == product_id)
 			{
-				if( cur_dev->interface_number == enumid )
+				if( cur_dev->interface_number == enumid ||
+					cur_dev->interface_number == -1 && menum == enumid)
 				{
 					path_to_open = cur_dev->path;
 					break;
@@ -370,6 +371,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 		wchar_t wstr[255];
 
 		res = hid_get_serial_number_string(handle, wstr, 255);
+		printf("Found %s. ", devnames[i]);
 		wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d  (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle);
 		
 		sv->udev[i] = handle;
@@ -1164,7 +1166,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 			le.sensor_id = POP1;
 			le.length = POP2;
 			le.timestamp = POP4;
-			if( le.sensor_id == 0xff ) break;
+			if( le.sensor_id > 0xfd ) continue;
 			handle_lightcap( obj, &le );
 		}		
 		break;
@@ -1182,7 +1184,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 			le.sensor_id = (uint8_t)POP2;
 			le.length = POP2;
 			le.timestamp = POP4;
-			if( le.sensor_id == 0xff ) break;
+			if( le.sensor_id > 0xfd ) continue;  //
 			handle_lightcap( obj, &le );
 		}		
 		break;