Merge remote-tracking branch 'origin/master' into imu

20 files changed, 1063 insertions, 474 deletions

diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c
index 4b888aa..53a1d48 100644
--- a/src/epnp/epnp.c
+++ b/src/epnp/epnp.c
@@ -134,7 +134,7 @@ void epnp_choose_control_points(epnp *self) {
 	// Take C1, C2, and C3 from PCA on the reference points:
 	CvMat *PW0 = cvCreateMat(self->number_of_correspondences, 3, CV_64F);
 
-	double pw0tpw0[3 * 3] = {}, dc[3], uct[3 * 3];
+	double pw0tpw0[3 * 3] = {0}, dc[3], uct[3 * 3];
 	CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
 	CvMat DC = cvMat(3, 1, CV_64F, dc);
 	CvMat UCt = cvMat(3, 3, CV_64F, uct);
diff --git a/src/poser.c b/src/poser.c
index 3fb4fe8..9a0de24 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -1,8 +1,12 @@
 #include "math.h"
 #include <linmath.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <survive.h>
 
+#define _USE_MATH_DEFINES // for C
+#include <math.h>
+
 void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
 	if (poser_data->rawposeproc) {
 		poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata);
@@ -18,6 +22,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
 	} else {
 		const FLT up[3] = {0, 0, 1};
 
+		if (quatmagnitude(lighthouse_pose->Rot) == 0) {
+			SurviveContext *ctx = so->ctx;
+			SV_INFO("Pose func called with invalid pose.");
+			return;
+		}
+
 		// Assume that the space solved for is valid but completely arbitrary. We are going to do a few things:
 		// a) Using the gyro data, normalize it so that gravity is pushing straight down along Z
 		// c) Assume the object is at origin
@@ -44,8 +54,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
 		ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb);
 
 		// Now find the space with the same origin, but rotated so that gravity is up
-		SurvivePose lighthouse2objUp = {}, object2objUp = {};
-		quatfrom2vectors(object2objUp.Rot, so->activations.accel, up);
+		SurvivePose lighthouse2objUp = {0}, object2objUp = {0};
+		if (quatmagnitude(so->activations.accel)) {
+			quatfrom2vectors(object2objUp.Rot, so->activations.accel, up);
+		} else {
+			object2objUp.Rot[0] = 1.0;
+		}
 
 		// Calculate the pose of the lighthouse in this space
 		ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj);
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index c7d9033..b44225e 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -1,12 +1,13 @@
+#include "linmath.h"
 #include "survive_cal.h"
+#include <dclapack.h>
+#include <linmath.h>
 #include <math.h>
-#include <string.h>
-#include "linmath.h"
-#include <survive.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <dclapack.h>
-#include <linmath.h>
+#include <string.h>
+#include <survive.h>
+#include <survive_reproject.h>
 
 typedef struct
 {
@@ -53,7 +54,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
 			printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] );
 		}
 
-		SurvivePose additionalTx = {};
+		SurvivePose additionalTx = {0};
 
 		int lh, cycle;
 		FLT dz, dy, dx;
@@ -255,9 +256,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 			int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
 			if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
+			FLT out[2] = {};
+			survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
+
 			//Find out where our ray shoots forth from.
-			FLT ax = fs->angles[p][lh][0];
-			FLT ay = fs->angles[p][lh][1];
+			FLT ax = out[0];
+			FLT ay = out[1];
+
 			//NOTE: Inputs may never be output with cross product.
 			//Create a fictitious normalized ray.  Imagine the lighthouse is pointed
 			//straight in the +z direction, this is the lighthouse ray to the point.
@@ -338,8 +343,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 		if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
 		//Find out where our ray shoots forth from.
-		FLT ax = fs->angles[p][lh][0];
-		FLT ay = fs->angles[p][lh][1];
+		FLT out[2] = {};
+		survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
+
+		// Find out where our ray shoots forth from.
+		FLT ax = out[0];
+		FLT ay = out[1];
+
 		FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
 		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
 
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index c922b2e..9cdab45 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -1,12 +1,13 @@
+#include "linmath.h"
 #include "survive_cal.h"
+#include <dclapack.h>
+#include <linmath.h>
 #include <math.h>
-#include <string.h>
-#include "linmath.h"
-#include <survive.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <dclapack.h>
-#include <linmath.h>
+#include <string.h>
+#include <survive.h>
+#include <survive_reproject.h>
 
 // Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
 
@@ -127,7 +128,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
 		int LH_ID;
 
-		SurvivePose alignLh0ToXAxis = {};
+		SurvivePose alignLh0ToXAxis = {0};
 		for( LH_ID = 0; LH_ID < 2; LH_ID++ )
 		{
 			int i;
@@ -139,8 +140,10 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 				//Load all our valid points into something the LHFinder can use.
 				if( fs->lengths[i][LH_ID][0] > 0 )
 				{
-					S_in[0][max_hits] = fs->angles[i][LH_ID][0];
-					S_in[1][max_hits] = fs->angles[i][LH_ID][1];
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out);
+					S_in[0][max_hits] = out[0];
+					S_in[1][max_hits] = out[1];
 					X_in[0][max_hits] = so->sensor_locations[i*3+0];
 					X_in[1][max_hits] = so->sensor_locations[i*3+1];
 					X_in[2][max_hits] = so->sensor_locations[i*3+2];
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 401ea2a..c05450a 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -6,6 +6,7 @@
 
 #include <poser.h>
 #include <survive.h>
+#include <survive_reproject.h>
 
 #include "epnp/epnp.h"
 #include "linmath.h"
@@ -13,7 +14,7 @@
 #include "stdio.h"
 
 static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) {
-	SurvivePose rtn = {};
+	SurvivePose rtn = {0};
 	// std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl;
 	if (pnp->number_of_correspondences <= 3) {
 		SurviveContext *ctx = so->ctx;
@@ -27,6 +28,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
 
 	CvMat R = cvMat(3, 3, CV_64F, r);
 	CvMat T = cvMat(3, 1, CV_64F, rtn.Pos);
+
+	// Super degenerate inputs will project us basically right in the camera. Detect and reject
+	if (magnitude3d(rtn.Pos) < 0.25) {
+		return rtn;
+	}
+
 	// Requested output is camera -> world, so invert
 	if (cameraToWorld) {
 		FLT tmp[3];
@@ -65,7 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 
 		for (size_t i = 0; i < so->sensor_ct; i++) {
 			FLT *lengths = pdfs->lengths[i][lh];
-			FLT *ang = pdfs->angles[i][lh];
+			FLT *_ang = pdfs->angles[i][lh];
+			FLT ang[2];
+			survive_apply_bsd_calibration(so->ctx, lh, _ang, ang);
 			if (lengths[0] < 0 || lengths[1] < 0)
 				continue;
 
@@ -81,7 +90,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 		}
 
 		SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+
+		if (quatmagnitude(lighthouse2object.Rot) != 0.0) {
+			PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+		}
 
 		epnp_dtor(&pnp);
 	}
@@ -89,13 +101,15 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 	return 0;
 }
 
-static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene,
-								const PoserDataLight *lightData) {
-	int lh = lightData->lh;
+static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene, uint32_t timecode,
+								int lh) {
 	for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
-		if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, lightData->timecode,
+		if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode,
 												 sensor_idx, lh)) {
-			double *angles = scene->angles[sensor_idx][lh];
+			FLT *_angles = scene->angles[sensor_idx][lh];
+			FLT angles[2];
+			survive_apply_bsd_calibration(so->ctx, lh, _angles, angles);
+
 			epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0],
 									so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
 									tan(angles[0]), tan(angles[1]));
@@ -116,43 +130,55 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 		PoserDataLight *lightData = (PoserDataLight *)pd;
 
 		SurvivePose posers[2];
-		bool hasData[2] = {0, 0};
-		for (int lh = 0; lh < 1; lh++) {
+		int meas[2] = {0, 0};
+		for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) {
 			if (so->ctx->bsd[lh].PositionSet) {
 				epnp pnp = {.fu = 1, .fv = 1};
 				epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct);
 
-				add_correspondences(so, &pnp, scene, lightData);
+				add_correspondences(so, &pnp, scene, lightData->timecode, lh);
+				static int required_meas = -1;
+				if (required_meas == -1)
+					required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4);
 
-				if (pnp.number_of_correspondences > 4) {
+				if (pnp.number_of_correspondences > required_meas) {
 
-					SurvivePose pose = solve_correspondence(so, &pnp, false);
+					SurvivePose objInLh = solve_correspondence(so, &pnp, false);
+					if (quatmagnitude(objInLh.Rot) != 0) {
+						SurvivePose *lh2world = &so->ctx->bsd[lh].Pose;
 
-					SurvivePose txPose = {};
-					quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos);
-					for (int i = 0; i < 3; i++) {
-						txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i];
+						SurvivePose txPose = {.Rot = {1}};
+						ApplyPoseToPose(&txPose, lh2world, &objInLh);
+						posers[lh] = txPose;
+						meas[lh] = pnp.number_of_correspondences;
 					}
-
-					quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot);
-
-					posers[lh] = txPose;
-					hasData[lh] = 1;
 				}
 
 				epnp_dtor(&pnp);
 			}
 		}
 
-		if (hasData[0] && hasData[1]) {
+		if (meas[0] > 0 && meas[1] > 0) {
 			SurvivePose interpolate = {0};
-			for (size_t i = 0; i < 3; i++) {
-				interpolate.Pos[i] = (posers[0].Pos[i] + posers[1].Pos[i]) / 2.;
+			bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am
+
+			if (winnerTakesAll) {
+				int winner = meas[0] > meas[1] ? 0 : 1;
+				PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]);
+			} else {
+				double a, b;
+				a = meas[0] * meas[0];
+				b = meas[1] * meas[1];
+
+				double t = a + b;
+				for (size_t i = 0; i < 3; i++) {
+					interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t);
+				}
+				quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t));
+				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
 			}
