Merge branch 'master' into alternate_disambiguator

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

21 files changed, 1447 insertions, 378 deletions

diff --git a/Makefile b/Makefile
index a642877..1899c4c 100644
--- a/Makefile
+++ b/Makefile
@@ -32,6 +32,12 @@ LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE)
 LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
 LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
 
+
+#If you want to use HIDAPI on Linux.
+#CFLAGS:=$(CFLAGS) -DHIDAPI
+#REDISTS:=$(REDISTS) redist/hid-linux.o
+#LDFLAGS:=$(LDFLAGS) -ludev
+
 #Useful Preprocessor Directives:
 # -DUSE_DOUBLE = use double instead of float for most operations.
 # -DNOZLIB = use puff.c
diff --git a/README.md b/README.md
index 45ac38f..db1d6ce 100644
--- a/README.md
+++ b/README.md
@@ -1,8 +1,10 @@
 # libsurvive
 
-**WARNING PROJECT IN EXPERIMENTAL PHASE**
+![Logo](https://cloud.githubusercontent.com/assets/2748168/24084003/9095c98a-0cb8-11e7-88a3-575f9f4c7bb4.png)
 
-Discord: https://discordapp.com/invite/7QbCAGS
+An Open-Source tool for working with lighthouse-based trakcing data, including support for the HTC Vive, which is still in the experimental phase.
+
+Most of the development is discussed on Discord.  Join the chat and discussion here: https://discordapp.com/invite/7QbCAGS
 
 ## Livestream collection
 | Note                                   | Youtube URL                                 | Run time |
@@ -40,7 +42,7 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L
 2. Put it under an open-source instead of a force-source license.  (GPL to MIT/X11)
 3. Write it in C.
 4. Avoid extra layers where convenient.
-5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.]
+5. (long shot) Make the vive viable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.]
 
 Will ~~I~~ we succeed?  Probably not.
 
@@ -72,8 +74,12 @@ I may or may not read data from the Vive regarding configuration.  If I do, it w
 
 ## Intel Integrated Graphics
 
-The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz).
+The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz).
 
 To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time).
 
-but HDMI will require non-certified drivers to allow custom resolutions (due to WHQL restrictions). Haswell (Iris) graphics and later can use the new intel beta drivers to allow custom resolutions.]
+
+
+## Addendum and notes
+
+Thanks to Mr. Fault for our logo!
diff --git a/calibrate.c b/calibrate.c
index 82869e0..62acbf6 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -56,7 +56,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 //	if( timeinsweep < 0 ) return;
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	if( sensor_id < 0 ) return;
-	if( acode == -1 ) return;
+	if( acode < 0 ) return;
 //return;
 	int jumpoffset = sensor_id;
 	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
diff --git a/dave/AffineSolve b/dave/AffineSolve
index 98a9590..7d51b34 100755
--- a/dave/AffineSolve
+++ b/dave/AffineSolve
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 4fba56b..685062e 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -11,10 +11,13 @@
 #include <math.h>
 #include "dclapack.h"
 #include <linmath.h>
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
 #define LH_ID    0
 #define NUM_HMD 32
+#define INDIR "full_test_triangle_on_floor/"
 
-#define MAX_POINTS 128
+#define MAX_POINTS SENSORS_PER_OBJECT
 //#define _ABS(a)  ( (a)<=0 ? -(a) : (a) )
 #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
 #define RANDF  ( (float)rand() / (float)RAND_MAX )
