Merge branch 'master' into alternate_disambiguator

# Conflicts:
#	src/survive_data.c
#	src/survive_vive.c

6 files changed, 215 insertions, 47 deletions

diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index e81e154..80f65a9 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 //    FLT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
 //    FLT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLT ydist  = 1.0 / urhat_len;
-    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+    printf("ydist %f\n", ydist);
+//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 	if( x_y != x_y ) x_y = 0;
 	if( y_y != y_y ) y_y = 0;
 	if( z_y != z_y ) z_y = 0;
-/*
+
     // Exhaustively flip the minus sign of the z axis until we find the right one . . .
     FLT bestErr = 9999.0;
     FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
@@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
-                printf("err %f hand %f\n", err, hand);
+//                printf("err %f hand %f\n", err, hand);
                 
                 // If we are the best right-handed frame so far
                 //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
@@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
         x_y = -x_y;
     }
-    printf("bestErr %f\n", bestErr);
-*/
+//    printf("bestErr %f\n", bestErr);
 
+/*
     //-------------------------
     // A test version of the rescaling to the proper length
     //-------------------------
@@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 		if( y_y != y_y ) y_y = 0;
 		if( z_y != z_y ) z_y = 0;
 
-		printf( "---> %f %f %f\n", x_y, y_y, z_y );
+//		printf( "---> %f %f %f\n", x_y, y_y, z_y );
 
         // Exhaustively flip the minus sign of the z axis until we find the right one . . .
         FLT bestErr = 9999.0;
@@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                     FLT cx,cy,cz;
                     CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
                     FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
-                  printf("err %f hand %f\n", err, hand);
+                  //printf("err %f hand %f\n", err, hand);
                 
                     // If we are the best right-handed frame so far
                     if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
@@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
     }
     ydist = bestYdist;
-
+*/
 /*
     for (i=0; i<nPoints; i++) {
         FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
@@ -441,7 +442,71 @@ PRINT(ab,2,1);
     T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
 
-    PRINT_MAT(T,4,4);
+
+	FLT T2[4][4];
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+	FLT temp[4][4];
+	FLT quat[4], quatNorm[4];
+	FLT euler[3];
+
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+
+	PRINT_MAT(T,4,4);
+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);
+
+
+
+	//memcpy(T2,T,16*sizeof(float));
+//	matrix44copy(T2,T);
+	matrix44transpose(T2,T);
+	quatfrommatrix( quat, &T2[0][0] );
+	quatnormalize(quatNorm,quat);
+	quattoeuler(euler,quatNorm);
+	quattomatrix( T2, quatNorm );
+	printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat));
+	PRINT(T,4,4);
+
+//	matrix44copy(temp,T2);
+	matrix44transpose(temp,T2);
+
+	memcpy(T2,temp,16*sizeof(float));
+	for (i=0; i<3; i++) {
+		for (j=0; j<3; j++) {
+			T[i][j] = T2[j][i];
+		}
+	}
+	PRINT(T2,4,4);
+
+
+/*
+    CrossProduct(T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1]);
+    CrossProduct(T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2]);
+    CrossProduct(T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0]);
+	float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]);
+	float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]);
+	float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]);
+	T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen;
+	T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen;
+	T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen;
+*/
+
+
+ //   PRINT_MAT(T,4,4);
     //-------------------
     // Plot the output points
     //-------------------
@@ -453,7 +518,7 @@ PRINT(ab,2,1);
         S_out[1][i] = atan2(Tz, Ty);   // vert
         //S_out[0][i] = Tx;
         //S_out[1][i] = Tz;
-        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
+//        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
     }
 
 }
diff --git a/src/survive_cal.c b/src/survive_cal.c
index dfa2e85..19eb3ca 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -223,6 +223,21 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 				//cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
+	case 3: //Look for light sync lengths.
+	{
+		if( acode >= -2 ) break;
+		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;
+
+		cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
+		break;
+	}
+
 	}
 	
 }
@@ -299,6 +314,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 				if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) 
 				{
 					//printf( "Dev %d, LH %d not enough visible points found.\n", i, j );
+					reset_calibration( cd );
 					cd->found_common = 0;
 					return;
 				}
@@ -359,6 +375,8 @@ static void reset_calibration( struct SurviveCalData * cd )
 	cd->found_common = 0;
 	cd->times_found_common = 0;
 	cd->stage = 2;
+
+	memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) );
 }
 
 static void handle_calibration( struct SurviveCalData *cd )
@@ -384,9 +402,45 @@ static void handle_calibration( struct SurviveCalData *cd )
 #else
 	mkdir( "calinfo", 0755 );
 #endif