-			quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, .5);
-			PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
 		} else {
-			if (hasData[lightData->lh])
+			if (meas[lightData->lh])
 				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]);
 		}
 		return 0;
diff --git a/src/poser_sba.c b/src/poser_sba.c
index efdeeef..1dbc820 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -3,6 +3,7 @@
 #define USE_DOUBLE
 #endif
 
+#include <malloc.h>
 #include <sba/sba.h>
 
 #include "poser.h"
@@ -18,7 +19,6 @@
 #include "survive_reproject.h"
 
 typedef struct {
-	survive_calibration_config calibration_config;
 	PoserData *pdfs;
 	SurviveObject *so;
 	SurvivePose obj_pose;
@@ -32,26 +32,40 @@ typedef struct {
 } sba_context_single_sweep;
 
 typedef struct SBAData {
-  int last_acode;
-  int last_lh;
-  int failures_to_reset;
-  int failures_to_reset_cntr;
-  SurviveIMUTracker tracker;
-  bool useIMU;
+	int last_acode;
+	int last_lh;
+
+	int failures_to_reset;
+	int failures_to_reset_cntr;
+	int successes_to_reset;
+	int successes_to_reset_cntr;
+
+	FLT max_error;
+
+	FLT sensor_variance;
+	FLT sensor_variance_per_second;
+	int sensor_time_window;
+
+	int required_meas;
+
+	SurviveIMUTracker tracker;
+	bool useIMU;
+
+	SurviveObject *so;
 } SBAData;
 
-void metric_function(int j, int i, double *aj, double *xij, void *adata) {
+static void metric_function(int j, int i, double *aj, double *xij, void *adata) {
 	sba_context *ctx = (sba_context *)(adata);
 	SurviveObject *so = ctx->so;
 
 	SurvivePose obj2world = ctx->obj_pose;
-	FLT sensorInWorld[3] = {};
+	FLT sensorInWorld[3] = {0};
 	ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]);
-	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld,
-											xij);
+	survive_calibration_config cfg = so->ctx->calibration_config;
+	survive_reproject_from_pose_with_config(so->ctx, &cfg, j, (SurvivePose *)aj, sensorInWorld, xij);
 }
 
-size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) {
+static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) {
 	size_t measCount = 0;
 	size_t size = so->sensor_ct * NUM_LIGHTHOUSES; // One set per lighthouse
 	for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
@@ -72,35 +86,29 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *
 	return measCount;
 }
 
-size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl,
-											   SurviveSensorActivations *scene, char *vmask, double *meas, int acode,
-											   int lh) {
-	size_t rtn = 0;
-
-	for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
-		const uint32_t *data_timecode = scene->timecode[sensor][lh];
-		if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) {
-			double *a = scene->angles[sensor][lh];
-			vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
-			meas[rtn++] = a[acode & 0x1];
-		} else {
-			vmask[sensor * NUM_LIGHTHOUSES + lh] = 0;
-		}
-	}
-
-	return rtn;
-}
-
-size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene,
-								  char *vmask, double *meas) {
+static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask,
+										 double *meas, double *cov) {
 	size_t rtn = 0;
+	SurviveObject *so = d->so;
 
 	for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
 		for (size_t lh = 0; lh < 2; lh++) {
-			if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, pdl->timecode,
-													 sensor, lh)) {
-				double *a = scene->angles[sensor][lh];
+			if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) {
+				const double *a = scene->angles[sensor][lh];
+				// FLT a[2];
+				// survive_apply_bsd_calibration(so->ctx, lh, _a, a);
 				vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
+
+				if (cov) {
+					*(cov++) = d->sensor_variance +
+							   abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second /
+								   (double)so->timebase_hz;
+					*(cov++) = 0;
+					*(cov++) = 0;
+					*(cov++) = d->sensor_variance +
+							   abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second /
+								   (double)so->timebase_hz;
+				}
 				meas[rtn++] = a[0];
 				meas[rtn++] = a[1];
 			} else {
@@ -125,7 +133,7 @@ typedef struct {
 	SurvivePose poses;
 } sba_set_position_t;
 
-void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) {
+static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) {
 	sba_set_position_t *user = _user;
 	assert(user->hasInfo == false);
 	user->hasInfo = 1;
@@ -133,7 +141,7 @@ void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_po
 }
 void *GetDriver(const char *name);
 
-void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) {
+static void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
 
@@ -153,11 +161,11 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
 
 	FLT out[2];
-	survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out);
+	survive_reproject_from_pose(so->ctx, lh, camera, xyz, out);
 	*xij = out[acode];
 }
 
-void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
+static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
 	int lh = j & 1;
@@ -174,116 +182,27 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
-	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
-}
-
-static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl,
-													 SurviveObject *so, SurviveSensorActivations *scene, int acode,
-													 int lh, int max_iterations /* = 50*/,
-													 double max_reproj_error /* = 0.005*/) {
-	double *covx = 0;
-
-	char *vmask = alloca(sizeof(char) * so->sensor_ct);
-	double *meas = alloca(sizeof(double) * so->sensor_ct);
-	size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh);
-
-	static int failure_count = 500;
-	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) {
-		if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
-			SurviveContext *ctx = so->ctx;
-			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
-			failure_count = 0;
-		}
-		return -1;
-	}
-	failure_count = 0;
-
-	SurvivePose soLocation = so->OutPose;
-	bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]);
-
-	{
-		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
-		PoserCB driver = (PoserCB)GetDriver(subposer);
-		SurviveContext *ctx = so->ctx;
-		if (driver) {
-			PoserData hdr = pdl->hdr;
-			memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
-			pdl->hdr.pt = hdr.pt;
-			pdl->hdr.rawposeproc = sba_set_position;
-
-			sba_set_position_t locations = {};
-			pdl->hdr.userdata = &locations;
-			driver(so, &pdl->hdr);
-			pdl->hdr = hdr;
-
-			if (locations.hasInfo == false) {
-
-				return -1;
-			} else if (locations.hasInfo) {
-				soLocation = locations.poses;
-			}
-		} else {
-			SV_INFO("Not using a seed poser for SBA; results will likely be way off");
-		}
-	}
-
-	double opts[SBA_OPTSSZ] = {};
-	double info[SBA_INFOSZ] = {};
-
-	sba_context_single_sweep ctx = {.hdr = {options, &pdl->hdr, so}, .acode = acode, .lh = lh};
-
-	opts[0] = SBA_INIT_MU;
-	opts[1] = SBA_STOP_THRESH;
-	opts[2] = SBA_STOP_THRESH;
-	opts[3] = SBA_STOP_THRESH;
-	opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
-	opts[4] = 0.0;
-
-	int status = sba_str_levmar(1, // Number of 3d points
-								0, // Number of 3d points to fix in spot
-								so->sensor_ct, vmask,
-								soLocation.Pos, // Reads as the full pose though
-								7,				// pnp -- SurvivePose
-								meas,			// x* -- measurement data
-								0,				// cov data
-								1,				// mnp -- 2 points per image
-								str_metric_function_single_sweep,
-								0,				// jacobia of metric_func
-								&ctx,			// user data
-								max_iterations, // Max iterations
-								0,				// verbosity
-								opts,			// options
-								info);			// info
-
-	if (status > 0) {
-		quatnormalize(soLocation.Rot, soLocation.Rot);
-		PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
-
-		SurviveContext *ctx = so->ctx;
-		// Docs say info[0] should be divided by meas; I don't buy it really...
-		static int cnt = 0;
-		if (cnt++ > 1000 || meas_size < 8) {
-		  SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size);
-			SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
-			cnt = 0;
-		}
-	}
-
-	return info[1] / meas_size * 2;
+	survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij);
 }
 
-static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
-										SurviveSensorActivations *scene, int max_iterations /* = 50*/,
-										double max_reproj_error /* = 0.005*/) {
+static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
+										int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
+	SurviveObject *so = d->so;
 
 	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
 	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
-	size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas);
+	double *cov =
+		d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0;
+	size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov);
 
 	static int failure_count = 500;
-	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) {
-		if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
+	bool hasAllBSDs = true;
+	for (int lh = 0; lh < so->ctx->activeLighthouses; lh++)
+		hasAllBSDs &= so->ctx->bsd[lh].PositionSet;
+
+	if (!hasAllBSDs || meas_size < d->required_meas) {
+		if (hasAllBSDs && failure_count++ == 500) {
 			SurviveContext *ctx = so->ctx;
 			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
 			failure_count = 0;
@@ -295,10 +214,9 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 	SurvivePose soLocation = so->OutPose;
 	bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
 
-	if(d->failures_to_reset_cntr == 0 || currentPositionValid == 0)
-	{
-		SurviveContext *ctx = so->ctx;	  
-	  SV_INFO("Running seed poser");
+	if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) {
+		SurviveContext *ctx = so->ctx;
+		// SV_INFO("Must rerun seed poser");
 		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
 		PoserCB driver = (PoserCB)GetDriver(subposer);
 
@@ -308,26 +226,27 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 			pdl->hdr.pt = hdr.pt;
 			pdl->hdr.rawposeproc = sba_set_position;
 
-			sba_set_position_t locations = {};
+			sba_set_position_t locations = {0};
 			pdl->hdr.userdata = &locations;
 			driver(so, &pdl->hdr);
 			pdl->hdr = hdr;
 
 			if (locations.hasInfo == false) {
-
 				return -1;
 			} else if (locations.hasInfo) {
 				soLocation = locations.poses;
 			}
+
+			d->successes_to_reset_cntr = d->successes_to_reset;
 		} else {
 			SV_INFO("Not using a seed poser for SBA; results will likely be way off");
 		}
 	}
 
-	double opts[SBA_OPTSSZ] = {};
-	double info[SBA_INFOSZ] = {};
+	double opts[SBA_OPTSSZ] = {0};
+	double info[SBA_INFOSZ] = {0};
 
-	sba_context ctx = {options, &pdl->hdr, so};
+	sba_context ctx = {&pdl->hdr, so};
 
 	opts[0] = SBA_INIT_MU;
 	opts[1] = SBA_STOP_THRESH;
@@ -342,7 +261,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 								soLocation.Pos, // Reads as the full pose though
 								7,				// pnp -- SurvivePose
 								meas,			// x* -- measurement data
-								0,				// cov data
+								cov,			// cov data
 								2,				// mnp -- 2 points per image
 								str_metric_function,
 								0,				// jacobia of metric_func
@@ -352,8 +271,16 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 								opts,			// options
 								info);			// info
 
-	if (status > 0) {
-	  d->failures_to_reset_cntr = d->failures_to_reset;
+	if (currentPositionValid) {
+		FLT distp[3];
+		sub3d(distp, so->OutPose.Pos, soLocation.Pos);
+		FLT distance = magnitude3d(distp);
+		;
+		if (distance > 1.)
+			status = -1;
+	}
+	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
+		d->failures_to_reset_cntr = d->failures_to_reset;
 		quatnormalize(soLocation.Rot, soLocation.Rot);
 		PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
 	}
@@ -362,8 +289,8 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
 		static int cnt = 0;
-		if (cnt++ > 1000 || meas_size < 8) {
-		  SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
+		if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) {
+			SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
 			SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
 			cnt = 0;
 		}
@@ -373,15 +300,15 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 }
 
 // Optimizes for LH position assuming object is posed at 0