@@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3];
 void ReadHmdPoints()
 {
     int i;
-    FILE *fin = fopen("HMD_points.csv","r");
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
     if (fin==NULL) {
         printf("ERROR: could not open HMD_points.csv for reading\n");
         exit(1);
@@ -52,7 +55,7 @@ void ReadPtinfo()
     for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
 
     // Read ptinfo.csv
-    FILE *fin = fopen("ptinfo.csv", "r");
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
     if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
     while (!feof(fin))
     {
@@ -74,6 +77,350 @@ void ReadPtinfo()
     fclose(fin);
 }
 
+void AffineSolve(
+    float T[4][4],           // OUTPUT: transform
+    float O[MAX_POINTS][4],  // INPUT:  points, offsets
+    float N[MAX_POINTS][3],  // INPUT:  plane normals
+    float D[MAX_POINTS],     // INPUT:  plane offsets
+    int nPoints, int nIter,
+    float stepSizeRot, float stepSizePos, float falloff, int constrain)
+{
+    int i,j,k,iter;
+    //T[3][3] = 1.0f;
+    
+    printf("iter x y z error\n");
+    
+    float gradDot     = 1.0;
+    float prevGradDot = 1.0;
+    float de_dT[3][4];  // the gradient
+    float conj[3][4];   // the conjugate
+    float errorSq=0.0;
+    for (iter=0; iter<nIter; iter++)
+    {
+        //----------------------------------
+        // Calculate the gradient direction
+        //----------------------------------
+        errorSq = 0.0;
+        memset(de_dT, 0, 3*4*sizeof(float));
+        for (i=0; i<nPoints; i++)
+        {
+            // What is the plane deviation error
+            float Ei = -D[i];
+            for (j=0; j<3; j++) {
+                float Tj_oi = 0.0f;
+                for (k=0; k<4; k++) {
+                    Tj_oi += T[j][k] * O[i][k];
+                }
+                Ei += N[i][j] * Tj_oi;
+            }
+//            printf("E[%d] %f\n", i, Ei);
+        
+            // Figure out contribution to the error
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
+                }
+            }
+            
+            errorSq += Ei*Ei;
+        }
+
+//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
+//exit(1);
+        // Constrain the gradient (such that dot products are zero)
+        if (constrain)
+        {
+            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
+            for (k=0; k<3; k++) {
+                T0T1 += T[0][k] * T[1][k];
+                T1T2 += T[1][k] * T[2][k];
+                T2T0 += T[2][k] * T[0][k];
+            }
+//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
+            for (k=0; k<3; k++) {
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
+            }
+        }
+
+        // Calculate the gradient dot product
+        //  (used by conjugate gradient method)
+        prevGradDot = gradDot;
+        gradDot = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<4; k++) {
+                gradDot += de_dT[j][k] * de_dT[j][k];
+            }
+        }
+
+//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
+
+        //----------------------------------
+        // Calculate the conjugate direction
+        //----------------------------------
+//        if (iter==0) {
+            // First iteration, just use the gradient
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = -de_dT[j][k];
+                }
+            }
+/*        } else {
+            // Calculate "beta" for Fletcher Reeves method
+            float beta = gradDot / prevGradDot;
+//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
+
+            // Update the conjugate
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
+                }
+            }            
+        }
+*/
+
+//        PRINT_MAT(de_dT,4,4);
+//        exit(1);
+
+        //----------------------------------
+        // How large is the gradient ?
+        //----------------------------------
+        
+        double gradSizeRot = 0.0;
+        double gradSizePos = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<3; k++) {
+                gradSizeRot += _ABS(conj[j][k]);
+            }
+            gradSizePos += _ABS(conj[j][k]);
+        }
+        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
+        
+        //----------------------------------
+        // Descend in the gradient direction
+        //----------------------------------
+        if (gradSizeRot > TOO_SMALL) {
+            float scaleRot = stepSizeRot / gradSizeRot;
+            for (j=0; j<3; j++) {
+                for (k=0; k<3; k++) {
+                    T[j][k] += scaleRot * conj[j][k];
+                }
+            }
+            stepSizeRot *= falloff;
+        }
+        
+        if (gradSizePos > TOO_SMALL) {
+            float scalePos = stepSizePos / gradSizePos;
+            for (j=0; j<3; j++) {
+                T[j][3] += scalePos * conj[j][3];
+            }
+            stepSizePos *= falloff;
+        }
+        
+        // Constrain the gradient (such that scaling is one)
+        if (constrain)
+        {
+            // Measure the scales
+            float len[3] = {0.0, 0.0, 0.0};
+            for (j=0; j<3; j++) {
+                double lenSq = 0.0;
+                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
+                len[j] = sqrt(lenSq);
+            }
+            
+            // How far off is the scale?
+            float xzLen = 0.5 * (len[0] + len[2]);
+            if (xzLen > TOO_SMALL) {
+                float inv_xzLen = 1.0 / xzLen;
+                for (j=0; j<3; j++) {
+                    T[3][j] *= inv_xzLen;
+                }
+            }
+            
+            // Rescale the thing
+            for (j=0; j<3; j++)
+            {
+                if (len[j] > TOO_SMALL) {
+                    float inv_len = 1.0 / len[j];
+                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
+                }
+            }
+        }
+    }
+    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
+    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
+}
+
+int main()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
+
+    //-------------------------
+    // Package the lighthouse data for "AffineSolve"
+    //-------------------------
+
+    // Data for the "iterative" affine solve formula
+    float Tcalc[4][4];
+    float O[MAX_POINTS][4];
+    float N[MAX_POINTS][3];
+    float D[MAX_POINTS];
+    int nPlanes = 0;
+
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        for (axis=0; axis<2; axis++)
+        {
+            if (hmd_angle[sen][axis] != -9999.0)
+            {
+                // Set the offset
+                O[nPlanes][0] = hmd_pos[sen][0];
+                O[nPlanes][1] = hmd_pos[sen][1];
+                O[nPlanes][2] = hmd_pos[sen][2];
+                O[nPlanes][3] = 1.0;
+        
+                // Calculate the plane equation
+                if (axis == 0) {   // Horizontal
+                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] = 0.0;
+                    D[nPlanes]    = 0.0;
+                } else {           // Vertical
+                    N[nPlanes][0] = 0.0;
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
+                    D[nPlanes]    = 0.0;
+                }
+
+                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
+                    nPlanes,
+                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
+                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
+                    D[nPlanes]);
+                nPlanes++;
+            }
+        }
+    }
+
+    
+    printf("nPlanes %d\n", nPlanes);
+        
+    //}
+    
+    PRINT_MAT(Tcalc,4,4);
+
+    
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
+
+    // Data for the "fake" ortho solve formula
+    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+    int nPoints=0;  
+
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    int loop;
+    for (loop=0; loop<1; loop++)
+    {
+        // Run OrthoSolve
+        OrthoSolve(
+            Tortho,     // OUTPUT: 4x4 transformation matrix
+            S_out,      // OUTPUT: array of output screenspace points
+            S_in,       // INPUT:  array of screenspace points
+            X_in,       // INPUT:  array of offsets
+            nPoints);
+			printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+		// Come up with rotation and transposed version of Tortho
+		FLT TorthoTr[4][4], Rortho[4][4], RorthoTr[4][4];
+		TRANSP(Tortho,TorthoTr,4,4);
+		memcpy(Rortho,Tortho,4*4*sizeof(FLT));
+		Rortho[3][0]=0.0;  Rortho[3][1]=0.0;  Rortho[3][2]=0.0;
+		TRANSP(Rortho,RorthoTr,4,4);
+		PRINT(Tortho,4,4);
+		PRINT(Rortho,4,4);
+
+		// Print out some quaternions
+		FLT Tquat[4], TquatTr[4], Rquat[4], RquatTr[4];
+		quatfrommatrix(Tquat,   &Tortho[0][0] );
+		quatfrommatrix(TquatTr, &TorthoTr[0][0] );
+		quatfrommatrix(Rquat,   &Rortho[0][0] );
+		quatfrommatrix(RquatTr, &RorthoTr[0][0] );
+		printf( "Tquat  : %f %f %f %f = %f\n", Tquat  [0], Tquat  [1], Tquat  [2], Tquat  [3], quatmagnitude(Tquat));
+		printf( "TquatTr: %f %f %f %f = %f\n", TquatTr[0], TquatTr[1], TquatTr[2], TquatTr[3], quatmagnitude(TquatTr));
+		printf( "Rquat  : %f %f %f %f = %f\n", Rquat  [0], Rquat  [1], Rquat  [2], Rquat  [3], quatmagnitude(Rquat));
+		printf( "RquatTr: %f %f %f %f = %f\n", RquatTr[0], RquatTr[1], RquatTr[2], RquatTr[3], quatmagnitude(RquatTr));
+
+		// Flip y and z axies
+		FLT T2[4][4] = {
+			{ Tortho[0][0], -Tortho[0][2], -Tortho[0][1], 0.0 },
+			{ Tortho[1][0], -Tortho[1][2], -Tortho[1][1], 0.0 },
+			{ Tortho[2][0], -Tortho[2][2], -Tortho[2][1], 0.0 },
+			{          0.0,           0.0,           0.0, 1.0 } };
+PRINT(T2,4,4);
+
+		// Print out the quaternions
+		FLT T2quat[4];
+		quatfrommatrix(T2quat,   &T2[0][0] );
+		printf( "T2quat : %f %f %f %f = %f\n", T2quat [0], T2quat [1], T2quat [2], T2quat [3], quatmagnitude(T2quat));
+    }
+
+    // Run the calculation for Tcalc
+    //int run;
+    //for (run=0; run<100; run++) {
+/*
+    // Initialize Tcalc to the identity matrix
+    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
+    //memset(Tcalc, 0, 4*4*sizeof(float));
+    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
+
+    // Solve it!
+    AffineSolve(
+        Tcalc,           // OUTPUT: transform
+        O,  // INPUT:  points, offsets
+        N,  // INPUT:  plane normals
+        D,     // INPUT:  plane offsets
+        nPlanes, NITER,
+        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
+        1);
+*/
+    // insert code here...
+    return 0;
+}
+
+
+
+
+
+#if 0
 #define PRINT_MAT(A,M,N) { \
     int m,n; \
     printf(#A "\n"); \
@@ -433,308 +780,4 @@ PRINT(ab,2,1);
 //    printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
 //    printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist);
 }