+	int sen, axis, lh;
+	FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+
+	//Just to get it out of the way early, we'll calculate the sync-pulse-lengths here.
+	FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" );
+
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+	{
+		int count = cd->all_sync_counts[sen][lh];
+		int i;
+		double totaltime;
+
+		totaltime = 0;
+
+		if( count < 20 ) continue;
+		for( i = 0; i < count; i++ )
+		{
+			totaltime += cd->all_sync_times[sen][lh][i];
+		}
+		FLT avg = totaltime/count;
+
+		double stddev = 0.0;
+		for( i = 0; i < count; i++ )
+		{
+			stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg);
+		}
+		stddev /= count;
+
+		fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev );
+	}
+
+	fclose( sync_time_info );
+
+
+
+
 	FILE * hists = fopen( "calinfo/histograms.csv", "w" );
 	FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
-	int sen, axis, lh;
 	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
 	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
 	for( axis = 0; axis < 2; axis++ )
@@ -467,7 +521,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 		FLT stddevlen = 0;
 
 		#define HISTOGRAMSIZE   31
-		#define HISTOGRAMBINANG 0.00001  //TODO: Tune
+		#define HISTOGRAMBINANG ((3.14159)/400000.0)  //TODO: Tune
 
 		int histo[HISTOGRAMSIZE];
 		memset( histo, 0, sizeof( histo ) );
@@ -552,6 +606,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 			fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
 			fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
 			fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}
 
 		int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd );
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 9d7b3a9..8f4e4de 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -35,7 +35,9 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t
 #define MAX_SENSORS_TO_CAL 96
 
 #define MIN_PTS_BEFORE_CAL 24
-#define DRPTS 128
+
+
+#define DRPTS 32  //Number of samples required in collection phase.
 
 #define MAX_POSE_OBJECTS 10
 
@@ -54,6 +56,9 @@ struct SurviveCalData
 	int8_t found_common;
 	int8_t times_found_common;
 
+	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS];
+	int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES];
+
 	//For camfind (4+)
 	//Index is calculated with:      int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
 	FLT avgsweeps[MAX_CAL_PT_DAT];
diff --git a/src/survive_data.c b/src/survive_data.c
index 00e66f0..ee180b1 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -255,24 +255,47 @@ 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.
+
+	int32_t acode = (length+main_divisor+50)/(main_divisor*2);
+	if( acode & 1 ) return -1;
+
+	return (acode>>1) - 6;
+}
 
 //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 )
 {
-	handle_lightcap2(so,le);
-	return;
-
 	SurviveContext * ctx = so->ctx;
-	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
-
-	//if( so->codename[0] != 'H' )
+//	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 )
 	{
 		return;
 	}
 
+#if 0
+	if( so->codename[0] == 'H' )
+	{
+		static int lt;
+		static int last;
+		if( le->length > 1000 )
+		{
+			int dl = le->timestamp - lt;
+			lt = le->timestamp;
+			if( dl > 10000 || dl < -10000 )
+				printf( "+++%s %3d %5d %9d  ", so->codename, le->sensor_id, le->length, dl );
+			if( dl > 100000 ) printf(" \n" );
+		}
+		last=le->length;
+	}
+#endif
+
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
 
@@ -298,7 +321,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
-		//printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta);
+
 
 		so->did_handle_ootx = 0;
 
@@ -343,6 +366,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				}
 			}
 		}
+
+		//Extra tidbit for storing length-of-sync-pulses.
+		{
+			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 );
+	}
 	}
 
 
@@ -366,17 +397,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 
 		int32_t acode_array[2] =
 			{
-				(so->last_sync_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
-				(so->last_sync_length[1]+main_divisor+50)/(main_divisor*2),	//Check the +50 in the future to see how well this works on a variety of hardware.
+				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] & 1 ) return;
-		if( acode_array[1] & 1 ) return;
-
-		acode_array[0] = (acode_array[0]>>1) - 6;
-		acode_array[1] = (acode_array[1]>>1) - 6;
-
+		if( acode_array[0] < 0 ) return;
+		if( acode_array[1] < 0 ) return;
 
 		int acode = acode_array[0];
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 463481a..d4604d8 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	}
 
+	//We don't use sync times, yet.
+	if( acode < -1 ) return;
+
 	if( base_station > NUM_LIGHTHOUSES ) return;
 
 	//No loner need sync information past this point.
diff --git a/src/survive_vive.c b/src/survive_vive.c
index c04fc03..f6465b2 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -141,10 +141,10 @@ void survive_data_cb( SurviveUSBInterface * si );
 
 //USB Subsystem 
 void survive_usb_close( SurviveContext * t );