-static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so,
-					  int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
+static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/,
+					  double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
 
 	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
 	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
 	size_t meas_size = construct_input(so, pdfs, vmask, meas);
 
-	sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose},
+	sba_context sbactx = {&pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose},
 						  .obj_pose = so->OutPose};
 
 	{
@@ -400,7 +327,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 		} else {
 			SV_INFO("Not using a seed poser for SBA; results will likely be way off");
 			for (int i = 0; i < 2; i++) {
-				so->ctx->bsd[i].Pose = (SurvivePose){};
+				so->ctx->bsd[i].Pose = (SurvivePose){0};
 				so->ctx->bsd[i].Pose.Rot[0] = 1.;
 			}
 		}
@@ -408,8 +335,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 		// PoserCharlesSlow(so, (PoserData *)pdfs);
 	}
 
-	double opts[SBA_OPTSSZ] = {};
-	double info[SBA_INFOSZ] = {};
+	double opts[SBA_OPTSSZ] = {0};
+	double info[SBA_INFOSZ] = {0};
 
 	opts[0] = SBA_INIT_MU;
 	opts[1] = SBA_STOP_THRESH;
@@ -419,7 +346,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 	opts[4] = 0.0;
 
 	int status = sba_mot_levmar(so->sensor_ct,						  // number of 3d points
-								NUM_LIGHTHOUSES,					  // Number of cameras -- 2 lighthouses
+								so->ctx->activeLighthouses,			  // Number of cameras -- 2 lighthouses
 								0,									  // Number of cameras to not modify
 								vmask,								  // boolean vis mask
 								(double *)&sbactx.camera_params[0],   // camera parameters
@@ -436,9 +363,13 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 								info);			// info
 
 	if (status >= 0) {
-		SurvivePose additionalTx = {};
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose);
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose);
+		SurvivePose additionalTx = {0};
+		for (int i = 0; i < so->ctx->activeLighthouses; i++) {
+			if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) {
+				PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i],
+											   &sbactx.obj_pose);
+			}
+		}
 	} else {
 		SurviveContext *ctx = so->ctx;
 		SV_INFO("SBA was unable to run %d", status);
@@ -457,14 +388,31 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 }
 
 int PoserSBA(SurviveObject *so, PoserData *pd) {
+	SurviveContext *ctx = so->ctx;
 	if (so->PoserData == 0) {
 		so->PoserData = calloc(1, sizeof(SBAData));
 		SBAData *d = so->PoserData;
-		d->failures_to_reset_cntr = 0; 
-		d->failures_to_reset = 30;
+		d->failures_to_reset_cntr = 0;
+		d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
+		d->successes_to_reset_cntr = 0;
+		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100);
+
+		d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
+		d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
+		d->sensor_time_window =
+			survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
+		d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001);
+		d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
+		d->so = so;
+
+		SV_INFO("Initializing SBA:");
+		SV_INFO("\tsba-required-meas: %d", d->required_meas);
+		SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance);
+		SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second);
+		SV_INFO("\tsba-time-window: %d", d->sensor_time_window);
+		SV_INFO("\tsba-max-error: %f", d->max_error);
 	}
 	SBAData *d = so->PoserData;
-	SurviveContext *ctx = so->ctx;
 	switch (pd->pt) {
 	case POSERDATA_LIGHT: {
 		// No poses if calibration is ongoing
@@ -476,17 +424,21 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		// only process sweeps
 		FLT error = -1;
 		if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
-			survive_calibration_config config = *survive_calibration_default_config();
-			error = run_sba_find_3d_structure(d, config, lightData, so, scene, 50, .5);
+			error = run_sba_find_3d_structure(d, lightData, scene, 100, .5);
+
 			d->last_lh = lightData->lh;
 			d->last_acode = lightData->acode;
 		}
 
 		if (error < 0) {
-			if(d->failures_to_reset_cntr > 0)
-			  d->failures_to_reset_cntr--;			
-		} else if(d->useIMU) {
-		  survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+			if (d->failures_to_reset_cntr > 0)
+				d->failures_to_reset_cntr--;
+		} else {
+			if (d->useIMU) {
+				survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+			}
+			if (d->successes_to_reset_cntr > 0)
+				d->successes_to_reset_cntr--;
 		}
 
 		return 0;
@@ -494,9 +446,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 	case POSERDATA_FULL_SCENE: {
 		SurviveContext *ctx = so->ctx;
 		PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd);
-		survive_calibration_config config = *survive_calibration_default_config();
-		SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config));
-		double error = run_sba(config, pdfs, so, 50, .005);
+		double error = run_sba(pdfs, so, 100, .005);
 		// std::cerr << "Average reproj error: " << error << std::endl;
 		return 0;
 	}
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 2d3f802..4628207 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -13,6 +13,8 @@
 #include <stdlib.h>
 #else
 #include <malloc.h> //for alloca
+#include <survive_reproject.h>
+
 #endif
 
 
@@ -1443,7 +1445,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		lighthousePose.Pos[1] = refinedEstimateGd.y;
 		lighthousePose.Pos[2] = refinedEstimateGd.z;
 
-		SurvivePose assumedObj = {};
+		SurvivePose assumedObj = {0};
 		FLT negZ[3] = {0, 0, 1};
 		quatfrom2vectors(assumedObj.Rot, toriData->down, negZ);
 
@@ -1786,8 +1788,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					to->sensor[sensorCount].point.x = point[0];
 					to->sensor[sensorCount].point.y = point[1];
 					to->sensor[sensorCount].point.z = point[2];
-					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
-					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out);
+
+					to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2;   // lighthouse 0, angle 1 (vertical)
 
 					sensorCount++;
 				}
@@ -1822,8 +1828,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					to->sensor[sensorCount].point.x = point[0];
 					to->sensor[sensorCount].point.y = point[1];
 					to->sensor[sensorCount].point.z = point[2];
-					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
-					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out);
+
+					to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2;   // lighthosue 0, angle 1 (vertical)
 					sensorCount++;
 				}
 			}
diff --git a/src/survive.c b/src/survive.c
index e6bf69f..1a782a2 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -36,8 +36,8 @@ static void survivefault(struct SurviveContext *ctx, const char *fault) {
 }
 
 static void survivenote(struct SurviveContext *ctx, const char *fault) {
-  survive_recording_info_process(ctx, fault);
-  fprintf(stderr, "Info: %s\n", fault);
+	survive_recording_info_process(ctx, fault);
+	fprintf(stderr, "Info: %s\n", fault);
 }
 
 static void *button_servicer(void *context) {
@@ -101,16 +101,28 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	}
 #endif
 #ifdef MANUAL_REGISTRATION
-// note: this manual registration is currently only in use on builds using Visual Studio.
+	// note: this manual registration is currently only in use on builds using Visual Studio.
 
+	static int did_manual_driver_registration = 0;
+	if (did_manual_driver_registration == 0) {
 #define MANUAL_DRIVER_REGISTRATION(func)                                                                               \
 	int func(SurviveObject *so, PoserData *pd);                                                                        \
 	RegisterDriver(#func, &func);
 
-	MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow)
-	MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho)
-	MANUAL_DRIVER_REGISTRATION(PoserDummy)
-	MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
+		MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow)
+		MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho)
+		MANUAL_DRIVER_REGISTRATION(PoserDummy)
+		MANUAL_DRIVER_REGISTRATION(PoserEPNP)
+		MANUAL_DRIVER_REGISTRATION(PoserSBA)
+
+		MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
+		MANUAL_DRIVER_REGISTRATION(DriverRegPlayback)
+
+		MANUAL_DRIVER_REGISTRATION(DisambiguatorCharles)
+		MANUAL_DRIVER_REGISTRATION(DisambiguatorStateBased)
+		MANUAL_DRIVER_REGISTRATION(DisambiguatorTurvey)
+		did_manual_driver_registration = 1;
+	}
 #endif
 
 	SurviveContext *ctx = calloc(1, sizeof(SurviveContext));