-
-void AffineSolve(
-    float T[4][4],           // OUTPUT: transform
-    float O[MAX_POINTS][4],  // INPUT:  points, offsets
-    float N[MAX_POINTS][3],  // INPUT:  plane normals
-    float D[MAX_POINTS],     // INPUT:  plane offsets
-    int nPoints, int nIter,
-    float stepSizeRot, float stepSizePos, float falloff, int constrain)
-{
-    int i,j,k,iter;
-    //T[3][3] = 1.0f;
-    
-    printf("iter x y z error\n");
-    
-    float gradDot     = 1.0;
-    float prevGradDot = 1.0;
-    float de_dT[3][4];  // the gradient
-    float conj[3][4];   // the conjugate
-    float errorSq=0.0;
-    for (iter=0; iter<nIter; iter++)
-    {
-        //----------------------------------
-        // Calculate the gradient direction
-        //----------------------------------
-        errorSq = 0.0;
-        memset(de_dT, 0, 3*4*sizeof(float));
-        for (i=0; i<nPoints; i++)
-        {
-            // What is the plane deviation error
-            float Ei = -D[i];
-            for (j=0; j<3; j++) {
-                float Tj_oi = 0.0f;
-                for (k=0; k<4; k++) {
-                    Tj_oi += T[j][k] * O[i][k];
-                }
-                Ei += N[i][j] * Tj_oi;
-            }
-//            printf("E[%d] %f\n", i, Ei);
-        
-            // Figure out contribution to the error
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
-                }
-            }
-            
-            errorSq += Ei*Ei;
-        }
-
-//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
-//exit(1);
-        // Constrain the gradient (such that dot products are zero)
-        if (constrain)
-        {
-            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
-            for (k=0; k<3; k++) {
-                T0T1 += T[0][k] * T[1][k];
-                T1T2 += T[1][k] * T[2][k];
-                T2T0 += T[2][k] * T[0][k];
-            }
-//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
-            for (k=0; k<3; k++) {
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
-            }
-        }
-
-        // Calculate the gradient dot product
-        //  (used by conjugate gradient method)
-        prevGradDot = gradDot;
-        gradDot = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<4; k++) {
-                gradDot += de_dT[j][k] * de_dT[j][k];
-            }
-        }
-
-//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
-
-        //----------------------------------
-        // Calculate the conjugate direction
-        //----------------------------------
-//        if (iter==0) {
-            // First iteration, just use the gradient
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = -de_dT[j][k];
-                }
-            }
-/*        } else {
-            // Calculate "beta" for Fletcher Reeves method
-            float beta = gradDot / prevGradDot;
-//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
-
-            // Update the conjugate
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
-                }
-            }            
-        }
-*/
-
-//        PRINT_MAT(de_dT,4,4);
-//        exit(1);
-
-        //----------------------------------
-        // How large is the gradient ?
-        //----------------------------------
-        
-        double gradSizeRot = 0.0;
-        double gradSizePos = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<3; k++) {
-                gradSizeRot += _ABS(conj[j][k]);
-            }
-            gradSizePos += _ABS(conj[j][k]);
-        }
-        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
-        
-        //----------------------------------
-        // Descend in the gradient direction
-        //----------------------------------
-        if (gradSizeRot > TOO_SMALL) {
-            float scaleRot = stepSizeRot / gradSizeRot;
-            for (j=0; j<3; j++) {
-                for (k=0; k<3; k++) {
-                    T[j][k] += scaleRot * conj[j][k];
-                }
-            }
-            stepSizeRot *= falloff;
-        }
-        
-        if (gradSizePos > TOO_SMALL) {
-            float scalePos = stepSizePos / gradSizePos;
-            for (j=0; j<3; j++) {
-                T[j][3] += scalePos * conj[j][3];
-            }
-            stepSizePos *= falloff;
-        }
-        
-        // Constrain the gradient (such that scaling is one)
-        if (constrain)
-        {
-            // Measure the scales
-            float len[3] = {0.0, 0.0, 0.0};
-            for (j=0; j<3; j++) {
-                double lenSq = 0.0;
-                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
-                len[j] = sqrt(lenSq);
-            }
-            
-            // How far off is the scale?
-            float xzLen = 0.5 * (len[0] + len[2]);
-            if (xzLen > TOO_SMALL) {
-                float inv_xzLen = 1.0 / xzLen;
-                for (j=0; j<3; j++) {
-                    T[3][j] *= inv_xzLen;
-                }
-            }
-            
-            // Rescale the thing
-            for (j=0; j<3; j++)
-            {
-                if (len[j] > TOO_SMALL) {
-                    float inv_len = 1.0 / len[j];
-                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
-                }
-            }
-        }
-    }
-    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
-    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
-}
-
-int main()
-{
-    int i,j,k,sen,axis;
-        
-    // Read the data files
-    ReadHmdPoints();
-    ReadPtinfo();
-
-    //-------------------------
-    // Package the lighthouse data for "AffineSolve"
-    //-------------------------
-
-    // Data for the "iterative" affine solve formula
-    float Tcalc[4][4];
-    float O[MAX_POINTS][4];
-    float N[MAX_POINTS][3];
-    float D[MAX_POINTS];
-    int nPlanes = 0;
-
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        for (axis=0; axis<2; axis++)
-        {
-            if (hmd_angle[sen][axis] != -9999.0)
-            {
-                // Set the offset
-                O[nPlanes][0] = hmd_pos[sen][0];
-                O[nPlanes][1] = hmd_pos[sen][1];
-                O[nPlanes][2] = hmd_pos[sen][2];
-                O[nPlanes][3] = 1.0;
-        
-                // Calculate the plane equation
-                if (axis == 0) {   // Horizontal
-                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] = 0.0;
-                    D[nPlanes]    = 0.0;
-                } else {           // Vertical
-                    N[nPlanes][0] = 0.0;
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
-                    D[nPlanes]    = 0.0;
-                }
-
-                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
-                    nPlanes,
-                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
-                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
-                    D[nPlanes]);
-                nPlanes++;
-            }
-        }
-    }
-
-    
-    printf("nPlanes %d\n", nPlanes);
-        
-    //}
-    
-    PRINT_MAT(Tcalc,4,4);
-
-    
-    //--------------------------------------------------
-    // Package the data for "OrthoSolve"
-    //--------------------------------------------------
-
-    // Data for the "fake" ortho solve formula
-    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
-    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
-    int nPoints=0;  
-
-    // Transform into the "OrthoSolve" format
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
-        {
-            S_in[0][nPoints] = hmd_angle[sen][0];
-            S_in[1][nPoints] = hmd_angle[sen][1];
-            X_in[0][nPoints] = hmd_pos[sen][0];
-            X_in[1][nPoints] = hmd_pos[sen][1];
-            X_in[2][nPoints] = hmd_pos[sen][2];
-            nPoints++;
-        }
-    }
-    printf("OrthoSolve nPoints %d\n", nPoints);
-    
-    //--------------------------------------------------
-    // Run the "OrthoSolve" and then the "AffineSolve"
-    //--------------------------------------------------    
-    
-    int loop;
-    for (loop=0; loop<1; loop++)
-    {
-        // Run OrthoSolve
-        OrthoSolve(
-            Tortho,     // OUTPUT: 4x4 transformation matrix
-            S_out,      // OUTPUT: array of output screenspace points
-            S_in,       // INPUT:  array of screenspace points
-            X_in,       // INPUT:  array of offsets
-            nPoints);
-    }
-
-    // Run the calculation for Tcalc
-    //int run;
-    //for (run=0; run<100; run++) {
-/*
-    // Initialize Tcalc to the identity matrix
-    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
-    //memset(Tcalc, 0, 4*4*sizeof(float));
-    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
-
-    // Solve it!
-    AffineSolve(
-        Tcalc,           // OUTPUT: transform
-        O,  // INPUT:  points, offsets
-        N,  // INPUT:  plane normals
-        D,     // INPUT:  plane offsets
-        nPlanes, NITER,
-        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
-        1);
-*/
-    // insert code here...
-    return 0;
-}
+#endif
diff --git a/dave/Makefile b/dave/Makefile
index bf62837..330bda3 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,7 +1,21 @@
+UNAME:=$(shell uname)
+
+CFLAGS:= -lm -I../redist -I../src -I../include/libsurvive
+
+ifeq ($(UNAME), Linux)
+CFLAGS:= $(CFLAGS) -lGL -lGLU -lglut -lX11 -I../../redist -DLINUX -lm -lpthread -DLINUX
+endif
+
+# Darwin is Mac OSX !!
+ifeq ($(UNAME), Darwin)
+CFLAGS:= $(CFLAGS) -I../../redist -w -framework OpenGL -framework GLUT
+endif
+
+
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
-	gcc -O3 -o dclapack_test dclapack_test.c -lm
-	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall
-
+	gcc -O3 -o dclapack_test dclapack_test.c $(CFLAGS)
+	gcc  -O3 -o AffineSolve   AffineSolve.c $(CFLAGS) ../redist/linmath.c #-Wall
+	gcc -O3 -o OrthoPlot OrthoPlot.c fileutil.c ../redist/linmath.c ../redist/os_generic.c $(CFLAGS)
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c
new file mode 100644
index 0000000..222828c
--- /dev/null
+++ b/dave/OrthoPlot.c
@@ -0,0 +1,451 @@
+/*
+ When creating your project, uncheck OWL,
+ uncheck Class Library, select Static
+ instead of Dynamic and change the target
+ model to Console from GUI.
+ Also link glut.lib to your project once its done.
+ */
+
+#include <stdio.h>   // Standard Header For Most Programs
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include "os_generic.h"
+#include "linmath.h"
+#include "fileutil.h"
+
+#ifdef __APPLE__
+#include <OpenGL/gl.h>   // The GL Header File
+#include <GLUT/glut.h>   // The GL Utility Toolkit (Glut) Header
+#else
+#include <GL/gl.h>
+#include <GL/glu.h>
+#endif
+#ifdef __linux__
+#include <GL/freeglut.h>
+#endif
+
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
+
+
+// Required to set up a window
+#define WIDTH  1280
+#define HEIGHT 1280
+#define FULLSCREEN 0
+int    keys[256];               // Regular keyboard keys
+int sp_keys[256];               // Special keyboard keycodes (GLUT specific)
+
+#define LH_ID    0
+#define NUM_HMD 32
+#define INDIR "dave/full_test_triangle_on_floor/"
+#define MAX_POINTS SENSORS_PER_OBJECT
+
+#define PI 3.1415926535897932386264
+#define MOVESPEED 1.0
+#define ROTSPEED  5.0
+
+// View space
+float posx=0.0f;
+float posy=0.0f;
+float posz=0.0f;
+float rotx=0.0f;
+float roty=0.0f;
+float rotz=0.0f;
+
+// Data for the "fake" ortho solve formula
+float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+int nPoints=0;  
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ);
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr);
+
+
+float hmd_pos[NUM_HMD][3];
+void ReadHmdPoints()
+{
+    int i;
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
+    if (fin==NULL) {
+        printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n");
+        exit(1);
+    }
+    
+    for (i=0; i<NUM_HMD; i++) {
+        fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2]));
+    }
+    
+    fclose(fin);
+}
+
+float hmd_angle[NUM_HMD][2];
+void ReadPtinfo()
+{
+    // Initialize to -9999
+    int i;
+    for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
+
+    // Read ptinfo.csv
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
+    if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
+    while (!feof(fin))
+    {
+        // Read the angle
+        int sen,lh,axis,count;
+        float angle, avglen, stddevang, stddevlen;
+        float max_outlier_length, max_outlier_angle;
+        int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n",
+                        &sen, &lh, &axis, &count,
+                        &angle, &avglen, &stddevang, &stddevlen,
+                        &max_outlier_length, &max_outlier_angle);
+        if (rt != 10) { break; }
+        
+        // If it's valid, store in the result
+        if (lh == LH_ID && sen < NUM_HMD) {
+            hmd_angle[sen][axis] = angle;
+        }
+    }
+    fclose(fin);
+}
+
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+/*
+ * init() is called at program startup
+ */
+void init()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
+
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
+
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		//printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+	//--------------------------------------------------
+	// Spawn a thread to read the HMD angles
+	//--------------------------------------------------
+	OGCreateThread(ThreadReadHmtAngles,0);
+
+    //--------------------------------------------------
+	// Initialize OpenGL
+    //--------------------------------------------------
+	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
+	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);				// Black Background
+	glClearDepth(1.0f);									// Depth Buffer Setup
+	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
+	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
+	glEnable ( GL_COLOR_MATERIAL );
+	glDisable(GL_CULL_FACE);
+	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
+}
+
+/*
+ * draw() is called once every frame
+ */
+void draw()
+{
+	int i,j;
+
+    //------------------------
+    // Check for keyboard input
+    //------------------------
+    if (keys['w'] || keys['W']) {
+        posz += MOVESPEED;
+    }
+    if (keys['s'] || keys['S']) {
+        posz -= MOVESPEED;
+    }
+    if (keys['a'] || keys['A']) {
+        roty += ROTSPEED;
+    }
+    if (keys['d'] || keys['D']) {
+        roty -= ROTSPEED;
+    }
+    if (keys[27]) {
+        exit(0);
+    }
+    
+    //------------------------
+    // Update the scene
+    //------------------------
+    
+    //------------------------
+    // Draw using OpenGL
+    //------------------------
+    
+    // Clear the screen
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
+ 
+    // Translate and rotate the camera
+    glLoadIdentity();  // Reset The Current Modelview Matrix
+//    glTranslatef(-posx,-posy,posz);						// Move Left 1.5 Units And Into The    
+
+	// Bouning box around the points (in radians)
+	float x0=-45.0 * (PI/180.0);
+	float x1= 45.0 * (PI/180.0);
+	float y0=-45.0 * (PI/180.0);
+	float y1= 45.0 * (PI/180.0);
+
+
+	//------------------------
+	// Read the angles from stdin
+	//------------------------
+	FLT hmdAngles[4][NUM_HMD];
+
+	// Read the hmd angles
+	OGLockMutex(read_mutex);
+	for (i=0; i<4; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			hmdAngles[i][j] = read_hmdAngles[i][j];
+		}
+	}
+	OGUnlockMutex(read_mutex);
+
+	// Draw the hmd bearing angles
+	nPoints=0;
+	for (i=0; i<NUM_HMD; i++) {
+		const double dist=10.0;
+		
+		int sweepx = (LH_ID==0) ? SWEEP_LX : SWEEP_RX;
+		int sweepy = (LH_ID==0) ? SWEEP_LY : SWEEP_RY;
+
+		// If the left lighthouse sees it
+		if (read_hmdAngleViewed[sweepx][i] >= read_frameno-6 &&
+		    read_hmdAngleViewed[sweepy][i] >= read_frameno-6 &&
+		    hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0)
+		{
+			S_in[0][nPoints] = hmdAngles[sweepy][i];
+			S_in[1][nPoints] = hmdAngles[sweepx][i];
+            X_in[0][nPoints] = hmd_pos[i][0];
+            X_in[1][nPoints] = hmd_pos[i][1];
+            X_in[2][nPoints] = hmd_pos[i][2];
+printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n",
+	i, S_in[0][nPoints], S_in[1][nPoints],
+	X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints],
+	read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno);
+			nPoints++;
+		}
+	}
+
+	read_frameno++;
+
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+
+	//------------------------
+	// Draw the inputs
+	//------------------------
+	glPointSize(3.0);
+
+	// Draw the input points
+	glColor3f(1.0,0.5,0.5);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_in[0][i]-x0)/(x1-x0),  (S_in[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+	// Draw the output points
+	glColor3f(0.5,0.5,1.0);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_out[0][i]-x0)/(x1-x0),  (S_out[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+
+    //
+    // We're Done
+    //
+    glutSwapBuffers ( );         // Swap The Buffers To Not Be Left With A Clear Screen
+}
+
+/*
+ * resize() is called when we change the screen size
+ */
+void resize(int width, int height)		// Resize And Initialize The GL Window
+{
+    glViewport(0,0,width,height);						// Reset The Current Viewport
+    
+    glMatrixMode(GL_PROJECTION);						// Select The Projection Matrix
+    glLoadIdentity();									// Reset The Projection Matrix
+    
+    // Uncomment For a 3D perspective
+    //gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f);
+    // Uncomment for a 2D perspective
+    glOrtho(0.0, 1.0, 0.0, 1.0, -10.0, 10.0);
+    
+    glMatrixMode(GL_MODELVIEW);							// Select The Modelview Matrix
+    glLoadIdentity();									// Reset The Modelview Matrix
+}
+
+/*
+ * These functions are called whenever a key is pressed
+ */
+void keyboardDown ( unsigned char key, int x, int y )  // Create Keyboard Function
+{
+    keys[key] = 1;
+}
+void keyboardUp ( unsigned char key, int x, int y )
+{
+    keys[key] = 0;
+}
+
+void specialKeyDown ( int key, int x, int y )  // Create Special Function (required for arrow keys)
+{
+    if (key<256) {
+        sp_keys[key] = 1;
+    }
+}
+void specialKeyUp (int key, int x, int y)
+{
+    if (key<256) {
+        sp_keys[key] = 0;
+    }
+}
+
+int main ( int argc, char** argv )   // Create Main Function For Bringing It All Together
+{
+    glutInit            ( &argc, argv ); // Erm Just Write It =)
+    glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode
+    glutInitWindowSize  ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size
+    glutCreateWindow    ( "OpenGL" ); // Window Title (argv[0] for current directory as title)
+    if (FULLSCREEN) {
+        glutFullScreen      ( );          // Put Into Full Screen
+    }
+    glutDisplayFunc     ( draw );  // Matching Earlier Functions To Their Counterparts
+    glutIdleFunc        ( draw );
+    glutReshapeFunc     ( resize );
+    glutKeyboardFunc    ( keyboardDown );
+    glutKeyboardUpFunc  ( keyboardUp );
+    glutSpecialFunc     ( specialKeyDown );
+    glutSpecialUpFunc   ( specialKeyUp );
+    init ();
+    glutMainLoop        ( );          // Initialize The Main Loop
+    return 0;
+}
+
+
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ)
+{
+	float x,y,z;
+	
+	glBegin(GL_LINES);
+
+	// X grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(minX, y, z);
+			glVertex3f(maxX, y, z);
+		}
+	}
+
+	// Y grid stripes
+	for (x=minX; x<maxX; x+=stepX) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(x, minY, z);
+			glVertex3f(x, maxY, z);
+		}
+	}
+
+	// Z grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (x=minX; x<maxX; x+=stepX) {
+			glVertex3f(x, y, minZ);
+			glVertex3f(x, y, maxZ);
+		}
+	}
+
+	glEnd();
+}
+
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr)
+{
+	FLT i0[3],j0[3],k0[3];
+	FLT i[3],j[3],k[3];
+	FLT q[4];
+	
+	i0[0]=1.0; i0[1]=0.0; i0[2]=0.0;
+	j0[0]=0.0; j0[1]=1.0; j0[2]=0.0;
+	k0[0]=0.0; k0[1]=0.0; k0[2]=1.0;
+	q [0]=qr;  q [1]=qx;  q [2]=qy;  q [3]=qz;
+	
+	quatrotatevector(i, q, i0);
+	quatrotatevector(j, q, j0);
+	quatrotatevector(k, q, k0);
+
+	glBegin(GL_LINES);
+	glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]);
+	glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]);
+	glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]);
+	glEnd();
+}
+
+
diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c
index b0667ef..3f48b08 100644
--- a/dave/dclapack_test.c
+++ b/dave/dclapack_test.c
@@ -1,6 +1,6 @@
 #define FLOAT float
 #define ORDER 50