-int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 );
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 );
 int survive_usb_poll( SurviveContext * ctx );
 int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic );
-int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
+int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
 
 #ifdef HIDAPI
 void * HAPIReceiver( void * v )
@@ -177,8 +177,8 @@ void * HAPIReceiver( void * v )
 #else
 static void handle_transfer(struct libusb_transfer* transfer)
 {
-	struct SurviveUSBInterface * iface = transfer->user_data;
-	struct SurviveContext * ctx = iface->ctx;
+	SurviveUSBInterface * iface = transfer->user_data;
+	SurviveContext * ctx = iface->ctx;
 
 	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
 	{
@@ -307,9 +307,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac
 	return -1;
 }
 
-int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 )
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 )
 {
-	struct SurviveContext * ctx = sv->ctx;
+	SurviveContext * ctx = sv->ctx;
 #ifdef HIDAPI
 	if( !GlobalRXUSBMutx )
 	{
@@ -488,10 +488,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
 	return 0;
 }
 
-int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
+int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
 {
 	int r;
-	struct SurviveViveData * sv = drv;
+	SurviveViveData * sv = drv;
 	printf( "*CALLING %p %p\n", ctx, sv );
 
 	//XXX TODO: Handle haptics, etc.
@@ -583,7 +583,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c
 	return 0;
 }
 
-void survive_vive_usb_close( struct SurviveViveData * sv )
+void survive_vive_usb_close( SurviveViveData * sv )
 {
 	int i;
 #ifdef HIDAPI
@@ -608,7 +608,7 @@ void survive_vive_usb_close( struct SurviveViveData * sv )
 #endif
 }
 
-int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
+int survive_vive_usb_poll( SurviveContext * ctx, void * v )
 {
 #ifdef HIDAPI
 	OGUnlockMutex( GlobalRXUSBMutx );
@@ -616,11 +616,11 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
 	OGUnlockMutex( GlobalRXUSBMutx );
 	return 0;
 #else
-	struct SurviveViveData * sv = v;
+	SurviveViveData * sv = v;
 	int r = libusb_handle_events( sv->usbctx );
 	if( r )
 	{
-		struct SurviveContext * ctx = sv->ctx;
+		SurviveContext * ctx = sv->ctx;
 		SV_ERROR( "Libusb poll failed." );
 	}
 	return r;
@@ -629,9 +629,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
 }
 
 
-int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic )
+int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic )
 {
-	struct SurviveContext * ctx = sv->ctx;
+	SurviveContext * ctx = sv->ctx;
 	int ret, count = 0, size = 0;
 	uint8_t cfgbuff[64];
 	uint8_t compressed_data[8192];
@@ -749,7 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno,
 #define POP2  (*(((uint16_t*)((readdata+=2)-2))))
 #define POP4  (*(((uint32_t*)((readdata+=4)-4))))
 
-static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
+static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 {
 
 	uint8_t startread[29];
@@ -771,7 +771,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
-
+	int i;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
@@ -916,11 +916,12 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		LightcapElement les[10];
 		int lese = 0; //les's end
 
+
 		//Second, go through all LEDs and extract the lightevent from them. 
 		{
 			uint8_t *marked;
 			marked = alloca(nrtime);
-			memset( marked, 0, sizeof( marked ) );
+			memset( marked, 0, nrtime );
 			int i, parpl = 0;
 			timecount--;
 			int timepl = 0;
@@ -933,8 +934,20 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 				led >>= 3;
 
 				while( marked[timepl] ) timepl++;
+
+#ifdef DEBUG_WATCHMAN
+				int i;
+				printf( "TP %d   TC: %d : ", timepl, timecount );
+				for( i = 0; i < nrtime; i++ )
+				{
+					printf( "%d", marked[i] );
+				}
+				printf( "\n" );
+#endif
+
 				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
 				uint32_t endtime = times[timepl++];
+
 				int end = timepl + adv;
 				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
 				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
@@ -978,7 +991,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	end:
 		{
 			SurviveContext * ctx = w->ctx;
-			SV_INFO( "Light decoding fault: %d\n", fault );
+			SV_INFO( "Light decoding fault: %d", fault );
 		}
 	}
 }
@@ -1015,7 +1028,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 	{
 	case USB_IF_HMD:
 	{
-		struct SurviveObject * headset = obj;
+		SurviveObject * headset = obj;
 		readdata+=2;
 		headset->buttonmask = POP1;		//Lens
 		headset->axis2 = POP2;			//Lens Separation