@@ -159,11 +171,18 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 			}
 
 			if (vartoupdate) {
-				if (av + 1 == argvend) {
-					fprintf(stderr, "Error: expected parameter after %s\n", *av);
-					showhelp = 1;
+				const char *name = *av + 2; // Skip the '--';
+				bool flagArgument = (av + 1 == argvend) || av[1][0] == '-';
+
+				if (flagArgument) {
+					bool value = strncmp("no-", name, 3) != 0;
+					if (value == 0) {
+						name += 3; // Skip "no-"
+					}
+					survive_configi(ctx, name, SC_OVERRIDE | SC_SET, value);
 				} else {
-					survive_configs(ctx, *av + 2, SC_OVERRIDE | SC_SET, *(av + 1));
+					const char *value = *(av + 1);
+					survive_configs(ctx, name, SC_OVERRIDE | SC_SET, value);
 					av++;
 				}
 			}
@@ -177,7 +196,10 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 		fprintf(stderr, " -p [poser]              - use a specific defaultposer.\n");
 		fprintf(stderr, " -l [lighthouse count]   - use a specific number of lighthoses.\n");
 		fprintf(stderr, " -c [config file]        - set config file\n");
-		fprintf(stderr, " -p [lighthouse count]   - use a specific number of lighthoses.\n");
+		fprintf(stderr, " --record [log file]     - Write all events to the given record file.\n");
+		fprintf(stderr, " --playback [log file]   - Read events from the given file instead of USB devices.\n");
+		fprintf(stderr, " --playback-factor [f]   - Time factor of playback -- 1 is run at the same timing as "
+						"original, 0 is run as fast as possible.\n");
 		return 0;
 	}
 
@@ -196,10 +218,14 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	ctx->configfunction = survive_default_htc_config_process;
 	ctx->rawposeproc = survive_default_raw_pose_process;
 
+	ctx->calibration_config = survive_calibration_config_ctor();
+	ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All);
+
 	return ctx;
 }
 
-static void *setup_func_by_name(SurviveContext *ctx, const char *name, const char *configname, const char *configdef) {
+void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *configname, const char *configdef,
+						int verbose) {
 	const char *Preferred = survive_configs(ctx, configname, SC_SETCONFIG, configdef);
 	const char *DriverName = 0;
 	const char *picked = 0;
@@ -207,12 +233,14 @@ static void *setup_func_by_name(SurviveContext *ctx, const char *name, const cha
 	void *func = 0;
 	int prefixLen = strlen(name);
 
-	SV_INFO("Available %s:", name);
+	if (verbose > 1)
+		SV_INFO("Available %ss:", name);
 	while ((DriverName = GetDriverNameMatching(name, i++))) {
 		void *p = GetDriver(DriverName);
 
 		bool match = strcmp(DriverName, Preferred) == 0 || strcmp(DriverName + prefixLen, Preferred) == 0;
-		SV_INFO("\t%c%s", match ? '*' : ' ', DriverName + prefixLen);
+		if (verbose > 1)
+			SV_INFO("\t%c%s", match ? '*' : ' ', DriverName + prefixLen);
 		if (!func || match) {
 			func = p;
 			picked = (DriverName + prefixLen);
@@ -221,7 +249,10 @@ static void *setup_func_by_name(SurviveContext *ctx, const char *name, const cha
 	if (!func) {
 		SV_ERROR("Error.  Cannot find any valid %s.", name);
 	}
-	SV_INFO("Totals %d %ss.  Using %s.", i - 1, name, picked);
+	if (verbose > 1)
+		SV_INFO("Totals %d %ss.", i - 1, name);
+	if (verbose > 0)
+		SV_INFO("Using '%s' for %s", picked, configname);
 
 	return func;
 }
@@ -230,6 +261,8 @@ int survive_startup(SurviveContext *ctx) {
 	int r = 0;
 	int i = 0;
 
+	survive_install_recording(ctx);
+
 	// initialize the button queue
 	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
 	ctx->buttonQueue.buttonservicesem = OGCreateSema();
@@ -237,15 +270,13 @@ int survive_startup(SurviveContext *ctx) {
 	// start the thread to process button data
 	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
 
-	PoserCB PreferredPoserCB = setup_func_by_name(ctx, "Poser", "defaultposer", "PoserTurveyTori");
-	ctx->lightcapfunction = setup_func_by_name(ctx, "Disambiguator", "disambiguator", "Turvey");
+	PoserCB PreferredPoserCB = GetDriverByConfig(ctx, "Poser", "defaultposer", "SBA", 2);
+	ctx->lightcapfunction = GetDriverByConfig(ctx, "Disambiguator", "disambiguator", "Turvey", 2);
 
 	const char *DriverName;
 
 	i = 0;
 
-	survive_install_recording(ctx);
-
 	while ((DriverName = GetDriverNameMatching("DriverReg", i++))) {
 		DeviceDriver dd = GetDriver(DriverName);
 		SV_INFO("Loading driver %s (%p) (%d)", DriverName, dd, i);
@@ -263,6 +294,40 @@ int survive_startup(SurviveContext *ctx) {
 
 	ctx->state = SURVIVE_RUNNING;
 
+	int calibrateMandatory = survive_configi(ctx, "calibrate", SC_GET, 0);
+	int calibrateForbidden = survive_configi(ctx, "calibrate", SC_GET, 1) == 0;
+	if (calibrateMandatory && calibrateForbidden) {
+		SV_INFO("Contradictory settings --calibrate and --no-calibrate specified. Switching to normal behavior.");
+		calibrateMandatory = calibrateForbidden = 0;
+	}
+
+	if (!calibrateForbidden) {
+		bool isCalibrated = true;
+		for (int i = 0; i < ctx->activeLighthouses; i++) {
+			isCalibrated &= ctx->bsd[i].PositionSet;
+		}
+
+		if (!isCalibrated) {
+			SV_INFO("Uncalibrated configuration detected. Attaching calibration. Please don't move tracked objects for "
+					"the duration of calibration. Pass '--no-calibrate' to skip calibration");
+		} else {
+			SV_INFO("Calibration requested. Previous calibration will be overwritten.");
+		}
+
+		bool doCalibrate = isCalibrated == false || calibrateMandatory;
+
+		if (doCalibrate) {
+			survive_cal_install(ctx);
+		}
+	}
+
+	// If lighthouse positions are known, broadcast them
+	for (int i = 0; i < ctx->activeLighthouses; i++) {
+		if (ctx->bsd[i].PositionSet) {
+			ctx->lighthouseposeproc(ctx, i, &ctx->bsd[i].Pose, 0);
+		}
+	}
+
 	return 0;
 }
 
@@ -493,4 +558,9 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in
 	return len;
 }
 
+const char *survive_object_codename(SurviveObject *so) { return so->codename; }
+int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; }
+const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; }
+const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
+
 #endif
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 3367aa0..3015b68 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -13,12 +13,13 @@
 #include "survive_internal.h"
 #include "survive_reproject.h"
 
+#include <assert.h>
+#include <linmath.h>
 #include <math.h>
+#include <poser.h>
 #include <string.h>
 #include <sys/stat.h>
 #include <sys/types.h>
-#include <linmath.h>
-#include <assert.h>
 
 #include "survive_config.h"
 
@@ -61,19 +62,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	//print_lighthouse_info_v6(&v6);
 
 	b->BaseStationID = v6.id;
-	b->fcalphase[0] = v6.fcal_0_phase;
-	b->fcalphase[1] = v6.fcal_1_phase;
-	b->fcaltilt[0] = tan(v6.fcal_0_tilt);
-	b->fcaltilt[1] = tan(v6.fcal_1_tilt);  //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
-	b->fcalcurve[0] = v6.fcal_0_curve;
-	b->fcalcurve[1] = v6.fcal_1_curve;
-	b->fcalgibpha[0] = v6.fcal_0_gibphase;
-	b->fcalgibpha[1] = v6.fcal_1_gibphase;
-	b->fcalgibmag[0] = v6.fcal_0_gibmag;
-	b->fcalgibmag[1] = v6.fcal_1_gibmag;
+	b->fcal.phase[0] = v6.fcal_0_phase;
+	b->fcal.phase[1] = v6.fcal_1_phase;
+	b->fcal.tilt[0] = (v6.fcal_0_tilt);
+	b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
+	b->fcal.curve[0] = v6.fcal_0_curve;
+	b->fcal.curve[1] = v6.fcal_1_curve;
+	b->fcal.gibpha[0] = v6.fcal_0_gibphase;
+	b->fcal.gibpha[1] = v6.fcal_1_gibphase;
+	b->fcal.gibmag[0] = v6.fcal_0_gibmag;
+	b->fcal.gibmag[1] = v6.fcal_1_gibmag;
 	b->accel[0] = v6.accel_dir_x;
 	b->accel[1] = v6.accel_dir_y;
 	b->accel[2] = v6.accel_dir_z;
+	b->mode = v6.mode_current;
 	b->OOTXSet = 1;
 
 	config_set_lighthouse(ctx->lh_config,b,id);
@@ -116,6 +118,9 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 
 void survive_cal_install( struct SurviveContext * ctx )
 {
+	if (ctx->calptr)
+		return;
+
 	int i;
 	struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
 
@@ -184,30 +189,8 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	const char * DriverName;
-//	const char * PreferredPoser = survive_configs(ctx, "configposer", "PoserCharlesSlow");
-	const char * PreferredPoser = survive_configs(ctx, "configposer", SC_SETCONFIG, "PoserTurveyTori");
-
-	SV_INFO( "Trying to load poser %s for cal.", PreferredPoser );
-	PoserCB SelectedPoserCB = 0;
-	const char * SelectedPoserName = 0;
-	i = 0;	
-	while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
-	{
-		PoserCB p = GetDriver( DriverName );
-		if( !SelectedPoserCB )
-		{
-			SelectedPoserCB = p;
-			SelectedPoserName = DriverName;
-		}
-		int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
-		if( ThisPoser )
-		{
-			SelectedPoserCB = p;
-			SelectedPoserName = DriverName;
-		}
-	}
-	cd->ConfigPoserFn = SelectedPoserCB;
-	SV_INFO( "Got config poser: %s (%p)", SelectedPoserName, cd->ConfigPoserFn );
+	cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0);
+
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
 	ctx->calptr = cd;
@@ -618,7 +601,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 	for( obj = 0; obj < cd->numPoseObjects; obj++ )
 	{
 		int i, j;
-		PoserDataFullScene fsd = {};
+		PoserDataFullScene fsd = {0};
 		fsd.hdr.pt = POSERDATA_FULL_SCENE;
 		for( j = 0; j < NUM_LIGHTHOUSES; j++ )
 		for( i = 0; i < SENSORS_PER_OBJECT; i++ )
@@ -636,8 +619,8 @@ static void handle_calibration( struct SurviveCalData *cd )
 			}
 			fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
 			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.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];
 		}
 
diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c
index fbba888..83b3681 100644
--- a/src/survive_charlesbiguator.c
+++ b/src/survive_charlesbiguator.c
@@ -1,6 +1,7 @@
 //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
 //
 #include "survive_internal.h"
+#include <assert.h>
 #include <math.h> /* for sqrt */
 #include <stdint.h>
 #include <string.h>
@@ -15,12 +16,16 @@ static int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	if (acode & 1)
 		return -1;
 
-	return (acode >> 1) - 6;
+	int32_t rtn = (acode >> 1) - 6;
+	if (rtn > 7 || rtn < 0) {
+		return -1;
+	}
+	return rtn;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////The charles disambiguator.  Don't use this, mostly here for
-///debugging.///////////////////////////////////////////////////////
+/// debugging.///////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) {
@@ -169,21 +174,23 @@ void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) {
 
 		int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz.
 		int acode = decode_acode(so->last_sync_length[0], main_divisor);
-		int whichlh;
-		if (acode < 0)
-			whichlh = 1;
-		else
-			whichlh = (acode >> 2);
-		int32_t dl = so->last_sync_time[whichlh];
 
-		if (!so->did_handle_ootx)
-			HandleOOTX(ctx, so);
+		// If acode isn't right; don't even think of emitting an event
+		if (acode >= 0) {
+			int whichlh = (acode >> 2);
+			assert(whichlh <= 1);
+			int32_t dl = so->last_sync_time[whichlh];
 
-		int32_t offset_from = le->timestamp - dl + le->length / 2;
+			if (!so->did_handle_ootx)
+				HandleOOTX(ctx, so);
 
-		// Make sure pulse is in valid window
-		if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) {
-			ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+			int32_t offset_from = le->timestamp - dl + le->length / 2;
+
+			// Make sure pulse is in valid window
+			if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time &&
+				offset_from > so->pulse_in_clear_time) {
+				ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+			}
 		}
 	} else {
 		// printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time -
diff --git a/src/survive_config.c b/src/survive_config.c
index 7b68ae4..d67cd8e 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -119,12 +119,13 @@ void config_read_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8
 	FLT defaults[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
 	bsd->BaseStationID = config_read_uint32(cg, "id", 0);
+	bsd->mode = config_read_uint32(cg, "mode", 0);
 	config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7);
-	config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2);
-	config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2);
-	config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2);
-	config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2);
-	config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2);
+	config_read_float_array(cg, "fcalphase", bsd->fcal.phase, defaults, 2);
+	config_read_float_array(cg, "fcaltilt", bsd->fcal.tilt, defaults, 2);
+	config_read_float_array(cg, "fcalcurve", bsd->fcal.curve, defaults, 2);
+	config_read_float_array(cg, "fcalgibpha", bsd->fcal.gibpha, defaults, 2);
+	config_read_float_array(cg, "fcalgibmag", bsd->fcal.gibmag, defaults, 2);
 	bsd->PositionSet = config_read_uint32(cg, "PositionSet", 0);
 }
 