-#include "dclapack.h"
+#include "../redist/dclapack.h"
 
 #include <stdio.h>
 #include <stdlib.h>
diff --git a/dave/fileutil.c b/dave/fileutil.c
new file mode 100644
index 0000000..04dc241
--- /dev/null
+++ b/dave/fileutil.c
@@ -0,0 +1,133 @@
+#include "fileutil.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+#define PI 3.14159265358979323846264
+
+
+og_mutex_t  read_mutex;
+og_thread_t read_thread;
+double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+int read_frameno=0;
+
+static FILE *fopen_orDie(const char *path, const char *flag)
+{
+	FILE *f = fopen(path, flag);
+	if (f == NULL) {
+		printf("ERROR could not oepn %s for %s\n", path, flag);
+		exit(1);
+	}
+	return f;
+}
+
+static void SeekToken(FILE *f, const char *keyword)
+{
+	char token[4096];
+	do {
+		fscanf(f, "%s", token);
+	} while( strcmp(token,keyword)!=0 && !feof(f) );
+}
+
+void LoadLighthousePos(
+        const char *path,
+        float *x, float *y, float *z,
+        float *qi, float *qj, float *qk, float *qreal)
+{
+	FILE *f = fopen_orDie(path,"r");
+	SeekToken(f, "POS:");
+	fscanf(f, "%f %f %f\n", x, y, z);
+	SeekToken(f, "QUAT:");
+	fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk);
+	fclose (f);
+}
+
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angles[NUM_SWEEP][NUM_HMD])
+{
+	int i,j;
+	char   type[256];
+	char   sweep[256];
+	int    id;
+	int    nSweep;
+	double ang;
+	double d1,d2,d3;  // revisit these numbers later
+
+	// Initialize all of the angles to -9999
+	for (i=0; i<NUM_SWEEP; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			angles[i][j] = -9999.0;   // Initially no value
+		}
+	}
+
+	FILE *f = fopen_orDie(path, "r");
+
+	while (!feof(f))
+	{
+		// Read the line from the file
+		int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf",
+			&type, &sweep, &id, &nSweep, &ang,
+			&d1,&d2,&d3);
+
+		if (rt<8) { break; }
+
+		// Only hmd points
+		if ( strcmp(type,"HMD")!=0 ) { continue; }
+
+		// Which sweep is it?
+		int sweepId=-1;
+		if      ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; }
+		else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; }
+		else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; }
+		else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; }
+		else { continue; }
+		
+		// Convert the angle from ticks to radians
+		angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 );
+	}
+
+	fclose(f);
+}
+
+void *ThreadReadHmtAngles(void *junk)
+{
+	char house[256];
+	char xy[256];
+	char hmd[256];
+	double ts;
+	int id;
+	int syncid;
+	int timeInSweep;
+	int length;
+	//lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length
+
+	while(1) {
+		int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+		if (rt==8)
+		{
+			//int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+			//printf( "%s %s %s %f %d %d %d %d\n", house, xy, hmd, ts,id, syncid, timeInSweep, length );
+
+			if( id < 0 ) continue;
+			int sweepId=0;
+			if ( house[0]=='R' ) { sweepId+=2; }
+			if ( xy[0]   =='Y' ) { sweepId++; }
+			double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 );
+		
+			if      ( strcmp(hmd,"HMD")==0 ) { id += 0;  } 
+			else if ( strcmp(hmd,"WM0")==0 ) { id += 32; }
+			else if ( strcmp(hmd,"WM1")==0 ) { id += 56; }
+			else    { continue; }
+		
+			if ( id<0 || id >NUM_HMD) { continue; }
+
+			OGLockMutex  (read_mutex);
+			read_hmdAngles[sweepId][id] = angle;
+			OGUnlockMutex(read_mutex);
+			read_hmdAngleViewed[sweepId][id] = read_frameno;
+		}
+	}
+}
+
+
diff --git a/dave/fileutil.h b/dave/fileutil.h
new file mode 100644
index 0000000..e5da244
--- /dev/null
+++ b/dave/fileutil.h
@@ -0,0 +1,35 @@
+#ifndef _fileutil_h_
+#define _fileutil_h_
+
+#include <pthread.h>
+#include "os_generic.h"
+
+void LoadLighthousePos(
+	const char *path, 
+	float *x, float *y, float *z,
+	float *qi, float *qj, float *qk, float *qreal);
+
+
+// first 32 are hmd, next 24 wm0 next 24 wm1
+#define NUM_HMD   80
+#define NUM_SWEEP 4
+#define SWEEP_LX  0
+#define SWEEP_LY  1
+#define SWEEP_RX  2
+#define SWEEP_RY  3
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angle[NUM_SWEEP][NUM_HMD]);
+
+
+extern og_mutex_t  read_mutex;
+extern og_thread_t read_thread;
+extern double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+extern int read_frameno;
+void *ThreadReadHmtAngles(void *junk);
+
+
+#endif // __fileutil_h_
+
+
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 98c926e..cf11e0c 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -45,6 +45,7 @@ typedef struct
 	//If "lengths[...]" < 0, means not a valid piece of sweep information.
 	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];
 
 	PoserDataIMU lastimu;
 } PoserDataFullScene;
diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c
index 947456f..25e9298 100644
--- a/redist/CNFGFunctions.c
+++ b/redist/CNFGFunctions.c
@@ -270,3 +270,89 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize )
 	CNFGPenY = y + textsize;
 	CNFGDrawText( text, textsize );
 }
+
+
+#ifdef CNFGOGL
+
+#ifdef _MSC_VER
+#include <windows.h>
+#pragma comment( lib, "OpenGL32.lib" )
+#endif
+#include <GL/gl.h>
+
+uint32_t CNFGColor( uint32_t RGB )
+{
+	unsigned char red = RGB & 0xFF;
+	unsigned char grn = ( RGB >> 8 ) & 0xFF;
+	unsigned char blu = ( RGB >> 16 ) & 0xFF;
+	glColor3ub( red, grn, blu );
+}
+
+void CNFGClearFrame()
+{
+	short w, h;
+	unsigned char red = CNFGBGColor & 0xFF;
+	unsigned char grn = ( CNFGBGColor >> 8 ) & 0xFF;
+	unsigned char blu = ( CNFGBGColor >> 16 ) & 0xFF;
+	glClearColor( red/255.0, grn/255.0, blu/255.0, 1.0 );
+	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
+	CNFGGetDimensions( &w, &h );
+	glMatrixMode( GL_PROJECTION );
+	glLoadIdentity();
+	glViewport( 0, 0, w, h );
+	glOrtho( 0, w, h, 0, 1, -1 );
+	glMatrixMode( GL_MODELVIEW );
+	glLoadIdentity();
+}
+
+
+void CNFGTackSegment( short x1, short y1, short x2, short y2 )
+{
+	if( x1 == x2 && y1 == y2 )
+	{
+		glBegin( GL_POINTS );
+		glVertex2f( x1+.5, y1+.5 );
+		glEnd();		
+	}
+	else
+	{
+		glBegin( GL_LINES );
+		glVertex2f( x1+.5, y1+.5 );
+		glVertex2f( x2+.5, y2+.5 );
+		glEnd();
+	}
+}
+
+void CNFGTackPixel( short x1, short y1 )
+{
+	glBegin( GL_POINTS );
+	glVertex2f( x1, y1 );
+	glEnd();
+}
+
+void CNFGTackRectangle( short x1, short y1, short x2, short y2 )
+{
+	glBegin( GL_QUADS );
+	glVertex2f( x1, y1 );
+	glVertex2f( x2, y1 );
+	glVertex2f( x2, y2 );
+	glVertex2f( x1, y2 );
+	glEnd();
+}
+
+void CNFGTackPoly( RDPoint * points, int verts )
+{
+	int i;
+	glBegin( GL_TRIANGLE_FAN );
+	glVertex2f( points[0].x, points[0].y );
+	for( i = 1; i < verts; i++ )
+	{
+		glVertex2f( points[i].x, points[i].y );
+	}
+	glEnd();
+}
+
+void CNFGInternalResize( short x, short y ) { }
+
+
+#endif
diff --git a/redist/CNFGFunctions.h b/redist/CNFGFunctions.h
index 9ecb1bd..179a20b 100644
--- a/redist/CNFGFunctions.h
+++ b/redist/CNFGFunctions.h
@@ -1,4 +1,4 @@
-//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose.
+//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose.
 
 #ifndef _DRAWFUCNTIONS_H
 #define _DRAWFUCNTIONS_H