@@ -132,12 +133,13 @@ void config_set_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8_
 	config_group *cg = lh_config + idx;
 	config_set_uint32(cg, "index", idx);
 	config_set_uint32(cg, "id", bsd->BaseStationID);
+	config_set_uint32(cg, "mode", bsd->mode);
 	config_set_float_a(cg, "pose", &bsd->Pose.Pos[0], 7);
-	config_set_float_a(cg, "fcalphase", bsd->fcalphase, 2);
-	config_set_float_a(cg, "fcaltilt", bsd->fcaltilt, 2);
-	config_set_float_a(cg, "fcalcurve", bsd->fcalcurve, 2);
-	config_set_float_a(cg, "fcalgibpha", bsd->fcalgibpha, 2);
-	config_set_float_a(cg, "fcalgibmag", bsd->fcalgibmag, 2);
+	config_set_float_a(cg, "fcalphase", bsd->fcal.phase, 2);
+	config_set_float_a(cg, "fcaltilt", bsd->fcal.tilt, 2);
+	config_set_float_a(cg, "fcalcurve", bsd->fcal.curve, 2);
+	config_set_float_a(cg, "fcalgibpha", bsd->fcal.gibpha, 2);
+	config_set_float_a(cg, "fcalgibmag", bsd->fcal.gibmag, 2);
 	config_set_uint32(cg, "PositionSet", bsd->PositionSet);
 }
 
diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c
index 39b23b6..73e61a8 100644
--- a/src/survive_disambiguator.c
+++ b/src/survive_disambiguator.c
@@ -1,3 +1,21 @@
 #include "survive.h"
 
-void handle_lightcap(SurviveObject *so, LightcapElement *le) { so->ctx->lightcapfunction(so, le); }
+#include "survive_playback.h"
+#include <os_generic.h>
+#include <stdio.h>
+
+//#define LOG_LIGHTDATA
+
+void handle_lightcap(SurviveObject *so, LightcapElement *le) {
+	survive_recording_lightcap(so, le);
+#ifdef LOG_LIGHTDATA
+	static FILE *flog;
+	static double start = 0;
+	if (!flog) {
+		flog = fopen("lightcap.txt", "wb");
+		start = OGGetAbsoluteTime();
+	}
+	fprintf(flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime() - start, le->sensor_id, le->length, le->timestamp);
+#endif
+	so->ctx->lightcapfunction(so, le);
+}
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 86b119f..9120f41 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -16,7 +16,8 @@
 void * GetDriver( const char * name );
 const char * GetDriverNameMatching( const char * prefix, int place );
 void   ListDrivers();
-
+void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *configname, const char *configdef,
+						int verbose);
 
 #endif
 
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 9261bb5..d5d1c08 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -6,7 +6,6 @@
 #include <survive.h>
 
 #include <string.h>
-#include <sys/time.h>
 
 #include "survive_config.h"
 #include "survive_default_devices.h"
@@ -14,8 +13,17 @@
 #include "os_generic.h"
 #include "stdarg.h"
 
+#ifdef _WIN32
+typedef long ssize_t;
+#define SSIZE_MAX LONG_MAX
+
+ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream);
+ssize_t getline(char **lineptr, size_t *n, FILE *stream);
+#endif
+
 typedef struct SurviveRecordingData {
 	bool alwaysWriteStdOut;
+	bool writeRawLight;
 	FILE *output_file;
 } SurviveRecordingData;
 
@@ -96,6 +104,16 @@ void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, in
 					angle, lh);
 }
 
+void survive_recording_lightcap(SurviveObject *so, LightcapElement *le) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	if (recordingData->writeRawLight) {
+		write_to_output(recordingData, "%s C %d %u %u\n", so->codename, le->sensor_id, le->timestamp, le->length);
+	}
+}
+
 void survive_recording_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep,
 									 uint32_t timecode, uint32_t length, uint32_t lh) {
 	SurviveRecordingData *recordingData = so->ctx->recptr;
@@ -133,6 +151,7 @@ void survive_recording_light_process(struct SurviveObject *so, int sensor_id, in
 		LH_Axis = "Y";
 		break;
 	}
+
 	write_to_output(recordingData, "%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode,
 					timeinsweep, timecode, length, lh);
 }
@@ -154,6 +173,7 @@ struct SurvivePlaybackData {
 
 	FLT time_factor;
 	double next_time_us;
+	bool hasRawLight;
 };
 typedef struct SurvivePlaybackData SurvivePlaybackData;
 
@@ -190,8 +210,31 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
 	return 0;
 }
 