@@ -49,6 +49,11 @@ void HandleDestroy();
 //Internal function for resizing rasterizer for rasterizer-mode.
 void CNFGInternalResize( short x, short y ); //don't call this.
 
+//Not available on all systems.  Use The OGL portion with care.
+#ifdef CNFGOGL
+void   CNFGSetVSync( int vson );
+void * CNFGGetExtension( const char * extname );
+#endif
 
 #ifdef __cplusplus
 };
diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c
index a029419..c5da925 100644
--- a/redist/CNFGWinDriver.c
+++ b/redist/CNFGWinDriver.c
@@ -13,7 +13,6 @@ static HWND lsHWND;
 static HDC lsWindowHDC;
 static HDC lsHDC;
 
-
 #ifdef RASTERIZER
 #include "CNFGRasterizer.h"
 
@@ -32,14 +31,34 @@ static void InternalHandleResize();
 #endif
 
 
+#ifdef CNFGOGL
+#include <GL/gl.h>
+static HGLRC           hRC=NULL; 
+static void InternalHandleResize() { }
+void CNFGSwapBuffers()
+{
+	SwapBuffers(lsWindowHDC);
+}
+#endif
+
 void CNFGGetDimensions( short * x, short * y )
 {
+	static int lastx, lasty;
+	RECT window;
+	GetClientRect( lsHWND, &window );
+	bufferx = ( window.right - window.left);
+	buffery = ( window.bottom - window.top);
+	if( bufferx != lastx || buffery != lasty )
+	{
+		lastx = bufferx;
+		lasty = buffery;
+		InternalHandleResize();
+	}
 	*x = bufferx;
 	*y = buffery;
 }
 
 
-
 void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 {
 	RECT r;
@@ -126,9 +145,49 @@ void CNFGSetup( const char * name_of_window, int width, int height )
 		hInstance,
 		NULL);               //no parameters to pass
 
-
 	lsWindowHDC = GetDC( lsHWND );
 
+#ifdef CNFGOGL
+	//From NeHe
+	static  PIXELFORMATDESCRIPTOR pfd =
+	{
+		sizeof(PIXELFORMATDESCRIPTOR),
+		1,
+		PFD_DRAW_TO_WINDOW |
+		PFD_SUPPORT_OPENGL |
+		PFD_DOUBLEBUFFER,
+		PFD_TYPE_RGBA,
+		32,
+		0, 0, 0, 0, 0, 0, 
+		0,
+		0,
+		0,
+		0, 0, 0, 0,
+		16,
+		0,
+		0,
+		PFD_MAIN_PLANE,
+		0,
+		0, 0, 0
+	};
+	GLuint      PixelFormat = ChoosePixelFormat( lsWindowHDC, &pfd );
+	if( !SetPixelFormat( lsWindowHDC, PixelFormat, &pfd ) )
+	{
+		MessageBox( 0, "Could not create PFD for OpenGL Context\n", 0, 0 );
+		exit( -1 );
+	}
+	if (!(hRC=wglCreateContext(lsWindowHDC)))                   // Are We Able To Get A Rendering Context?
+	{
+		MessageBox( 0, "Could not create OpenGL Context\n", 0, 0 );
+		exit( -1 );
+	}
+	if(!wglMakeCurrent(lsWindowHDC,hRC))                        // Try To Activate The Rendering Context
+	{
+		MessageBox( 0, "Could not current OpenGL Context\n", 0, 0 );
+		exit( -1 );
+	}
+#endif
+
 	lsHDC = CreateCompatibleDC( lsWindowHDC );
 	lsBitmap = CreateCompatibleBitmap( lsWindowHDC, bufferx, buffery );
 	SelectObject( lsHDC, lsBitmap );