-static int parse_and_run_lightcode(const char *line,
-								   SurvivePlaybackData *driver) {
+static int parse_and_run_rawlight(const char *line, SurvivePlaybackData *driver) {
+	driver->hasRawLight = 1;
+
+	char dev[10];
+	char op[10];
+	LightcapElement le;
+	int rr = sscanf(line, "%s %s %hhu %u %hu\n", dev, op, &le.sensor_id, &le.timestamp, &le.length);
+
+	SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+	if (!so) {
+		static bool display_once = false;
+		SurviveContext *ctx = driver->ctx;
+		if (display_once == false) {
+			SV_ERROR("Could not find device named %s from lineno %d\n", dev, driver->lineno);
+		}
+		display_once = true;
+
+		return -1;
+	}
+
+	handle_lightcap(so, &le);
+	return 0;
+}
+
+static int parse_and_run_lightcode(const char *line, SurvivePlaybackData *driver) {
 	char lhn[10];
 	char axn[10];
 	char dev[10];
@@ -206,8 +249,7 @@ static int parse_and_run_lightcode(const char *line,
 					&length, &lh);
 
 	if (rr != 9) {
-		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
-				driver->lineno, rr, line);
+		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n", driver->lineno, rr, line);
 		return -1;
 	}
 
@@ -232,12 +274,10 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
 	FILE *f = driver->playback_file;
 
 	if (f && !feof(f) && !ferror(f)) {
-		int i;
 		driver->lineno++;
-		char *line;
+		char *line = 0;
 
 		if (driver->next_time_us == 0) {
-			char *buffer;
 			size_t n = 0;
 			ssize_t r = getdelim(&line, &n, ' ', f);
 			if (r <= 0)
@@ -255,7 +295,6 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
 			return 0;
 		driver->next_time_us = 0;
 
-		char *buffer;
 		size_t n = 0;
 		ssize_t r = getline(&line, &n, f);
 		if (r <= 0)
@@ -263,16 +302,23 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
 
 		char dev[10];
 		char op[10];
-		if (sscanf(line, "%8s %8s", dev, op) < 2)
+		if (sscanf(line, "%8s %8s", dev, op) < 2) {
+			free(line);
 			return 0;
+		}
 
-		if ((op[0] != 'R' && op[0] != 'L' && op[0] != 'I') || op[1] != 0)
+		if (op[1] != 0) {
 			return 0;
+		}
 
 		switch (op[0]) {
+		case 'C':
+			parse_and_run_rawlight(line, driver);
+			break;
 		case 'L':
 		case 'R':
-			parse_and_run_lightcode(line, driver);
+			if (driver->hasRawLight == false)
+				parse_and_run_lightcode(line, driver);
 			break;
 		case 'I':
 			parse_and_run_imu(line, driver);
@@ -301,15 +347,15 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) {
 }
 
 void survive_install_recording(SurviveContext *ctx) {
-	const char *dataout_file = survive_configs(ctx, "record", SC_SETCONFIG, "");
-	int record_to_stdout = survive_configi(ctx, "record-stdout", SC_SETCONFIG, 0);
+	const char *dataout_file = survive_configs(ctx, "record", SC_GET, "");
+	int record_to_stdout = survive_configi(ctx, "record-stdout", SC_GET, 0);
 
 	if (strlen(dataout_file) > 0 || record_to_stdout) {
 		ctx->recptr = calloc(1, sizeof(struct SurviveRecordingData));
 
 		ctx->recptr->output_file = fopen(dataout_file, "w");
 		if (ctx->recptr->output_file == 0 && !record_to_stdout) {
-			SV_INFO("Could not open %s for writing\n", dataout_file);
+			SV_INFO("Could not open %s for writing", dataout_file);
 			free(ctx->recptr);
 			ctx->recptr = 0;
 			return;
@@ -319,11 +365,13 @@ void survive_install_recording(SurviveContext *ctx) {
 		if (record_to_stdout) {
 			SV_INFO("Recording to stdout");
 		}
+
+		ctx->recptr->writeRawLight = survive_configi(ctx, "record-rawlight", SC_GET, 1);
 	}
 }
 
 int DriverRegPlayback(SurviveContext *ctx) {
-	const char *playback_file = survive_configs(ctx, "playback", SC_SETCONFIG, "");
+	const char *playback_file = survive_configs(ctx, "playback", SC_GET, "");
 
 	if (strlen(playback_file) == 0) {
 		return 0;
@@ -332,9 +380,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
 	sp->ctx = ctx;
 	sp->playback_dir = playback_file;
-	sp->time_factor = survive_configf(ctx, "playback-factor", SC_SETCONFIG, 1.f);
-
-	printf("%s\n", playback_file);
+	sp->time_factor = survive_configf(ctx, "playback-factor", SC_GET, 1.f);
 
 	sp->playback_file = fopen(playback_file, "r");
 	if (sp->playback_file == 0) {
@@ -356,7 +402,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	while (!feof(sp->playback_file) && !ferror(sp->playback_file)) {
 		char *line = 0;
 		size_t n;
-		ssize_t r = getline(&line, &n, sp->playback_file);
+		int r = getline(&line, &n, sp->playback_file);
 
 		if (r <= 0)
 			continue;
diff --git a/src/survive_playback.h b/src/survive_playback.h
index 52638a9..c895120 100644
--- a/src/survive_playback.h
+++ b/src/survive_playback.h
@@ -5,7 +5,7 @@ void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len)
 
 void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose,
 										  SurvivePose *obj);
-
+void survive_recording_lightcap(SurviveObject *so, LightcapElement *le);
 void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
 void survive_recording_info_process(SurviveContext *ctx, const char *fault);
 void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length,
diff --git a/src/survive_process.c b/src/survive_process.c
index 6136148..e05e809 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -48,15 +48,22 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 	FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0);
 
 	//Need to now do angle correction.
-#if 1
-	BaseStationData * bsd = &ctx->bsd[base_station];
+	static int use_bsd_cal = -1;
+	if (use_bsd_cal == -1) {
+		use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
+		if (use_bsd_cal == 0) {
+			SV_INFO("Not using BSD calibration values");
+		}
+	}
+	if (use_bsd_cal) {
+		BaseStationData *bsd = &ctx->bsd[base_station];
 
-	//XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
-	angle += bsd->fcalphase[axis];
-//	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
-	
-	//TODO!!!
-#endif
+		// XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
+		// angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
+		//	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+
+		// TODO!!!
+	}
 
 	FLT length_sec = length / (FLT)so->timebase_hz;
 	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh);
@@ -80,7 +87,9 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 		.lh = lh,
 	};
 
-	SurviveSensorActivations_add(&so->activations, &l);
+	// Simulate the use of only one lighthouse in playback mode.
+	if (lh < ctx->activeLighthouses)
+		SurviveSensorActivations_add(&so->activations, &l);
 
 	survive_recording_angle_process(so, sensor_id, acode, timecode, length, angle, lh);
 
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index eabdb07..d6ba70b 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -1,80 +1,11 @@
 #include "survive_reproject.h"
 #include <../redist/linmath.h>
 #include <math.h>
+#include <stdio.h>
 #include <string.h>
 
-static void survive_calibration_options_config_normalize(
-	survive_calibration_options_config *option) {
-	if (!option->enable[0])
-		option->invert[0] = false;
-	if (!option->enable[1])
-		option->invert[1] = false;
-	if (!option->enable[0] && !option->enable[1])
-		option->swap = false;
-}
-
-void survive_calibration_options_config_apply(
-	const survive_calibration_options_config *option, const FLT *input,
-	FLT *output) {
-	FLT tmp[2]; // In case they try to do in place
-	for (int i = 0; i < 2; i++) {
-		tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) *
-				 input[i ^ option->swap];
-	}
-	for (int i = 0; i < 2; i++) {
-		output[i] = tmp[i];
-	}
-}
-
-survive_calibration_config
-survive_calibration_config_create_from_idx(size_t v) {
-	survive_calibration_config config;
-	memset(&config, 0, sizeof(config));
-
-	bool *_this = (bool *)&config;
-
-	for (size_t i = 0; i < sizeof(config); i++) {
-		_this[i] = (bool)(v & 1);
-		v = v >> 1;
-	}
-
-	survive_calibration_options_config_normalize(&config.phase);
-	survive_calibration_options_config_normalize(&config.tilt);
-	survive_calibration_options_config_normalize(&config.curve);
-	survive_calibration_options_config_normalize(&config.gibMag);
-
-	config.gibPhase.enable[0] = config.gibMag.enable[0];
-	config.gibPhase.enable[1] = config.gibMag.enable[1];
-
-	survive_calibration_options_config_normalize(&config.gibPhase);
-
-	if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1])
-		config.gibUseSin = false;
-
-	return config;
-}
-
-size_t
-survive_calibration_config_index(const survive_calibration_config *config) {
-	bool *_this = (bool *)config;
-	size_t v = 0;
-	for (size_t i = 0; i < sizeof(*config); i++) {
-		v = (v | _this[sizeof(*config) - i - 1]);
-		v = v << 1;
-	}
-	v = v >> 1;
-	return v;
-}
-
-static FLT gibf(bool useSin, FLT v) {
-	if (useSin)
-		return sin(v);
-	return cos(v);
-}
-
-void survive_reproject_from_pose_with_config(
-	const SurviveContext *ctx, const survive_calibration_config *config,
-	int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
+void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config,
+										  const SurvivePose *pose, const FLT *pt, FLT *out) {
 	LinmathQuat invq;
 	quatgetreciprocal(invq, pose->Rot);
 
@@ -88,57 +19,85 @@ void survive_reproject_from_pose_with_config(
 
 	FLT x = -t_pt[0] / -t_pt[2];
 	FLT y = t_pt[1] / -t_pt[2];
+	double xy[] = {x, y};
+	double ang[] = {atan(x), atan(y)};
+
+	const FLT *phase = bsd->fcal.phase;
+	const FLT *curve = bsd->fcal.curve;
+	const FLT *tilt = bsd->fcal.tilt;
+	const FLT *gibPhase = bsd->fcal.gibpha;
+	const FLT *gibMag = bsd->fcal.gibmag;
+	enum SurviveCalFlag f = config->use_flag;
+
+	for (int axis = 0; axis < 2; axis++) {
+		int opp_axis = axis == 0 ? 1 : 0;
+
+		out[axis] = ang[axis];
+
+		if (f & SVCal_Phase)
+			out[axis] -= config->phase_scale * phase[axis];
+		if (f & SVCal_Tilt)
+			out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis];
+		if (f & SVCal_Curve)
+			out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis];
+		if (f & SVCal_Gib)
+			out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis];
+	}
+}
 