@@ -182,6 +241,7 @@ void CNFGHandleInput()
 	}
 }
 
+#ifndef CNFGOGL
 
 #ifndef RASTERIZER
 
@@ -237,8 +297,7 @@ void CNFGClearFrame()
 	DeleteObject( lsClearBrush  );
 	lsClearBrush = CreateSolidBrush( CNFGBGColor );
 	SelectObject( lsHDC, lsClearBrush );
-
-	FillRect( lsHDC, &r, lsClearBrush );
+	FillRect( lsHDC, &r, lsClearBrush);
 }
 
 void CNFGTackPoly( RDPoint * points, int verts )
@@ -287,3 +346,5 @@ void CNFGSwapBuffers()
 void CNFGInternalResize( short bufferx, short  buffery ) { }
 #endif
 
+#endif
+
diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c
index 8a8904a..ebaed91 100644
--- a/redist/CNFGXDriver.c
+++ b/redist/CNFGXDriver.c
@@ -1,4 +1,4 @@
-//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose.
+//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose.
 //portions from 
 //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html
 
@@ -25,6 +25,17 @@ Window CNFGWindow;
 Pixmap CNFGPixmap;
 GC     CNFGGC;
 GC     CNFGWindowGC;
+Visual * CNFGVisual;
+
+
+#ifdef CNFGOGL
+#include <GL/glx.h>
+#include <GL/glxext.h>
+
+GLXContext CNFGCtx;
+void * CNFGGetExtension( const char * extname ) { return glXGetProcAddressARB((const GLubyte *) extname); }
+#endif
+
 int FullScreen = 0;
 
 void CNFGGetDimensions( short * x, short * y )
@@ -86,7 +97,7 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no )
 		exit( 1 );
 	}
 
- 	Visual * visual = DefaultVisual(CNFGDisplay, screen);
+ 	CNFGVisual = DefaultVisual(CNFGDisplay, screen);
 	CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen);
 
 	if (XineramaQueryExtension(CNFGDisplay, &a, &b ) &&
@@ -117,7 +128,9 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no )
 
 	CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen),
 		xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height,
-		0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr);
+		0, CNFGWinAtt.depth, InputOutput, CNFGVisual, 
+		CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, 
+		&setwinattr);
 
 	XMapWindow(CNFGDisplay, CNFGWindow);
 	XSetInputFocus( CNFGDisplay, CNFGWindow,   RevertToParent, CurrentTime );
@@ -155,7 +168,31 @@ void CNFGSetup( const char * WindowName, int w, int h )
 	XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt );
 
 	int depth = CNFGWinAtt.depth;
-	CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 );
+	int screen = DefaultScreen(CNFGDisplay);
+ 	CNFGVisual = DefaultVisual(CNFGDisplay, screen);
+
+#ifdef CNFGOGL
+	int attribs[] = { GLX_RGBA,
+		GLX_DOUBLEBUFFER, 
+		GLX_RED_SIZE, 1,
+		GLX_GREEN_SIZE, 1,
+		GLX_BLUE_SIZE, 1,
+		GLX_DEPTH_SIZE, 1,
+		None };
+	XVisualInfo * vis = glXChooseVisual(CNFGDisplay, screen, attribs);
+	CNFGVisual = vis->visual;
+	depth = vis->depth;
+	CNFGCtx = glXCreateContext( CNFGDisplay, vis, NULL, True );
+#endif
+
+	XSetWindowAttributes attr;
+	attr.background_pixel = 0;
+	attr.border_pixel = 0;
+	attr.colormap = XCreateColormap( CNFGDisplay, RootWindow(CNFGDisplay, 0), CNFGVisual, AllocNone);
+	attr.event_mask = StructureNotifyMask | ExposureMask | KeyPressMask;
+	int mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask;
+
+	CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CNFGVisual, mask, &attr );
 	XMapWindow(CNFGDisplay, CNFGWindow);
 	XFlush(CNFGDisplay);
 
@@ -163,7 +200,12 @@ void CNFGSetup( const char * WindowName, int w, int h )
 
 	Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False );
 	XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 );
+
 	XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask );
+
+#ifdef CNFGOGL
+	glXMakeCurrent( CNFGDisplay, CNFGWindow, CNFGCtx );
+#endif
 }
 
 void CNFGHandleInput()
@@ -226,7 +268,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 	if( !xi )
 	{
 		int screen = DefaultScreen(CNFGDisplay);
-		Visual * visual = DefaultVisual(CNFGDisplay, screen);
 		depth = DefaultDepth(CNFGDisplay, screen)/8;
 //		xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 );
 //		lw = w;
@@ -236,7 +277,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 	if( lw != w || lh != h )
 	{
 		if( xi ) free( xi );
-		xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 );
+		xi = XCreateImage(CNFGDisplay, CNFGVisual, depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 );
 		lw = w;
 		lh = h;
 	}
@@ -247,7 +288,25 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 }
 
 
-#ifndef RASTERIZER
+#ifdef CNFGOGL
+
+void   CNFGSetVSync( int vson )
+{
+	void (*glfn)( int );
+	glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalMESA" );	if( glfn ) glfn( vson );
+	glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalSGI" );	if( glfn ) glfn( vson );
+	glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalEXT" );	if( glfn ) glfn( vson );
+}
+
+void CNFGSwapBuffers()
+{
+	glFlush();
+	glFinish();
+	glXSwapBuffers( CNFGDisplay, CNFGWindow );
+}
+#endif
+
+#if !defined( RASTERIZER ) && !defined( CNFGOGL)
 
 
 uint32_t CNFGColor( uint32_t RGB )
@@ -302,3 +361,4 @@ void CNFGInternalResize( short x, short y ) { }
 #include "CNFGRasterizer.h"
 #endif
 
+
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