-	double ang_x = atan(x);
-	double ang_y = atan(y);
-
-	const BaseStationData *bsd = &ctx->bsd[lighthouse];
-	double phase[2];
-	survive_calibration_options_config_apply(&config->phase, bsd->fcalphase,
-											 phase);
-	double tilt[2];
-	survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt,
-											 tilt);
-	double curve[2];
-	survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve,
-											 curve);
-	double gibPhase[2];
-	survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha,
-											 gibPhase);
-	double gibMag[2];
-	survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag,
-											 gibMag);
+void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config,
+											 const FLT *in, FLT *out) {
+	const BaseStationCal *cal = &ctx->bsd[lh].fcal;
+	out[0] = in[0] + config->phase_scale * cal->phase[0];
+	out[1] = in[1] + config->phase_scale * cal->phase[1];
+
+	enum SurviveCalFlag f = config->use_flag;
+	FLT phase_scale = config->phase_scale;
+	FLT tilt_scale = config->tilt_scale;
+	FLT curve_scale = config->curve_scale;
+	FLT gib_scale = config->gib_scale;
+	const int iterations = 4;
+	for (int i = 0; i < iterations; i++) {
+		FLT last_out[2] = {out[0], out[1]};
+		FLT tlast_out[2] = {tan(out[0]), tan(out[1])};
+		bool last_iteration = i == iterations - 1;
+		for (int j = 0; j < 2; j++) {
+			int oj = j == 0 ? 1 : 0;
+			out[j] = in[j];
+			if (!last_iteration || (f & SVCal_Phase))
+				out[j] += phase_scale * cal->phase[j];
+			if (!last_iteration || (f & SVCal_Tilt))
+				out[j] += tan(tilt_scale * cal->tilt[j]) * tlast_out[oj];
+			if (!last_iteration || (f & SVCal_Curve))
+				out[j] += (cal->curve[j] * curve_scale) * tlast_out[oj] * tlast_out[oj];
+			if (!last_iteration || (f & SVCal_Gib))
+				out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale;
+		}
+	}
+}
 
-	out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y +
-			 gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0];
-	out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x +
-			 gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1];
+void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt,
+								 FLT *out) {
+	survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], &ctx->calibration_config, pose, pt, out);
 }
 
-void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse,
-								 const SurvivePose *pose, FLT *pt, FLT *out) {
-	survive_reproject_from_pose_with_config(
-		ctx, survive_calibration_default_config(), lighthouse, pose, pt, out);
+void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out) {
+	survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out);
 }
 
-void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
-					   FLT *out) {
-	survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose,
-								point3d, out);
+survive_calibration_config survive_calibration_config_ctor() {
+	return (survive_calibration_config){.use_flag = SVCal_All,
+										.phase_scale = 1.,
+										.tilt_scale = 1. / 10.,
+										.curve_scale = 1. / 10.,
+										.gib_scale = -1. / 10.};
 }
 
-const survive_calibration_config *survive_calibration_default_config() {
-	static survive_calibration_config *def = 0;
-	if (def == 0) {
-		def = malloc(sizeof(survive_calibration_config));
-		memset(def, 0, sizeof(survive_calibration_config));
-		*def = survive_calibration_config_create_from_idx(0);
-	}
-	return def;
+void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) {
+	survive_apply_bsd_calibration_by_config(ctx, lh, &ctx->calibration_config, in, out);
 }
 
-size_t survive_calibration_config_max_idx() {
-	survive_calibration_config cfg;
-	memset(&cfg, 0x1, sizeof(survive_calibration_config));
-	return survive_calibration_config_index(&cfg);
+void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config,
+											 int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) {
+	return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out);
 }
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 4d1801c..dc5c0d4 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -1,3 +1,4 @@
+#include <math.h>
 #include <survive.h>
 
 bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
@@ -21,9 +22,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 	int axis = (lightData->acode & 1);
 	uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
 	FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis];
+	uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis];
 
 	*angle = lightData->angle;
 	*data_timecode = lightData->timecode;
+	*length = lightData->length * 48000000;
 }
 
-uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);
\ No newline at end of file
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000;
diff --git a/src/survive_statebased_disambiguator.c b/src/survive_statebased_disambiguator.c
new file mode 100644
index 0000000..6b217ee
--- /dev/null
+++ b/src/survive_statebased_disambiguator.c
@@ -0,0 +1,488 @@
+//
+#include "survive_internal.h"
+#include <assert.h>
+#include <math.h> /* for sqrt */
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__)
+#define DEBUG_TB(...)
+/**
+ * The lighthouses go in the following order:
+ *
+ *     Ticks  State
+ *         0  ACode 0b1x0 (4) <--- B
+ *    20 000  ACode 0b0x0 (0) <--- A/c
+ *            LH A X Sweep
+ *   400 000  ACode 0b1x1 (5) <--- B
+ *   420 000  ACode 0b0x1 (1) <--- A/c
+ *            LH A Y SWEEP
+ *   800 000  ACode 0b0x0 (0) <--- B
+ *   820 000  ACode 0b1x0 (4) <--- A/c
+ *            LH B X Sweep
+ * 1 200 000  ACode 0b0x1 (1) <--- B
+ * 1 220 000  ACode 0b1x1 (5) <--- A/c
+ *            LH B Y SWEEP
+ * 1 600 000  < REPEAT >
+ *
+ * NOTE: Obviously you cut the data bit out for this
+ *
+ * This disambiguator works by finding where in that order it is, and tracking along with it.
+ * It is able to maintain this tracking for extended periods of time without further data
+ * by knowing the modulo of the start of the cycle and calculating appropriatly although this
+ * will run into issues when the timestamp rolls over or we simply drift off in accuracy.
+ *
+ * Neither case is terminal though; it will just have to find the modulo again which only takes
+ * a handful of pulses.
+ *
+ * The main advantage to this scheme is that its reasonably fast and is able to deal with being
+ * close enough to the lighthouse that the lengths are in a valid sync pulse range.
+ */
+
+// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window
+#define PULSE_WINDOW 20000
+#define CAPTURE_WINDOW 360000
+
+enum LighthouseState {
+	LS_UNKNOWN = 0,
+
+	LS_WaitLHA_ACode4 = 1,
+	LS_WaitLHA_ACode0,
+	LS_SweepAX,
+	LS_WaitLHA_ACode5,
+	LS_WaitLHA_ACode1,
+	LS_SweepAY,
+	LS_WaitLHB_ACode0,
+	LS_WaitLHB_ACode4,
+	LS_SweepBX,
+	LS_WaitLHB_ACode1,
+	LS_WaitLHB_ACode5,
+	LS_SweepBY,
+
+	LS_END
+};
+
+typedef struct {
+	int acode, lh, axis, window, offset;
+	bool is_sweep;
+} LighthouseStateParameters;
+
+// clang-format off
+const LighthouseStateParameters LS_Params[LS_END + 1] = {
+	{.acode = -1, .lh = -1, .axis = -1, .window = -1},
+
+	{.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW,   .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW},                // 0
+	{.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW,   .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW},                // 20000
+	{.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW, .is_sweep = 1}, // 40000
+
+	{.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW,   .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW},                // 400000
+	{.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW,   .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW},                // 420000
+	{.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW, .is_sweep = 1}, // 440000
+
+	{.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW,   .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW},                // 800000
+	{.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW,   .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW},                // 820000
+	{.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW, .is_sweep = 1}, // 840000
+
+	{.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW,   .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW},                // 1200000
+	{.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW,   .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW},                // 1220000
+	{.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW, .is_sweep = 1}, // 1240000
+
+	{.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW}                          // 1600000
+};
+// clang-format on
+
+enum LighthouseState LighthouseState_findByOffset(int offset) {
+	for (int i = 2; i < LS_END + 1; i++) {
+		if (LS_Params[i].offset > offset)
+			return i - 1;
+	}
+	assert(false);
+	return -1;
+}
+
+typedef struct {
+	SurviveObject *so;
+	/* We keep the last sync time per LH because lightproc expects numbers relative to it */
+	uint32_t time_of_last_sync[NUM_LIGHTHOUSES];
+
+	/* Keep running average of sync signals as they come in */
+	uint64_t last_sync_timestamp;
+	uint64_t last_sync_length;
+	int last_sync_count;
+
+	/**  This part of the structure is general use when we know our state */
+	enum LighthouseState state;
+	uint32_t mod_offset;
+	int confidence;
+
+	/** This rest of the structure is dedicated to finding a state when we are unknown */
+	int encoded_acodes;
+
+	int stabalize;
+	bool lastWasSync;
+
+	LightcapElement sweep_data[];
+} Disambiguator_data_t;
+
+static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) {
+	if (recent > prior)
+		return recent - prior;
+	return (0xFFFFFFFF - prior) + recent;
+}
+
+static int find_acode(uint32_t pulseLen) {
+	const static int offset = 50;
+	if (pulseLen < 2500 + offset)
+		return -1;
+
+	if (pulseLen < 3000 + offset)
+		return 0;
+	if (pulseLen < 3500 + offset)
+		return 1;
+	if (pulseLen < 4000 + offset)
+		return 2;
+	if (pulseLen < 4500 + offset)
+		return 3;
+	if (pulseLen < 5000 + offset)
+		return 4;
+	if (pulseLen < 5500 + offset)
+		return 5;
+	if (pulseLen < 6000 + offset)
+		return 6;
+	if (pulseLen < 6500 + offset)
+		return 7;
+
+	return -1;
+}
+
+static bool overlaps(const LightcapElement *a, const LightcapElement *b) {
+	int overlap = 0;
+	if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp)
+		overlap = a->length + a->timestamp - b->timestamp;
+	else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp)
+		overlap = b->length + b->timestamp - a->timestamp;
+
+	return overlap > a->length / 2;
+}
+
+const int SKIP_BIT = 4;
+const int DATA_BIT = 2;
+const int AXIS_BIT = 1;
+
+#define LOWER_SYNC_TIME 2250
+#define UPPER_SYNC_TIME 6750
+
+LightcapElement get_last_sync(Disambiguator_data_t *d) {
+	if (d->last_sync_count == 0) {
+		return (LightcapElement){0};
+	}
+
+	return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count,
+							 .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count,
+							 .sensor_id = -d->last_sync_count};
+}
+
+enum LightcapClassification { LCC_SWEEP, LCC_SYNC };
+static enum LightcapClassification naive_classify(Disambiguator_data_t *d, const LightcapElement *le) {
+	bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME;
+
+	if (clearlyNotSync) {
+		return LCC_SWEEP;
+	} else {
+		return LCC_SYNC;
+	}
+}
+
+#define ACODE_TIMING(acode)                                                                                            \
+	((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250)
+#define ACODE(s, d, a) ((s << 2) | (d << 1) | a)
+#define SWEEP 0xFF
+
+static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) {
+	assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data
+	SurviveContext *ctx = d->so->ctx;
+	DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset,
+			 (le->timestamp - LS_Params[state].offset));
+
+	return (le->timestamp - LS_Params[state].offset);
+}
+
+static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le,
+									 enum LighthouseState new_state);
+static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) {
+
+	// We chain together acodes / sweep indicators to form an int we can just switch on.
+	SurviveContext *ctx = d->so->ctx;
+	d->encoded_acodes &= 0xFF;
+	d->encoded_acodes = (d->encoded_acodes << 8) | newByte;
+
+	LightcapElement lastSync = get_last_sync(d);
+
+	// These combinations are checked for specificaly to allow for the case one lighthouse is either
+	//  missing or completely occluded.
+	switch (d->encoded_acodes) {
+	case (ACODE(0, 1, 0) << 8) | SWEEP:
+		d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync);
+
+		return (LS_SweepAX + 1);
+	case (ACODE(0, 1, 1) << 8) | SWEEP:
+		d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync);
+
+		return (LS_SweepAY + 1);
+	case (SWEEP << 8) | (ACODE(0, 1, 1)):
+		d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync);
+
+		return (LS_WaitLHB_ACode1 + 1);
+	case (SWEEP << 8) | (ACODE(1, 1, 0)):
+		d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync);
+
+		return (LS_WaitLHA_ACode4 + 1);
+	}
+
+	return LS_UNKNOWN;
+}
+static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) {
+	return CheckEncodedAcode(d, SWEEP);
+}
+static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) {
+	LightcapElement lastSync = get_last_sync(d);
+	int acode = find_acode(lastSync.length) > 0;
+	if (acode > 0) {
+		return CheckEncodedAcode(d, (acode | DATA_BIT));
+	} else {
+		// If we can't resolve an acode, just reset
+		d->encoded_acodes = 0;
+	}
+	return LS_UNKNOWN;
+}
+
+static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) {
+	enum LightcapClassification classification = naive_classify(d, le);
+
+	if (classification == LCC_SYNC) {
+		LightcapElement lastSync = get_last_sync(d);
+
+		// Handle the case that this is a new SYNC coming in
+		if (d->lastWasSync == false || overlaps(&lastSync, le) == false) {
+
+			if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) {
+				// Missed a sweep window; clear encoded values.
+				d->encoded_acodes = 0;
+			}
+
+			// Now that the previous two states are in, check to see if they tell us where we are
+			enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le);
+			if (new_state != LS_UNKNOWN)
+				return new_state;
+
+			// Otherwise, just reset the sync registers and do another
+			d->last_sync_timestamp = le->timestamp;
+			d->last_sync_length = le->length;
+			d->last_sync_count = 1;
+		} else {
+			d->last_sync_timestamp += le->timestamp;
+			d->last_sync_length += le->length;
+			d->last_sync_count++;
+		}
+
+		d->lastWasSync = true;
+	} else {
+		// If this is the start of a new sweep, check to see if the end of the sync solves
+		// the state
+		if (d->lastWasSync) {
+			enum LighthouseState new_state = EndSync(d, le);
+			if (new_state != LS_UNKNOWN)
+				return new_state;
+		}
+		d->lastWasSync = false;
+	}
+
+	return LS_UNKNOWN;
+}
+
+static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le,
+									 enum LighthouseState new_state) {
+
+	SurviveContext *ctx = d->so->ctx;
+	if (new_state >= LS_END)
+		new_state = 1;
+
+	d->encoded_acodes = 0;
+	d->state = new_state;
+
+	d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0;
+	memset(d->sweep_data, 0, sizeof(LightcapElement) * d->so->sensor_ct);
+
+	return new_state;
+}
+
+static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le);
+static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) {
+	// Just ignore small signals; this has a measurable impact on signal quality
+	if (le->length < 100)
+		return;
+
+	// We know what state we are in, so we verify that state as opposed to
+	// trying to suss out the acode.
+
+	// Calculate what it would be with and without data
+	uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length);
+	uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length);
+
+	// Take the least of the two erors
+	uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0;
+
+	// Errors do happen; either reflections or some other noise. Our scheme here is to
+	// keep a tally of hits and misses, and if we ever go into the negatives reset
+	// the state machine to find the state again.
+	if (error > 1250) {
+		// Penalize semi-harshly -- if it's ever off track it will take this many syncs
+		// to reset
+		const int penalty = 3;
+		if (d->confidence < penalty) {
+			SurviveContext *ctx = d->so->ctx;
+			SetState(d, le, LS_UNKNOWN);
+			SV_INFO("WARNING: Disambiguator got lost; refinding state for %s", d->so->codename);
+		}
+		d->confidence -= penalty;
+		return;
+	}
+
+	if (d->confidence < 100)
+		d->confidence++;
+
+	// If its a real timestep, integrate it here and we can take the average later
+	d->last_sync_timestamp += le->timestamp;
+	d->last_sync_length += le->length;
+	d->last_sync_count++;
+}
+
+static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) {
+	SurviveContext *ctx = d->so->ctx;
+
+	// Leaving a sync ...
+	if (LS_Params[d->state].is_sweep == 0) {
+		if (d->last_sync_count > 0) {
+			// Use the average of the captured pulse to adjust where we are modulo against.
+			// This lets us handle drift in any of the timing chararacteristics
+			LightcapElement lastSync = get_last_sync(d);
+			d->mod_offset = SolveForMod_Offset(d, d->state, &lastSync);
+
+			// Figure out if it looks more like it has data or doesn't. We need this for OOX
+			int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT);
+			int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode);
+			bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length);
+			int acode = LS_Params[d->state].acode;
+			if (hasData) {
+				acode |= DATA_BIT;
+			}
+			ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length,
+						   LS_Params[d->state].lh);
+
+			// Store last sync time for sweep calculations
+			d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp;
+		}
+	} else {
+		// Leaving a sweep ...
+		size_t avg_length = 0;
+		size_t cnt = 0;
+
+		for (int i = 0; i < d->so->sensor_ct; i++) {
+			LightcapElement le = d->sweep_data[i];
+			// Only care if we actually have data AND we have a time of last sync. We won't have the latter
+			// if we synced with the LH at cetain times.
+			if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) {
+				avg_length += le.length;
+				cnt++;
+			}
+		}
+		if (cnt > 0) {
+			double var = 1.5;
+			size_t minl = (1 / var) * (avg_length + cnt / 2) / cnt;
+			size_t maxl = var * (avg_length + cnt / 2) / cnt;
+
+			for (int i = 0; i < d->so->sensor_ct; i++) {
+				LightcapElement le = d->sweep_data[i];
+				// Only care if we actually have data AND we have a time of last sync. We won't have the latter
+				// if we synced with the LH at certain times.
+				if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0 && le.length >= minl &&
+					le.length <= maxl) {
+					int32_t offset_from =
+						timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]);
+
+					// Send the lightburst out.
+					if (offset_from > 0)
+						d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length,
+											  LS_Params[d->state].lh);
+				}
+			}
+		}
+	}
+	SetState(d, le, new_state);
+}
+
+static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) {
+	int le_offset = le->timestamp > d->mod_offset
+						? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset
+						: (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset;
+
+	/** Find where this new element fits into our state machine. This can skip states if its been a while since
+	 * its been able to process, or if a LH is missing. */
+	enum LighthouseState new_state = LighthouseState_findByOffset(le_offset);
+
+	if (d->state != new_state) {
+		// This processes the change -- think setting buffers, and sending OOTX / lightproc calls
+		ProcessStateChange(d, le, new_state);
+	}
+
+	const LighthouseStateParameters *param = &LS_Params[d->state];
+	if (param->is_sweep == 0) {
+		RunACodeCapture(param->acode, d, le);
+	} else if (le->length > d->sweep_data[le->sensor_id].length &&
+			   le->length < 7000 /*anything above 10k seems to be bullshit?*/) {
+		// Note we only select the highest length one per sweep. Also, we bundle everything up and send it later all at
+		// once.
+		// so that we can do this filtering. Might not be necessary?
+		d->sweep_data[le->sensor_id] = *le;
+	}
+}
+
+void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) {
+	SurviveContext *ctx = so->ctx;
+
+	// Note, this happens if we don't have config yet -- just bail
+	if (so->sensor_ct == 0) {
+		return;
+	}
+
+	if (so->disambiguator_data == NULL) {
+		DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct);
+		Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct);
+		d->so = so;
+		so->disambiguator_data = d;
+	}
+
+	Disambiguator_data_t *d = so->disambiguator_data;
+	// It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize.
+	if (d->stabalize < 200) {
+		d->stabalize++;
+		return;
+	}
+
+	if (d->state == LS_UNKNOWN) {
+		enum LighthouseState new_state = AttemptFindState(d, le);
+		if (new_state != LS_UNKNOWN) {
+			d->confidence = 0;
+
+			int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset;
+			enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset);
+			SetState(d, le, new_state1);
+			DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset);
+		}
+	} else {
+		PropagateState(d, le);
+	}
+}
+
+REGISTER_LINKTIME(DisambiguatorStateBased);
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 91f4b1f..53f6d42 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1715,19 +1715,9 @@ int survive_vive_close( SurviveContext * ctx, void * driver )
 	return 0;
 }
 
-void init_SurviveObject(SurviveObject* so) {
-	so->acc_scale = NULL;
-	so->acc_bias = NULL;
-	so->gyro_scale = NULL;
-	so->gyro_bias = NULL;
-	so->haptic = NULL;
-	so->PoserData = NULL;
-	so->disambiguator_data = NULL;
-}
-
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	const char *playback_dir = survive_configs(ctx, "playback", SC_SETCONFIG, "");
+	const char *playback_dir = survive_configs(ctx, "playback", SC_GET, "");
 	if(strlen(playback_dir) != 0) {
 	  SV_INFO("Playback is active; disabling USB driver");
 	  return 0;