text: #include "ootx_decoder.h"
  #include "survive.h"
  #include "survive_config.h"
  #include "survive_internal.h"
  #include "survive_kalman_tracker.h"
  #include "survive_recording.h"
  #include <assert.h>
  #include <math.h>
  #include <survive.h>
  
  static FLT freq_per_channel[NUM_GEN2_LIGHTHOUSES] = {
  	50.0521, 50.1567, 50.3673, 50.5796, 50.6864, 50.9014, 51.0096, 51.1182,
  
  	51.2273, 51.6685, 52.2307, 52.6894, 52.9217, 53.2741, 53.7514, 54.1150,
  };
  
  static void ootx_bad_crc_clbk(ootx_decoder_context *ct, ootx_packet *pkt, uint32_t checksum) {
  	SurviveContext *ctx = ((SurviveObject *)(ct->user))->ctx;
  	int id = ct->user1;
  
  	if (!ctx->bsd[id].OOTXSet)
  		SV_VERBOSE(200, "(%d) Failed CRC", ctx->bsd[id].mode != 255 ? ctx->bsd[id].mode : id);
  }
  static void ootx_error_clbk_d(ootx_decoder_context *ct, const char *msg) {
  	SurviveContext *ctx = ((SurviveObject *)(ct->user))->ctx;
  	int id = ct->user1;
  
  	if (!ctx->bsd[id].OOTXSet)
  		SV_INFO("(%d) %s", ctx->bsd[id].mode != 255 ? ctx->bsd[id].mode : id, msg);
  }
  
  STATIC_CONFIG_ITEM(SERIALIZE_OOTX, "serialize-ootx", 'i', "Serialize out ootx", 0)
  static void ootx_packet_clbk_d_gen2(ootx_decoder_context *ct, ootx_packet *packet) {
  	SurviveContext *ctx = ((SurviveObject *)(ct->user))->ctx;
  	int id = ct->user1;
  
  	lighthouse_info_v15 v15;
  	init_lighthouse_info_v15(&v15, packet->data);
  
  	if (survive_configi(ctx, SERIALIZE_OOTX_TAG, SC_GET, 0) == 1) {
  		char filename[128];
  		snprintf(filename, 128, "LH%02d_%08x.ootx", v15.mode_current & 0x7F, (unsigned)v15.id);
  		FILE *f = fopen(filename, "w");
  		fwrite(packet->data, packet->length, 1, f);
  		fclose(f);
  	}
  
  	BaseStationData *b = &ctx->bsd[id];
  
  	bool doSave = b->BaseStationID != v15.id || b->OOTXSet == false;
  
  	if (doSave) {
  	  SV_INFO("Got OOTX packet %d %08x", ctx->bsd[id].mode, (unsigned)v15.id);
  
  		b->BaseStationID = v15.id;
  		for (int i = 0; i < 2; i++) {
  			b->fcal[i].phase = v15.fcal_phase[i];
  			b->fcal[i].tilt = v15.fcal_tilt[i];
  			b->fcal[i].curve = v15.fcal_curve[i];
  			b->fcal[i].gibpha = v15.fcal_gibphase[i];
  			b->fcal[i].gibmag = v15.fcal_gibmag[i];
  			b->fcal[i].ogeephase = v15.fcal_ogeephase[i];
  			b->fcal[i].ogeemag = v15.fcal_ogeemag[i];
  		}
  
  		for (int i = 0; i < 3; i++) {
  			b->accel[i] = v15.accel_dir[i];
  		}
  
  		// Although we know this already....
  		b->mode = v15.mode_current & 0x7F;
  		b->OOTXSet = 1;
  
  		ctx->ootx_receivedproc(ctx, id);
  	}
  }
  
  static void ootx_packet_cblk_d_gen1(ootx_decoder_context *ct, ootx_packet *packet) {
  
  	SurviveContext *ctx = ((SurviveObject *)(ct->user))->ctx;
  	int id = ct->user1;
  
  	SV_INFO("Got OOTX packet %d", id);
  
  	lighthouse_info_v6 v6;
  	init_lighthouse_info_v6(&v6, packet->data);
  
  	BaseStationData *b = &ctx->bsd[id];
  
  	b->BaseStationID = v6.id;
  	b->fcal[0].phase = v6.fcal_0_phase;
  	b->fcal[1].phase = v6.fcal_1_phase;
  	b->fcal[0].tilt = tan(v6.fcal_0_tilt);
  	b->fcal[1].tilt = tan(v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
  	b->fcal[0].curve = v6.fcal_0_curve;
  	b->fcal[1].curve = v6.fcal_1_curve;
  	b->fcal[0].gibpha = v6.fcal_0_gibphase;
  	b->fcal[1].gibpha = v6.fcal_1_gibphase;
  	b->fcal[0].gibmag = v6.fcal_0_gibmag;
  	b->fcal[1].gibmag = 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;
  
  	ctx->ootx_receivedproc(ctx, id);
  }
  void survive_ootx_free_decoder_context(struct SurviveContext *ctx, int bsd_idx) {
  	ootx_decoder_context *decoderContext = ctx->bsd[bsd_idx].ootx_data;
  	ctx->bsd[bsd_idx].ootx_data = 0;
  	if (decoderContext == 0)
  		return;
  
  	SV_VERBOSE(5, "OOTX stats for LH%d", bsd_idx);
  	SV_VERBOSE(5, "\tBits seen:         %u (%d bytes)", decoderContext->stats.bits_seen,
  			   decoderContext->stats.bits_seen / 8);
  	SV_VERBOSE(5, "\tBad CRCs:          %u", decoderContext->stats.bad_crcs);
  	SV_VERBOSE(5, "\tBad sync bits:     %u", decoderContext->stats.bad_sync_bits);
  	SV_VERBOSE(5, "\tPackets found:     %u", decoderContext->stats.packets_found);
  	SV_VERBOSE(5, "\tPayload size:      %u", decoderContext->stats.used_bytes);
  	SV_VERBOSE(5, "\tPackage bits:      %u", decoderContext->stats.package_bits);
  	SV_VERBOSE(5, "\tGuessed bits:      %u (%5.2f%%)", decoderContext->stats.guess_bits,
  			   decoderContext->stats.guess_bits / (FLT)decoderContext->stats.package_bits * 100.);
  	FLT d = survive_run_time(ctx) - decoderContext->stats.started_s;
  	SV_VERBOSE(5, "\tTime:              %2.2f (%2.2fb/s, %2.2fb/s)", d, decoderContext->stats.bits_seen / d,
  			   decoderContext->stats.used_bytes * 8 / d);
  
  	ootx_free_decoder_context(decoderContext);
  	free(decoderContext);
  }
  void survive_ootx_behavior(SurviveObject *so, int8_t bsd_idx, int8_t lh_version, int ootx) {
  	struct SurviveContext *ctx = so->ctx;
  	if (ctx->bsd[bsd_idx].OOTXSet == false) {
  		ootx_decoder_context *decoderContext = ctx->bsd[bsd_idx].ootx_data;
  
  		if (decoderContext == 0) {
  			if (lh_version == 1) {
  				SV_INFO("OOTX not set for LH in channel %d; attaching ootx decoder using device %s",
  						ctx->bsd[bsd_idx].mode, so->codename);
  			} else {
  				SV_INFO("OOTX not set for LH %d; attaching ootx decoder using device %s", bsd_idx, so->codename);
  			}
  			decoderContext = ctx->bsd[bsd_idx].ootx_data = SV_CALLOC(1, sizeof(ootx_decoder_context));
  			ootx_init_decoder_context(decoderContext, survive_run_time(ctx));
  			decoderContext->user1 = bsd_idx;
  			decoderContext->user = so;
  			decoderContext->ignore_sync_bit_error = survive_configi(ctx, "ootx-ignore-sync-error", SC_SETCONFIG, 0);
  			decoderContext->ootx_packet_clbk = lh_version ? ootx_packet_clbk_d_gen2 : ootx_packet_cblk_d_gen1;
  			decoderContext->ootx_error_clbk = ootx_error_clbk_d;
  			decoderContext->ootx_bad_crc_clbk = ootx_bad_crc_clbk;
  		}
  		if (decoderContext->user == so) {
  			ootx_pump_bit(decoderContext, ootx);
  
  			if (ctx->bsd[bsd_idx].OOTXSet) {
  				survive_ootx_free_decoder_context(ctx, bsd_idx);
  			}
  		}
  	}
  }
  
  SURVIVE_EXPORT void survive_default_sync_process(SurviveObject *so, survive_channel channel, survive_timecode timecode,
  												 bool ootx, bool gen) {
  	struct SurviveContext *ctx = so->ctx;
  	int8_t bsd_idx = survive_get_bsd_idx(ctx, channel);
  	if (bsd_idx == -1) {
  		SV_WARN("Invalid channel requested(%d) for %s", channel, so->codename)
  		return;
  	}
  
  	assert(channel <= NUM_GEN2_LIGHTHOUSES);
  
  	survive_recording_sync_process(so, channel, timecode, ootx, gen);
  
  	bool isIniting = so->last_sync_time[bsd_idx] == 0;
  
  	if (!isIniting) {
  		survive_timecode time_delta = survive_timecode_difference(timecode, so->last_sync_time[bsd_idx]);
  		survive_timecode predicted_time = 48000000. / freq_per_channel[channel];
  		int rotations_since = (time_delta + predicted_time / 2) / predicted_time;
  		int skipped_syncs = rotations_since - 1;
  		time_delta -= predicted_time * skipped_syncs;
  
  		FLT hz = 48000000. / time_delta;
  		FLT err = fabs(hz - freq_per_channel[channel]);
  
  		SV_DATA_LOG("lh_freq_err[%d]", &err, 1, channel);
  
  		if (err > 1) {
  			if (skipped_syncs > 10) {
  				so->last_sync_time[bsd_idx] = 0;
  				so->stats.sync_resets[bsd_idx]++;
  			}
  			SV_VERBOSE(100, "Sync hz %2d: %8.6fhz (err: %0.6fhz) ootx: %d gen: %d time: %u count: %u", channel, hz, err,
  					   ootx, gen, timecode, so->stats.syncs[bsd_idx]);
  			so->stats.bad_syncs[bsd_idx] += skipped_syncs;
  			so->sync_count[bsd_idx] += skipped_syncs;
  			return;
  		} else {
  			SV_VERBOSE(400, "Sync   ch%2d       %6.3fhz (err: %0.6fhz) ootx: %d gen: %d time: %u count: %u", channel,
  					   hz, err, ootx, gen, timecode, so->stats.syncs[bsd_idx]);
  		}
  
  		so->stats.skipped_syncs[bsd_idx] += skipped_syncs;
  		// Every skipped sync halves our ootx success rate; but if we don't send anything in it will be wrong 100% of
  		// the time
  		for (int i = 0; i < skipped_syncs; i++) {
  			survive_ootx_behavior(so, bsd_idx, ctx->lh_version, -1);
  		}
  
  		so->last_time_between_sync[bsd_idx] = time_delta;
  	}
  
  	so->stats.syncs[bsd_idx]++;
  	so->sync_count[bsd_idx]++;
  	so->last_sync_time[bsd_idx] = timecode;
  
  	survive_ootx_behavior(so, bsd_idx, ctx->lh_version, ootx);
  
  	PoserDataLightGen2 l = {.common = {
  								.hdr =
  									{
  										.pt = POSERDATA_SYNC_GEN2,
  										.timecode = SurviveSensorActivations_long_timecode_light(&so->activations, timecode),
  									},
  								.lh = bsd_idx,
  							}};
  
  	if (bsd_idx < ctx->activeLighthouses)
  		if (SurviveSensorActivations_add_gen2(&so->activations, &l) == false) {
  			so->stats.rejected_data[bsd_idx]++;
  		}
  
  	so->stats.hit_from_lhs[bsd_idx]++;
  
  	if (ctx->lh_version != -1) {
  		SURVIVE_POSER_INVOKE(so, &l);
  	}
  }
  
  static inline int8_t determine_plane(SurviveObject *so, int8_t bsd_idx, FLT angle) {
  	static int naive_plane_only = -1;
  	if (naive_plane_only == -1)
  		naive_plane_only = survive_configi(so->ctx, "naive-plane-only", SC_GET, 0);
  	int8_t naive_plane = angle > LINMATHPI;
  	if (naive_plane_only != 0)
  		return naive_plane;
  
  	int8_t plane = naive_plane;
  	FLT m = .05;
  	bool borderLine = angle > LINMATHPI * (1 - 2 * m) && angle < LINMATHPI * (1 + 2 * m);
  	bool veryBorderLine = angle > LINMATHPI * (1 - m) && angle < LINMATHPI * (1 + m);
  	FLT angle_for_axis[2] = {angle - 2. / 3. * LINMATHPI, angle - 4. / 3. * LINMATHPI};
  
  	if (borderLine) {
  		if (veryBorderLine)
  			plane = -1;
  
  		FLT err[2] = {0};
  		for (int i = 0; i < 2; i++) {
  			err[i] = fabsf(angle_for_axis[i] - so->activations.angles_center[bsd_idx][i]);
  			if (isnan(err[i]))
  				err[i] = 1.;
  		}
  
  		if (err[0] <= .1 || err[1] <= .1) {
  			plane = err[1] < err[0];
  		}
  
  		if (plane != -1 && naive_plane != plane) {
  			so->stats.extent_hits++;
  			so->stats.min_extent = linmath_min(angle - LINMATHPI, so->stats.min_extent);
  			so->stats.max_extent = linmath_max(angle - LINMATHPI, so->stats.max_extent);
  		} else if (plane == -1) {
  			so->stats.extent_misses++;
  		} else if (plane == naive_plane) {
  			so->stats.naive_hits++;
  		}
  	}
  
  	return plane;
  }
  SURVIVE_EXPORT void survive_default_sweep_process(SurviveObject *so, survive_channel channel, int sensor_id,
  												  survive_timecode timecode, bool half_clock_flag) {
  	struct SurviveContext *ctx = so->ctx;
  
  	int8_t bsd_idx = survive_get_bsd_idx(ctx, channel);
  	if (bsd_idx == -1) {
  		SV_WARN("Invalid channel requested(%d) for %s", channel, so->codename)
  		return;
  	}
  
  	survive_notify_gen2(so, "sweep called");
  
  	survive_recording_sweep_process(so, channel, sensor_id, timecode, half_clock_flag);
  
  	survive_timecode last_sweep = so->last_sync_time[bsd_idx];
  	if (last_sweep == 0) {
  		return;
  	}
  	assert(channel <= NUM_GEN2_LIGHTHOUSES);
  
  	// SV_INFO("Sensor ch%2d %2d %d %12x %6d", channel, sensor_id, flag, timecode, timecode
  	// so->last_sync_time[bsd_idx]);
  
  	survive_timecode time_delta = survive_timecode_difference(timecode, last_sweep);
  	FLT time_since_sync = (time_delta / 48000000.);
  	if (half_clock_flag)
  		time_since_sync += 0.5 / 48000000.;
  
  	FLT hz = 48000000. / so->last_time_between_sync[bsd_idx];
  	if (fabs(hz - freq_per_channel[channel]) > 1.0) {
  		hz = freq_per_channel[channel];
  	}
  
  	survive_timecode predicted_time = 48000000. / freq_per_channel[channel];
  	int rotations_since = time_delta / predicted_time;
  
  	FLT time_per_rot = 1. / hz;
  	time_since_sync -= time_per_rot * rotations_since;
  
  	if (rotations_since > 5) {
  		SV_VERBOSE(100, "Dropping light data %d %f %f %u", channel, time_since_sync * 1000., time_per_rot * 1000.,
  				   timecode);
  		so->stats.dropped_light[bsd_idx]++;
  		return;
  	}
  
  	FLT angle = time_since_sync / time_per_rot * 2. * LINMATHPI;
  	FLT angle2 = (time_since_sync + .5 / 48000000.) / time_per_rot * 2. * LINMATHPI;
  
  	SV_VERBOSE(500, "%7.3f Sensor ch%2d.%02d   %+8.3fdeg %12f %d %.16f %u %u", survive_run_time(ctx), channel,
  			   sensor_id, angle / LINMATHPI * 180., angle2 / LINMATHPI * 180., half_clock_flag, time_since_sync,
  			   rotations_since + so->sync_count[bsd_idx], timecode);
  
  	FLT angle_for_axis[2] = {angle - 2. / 3. * LINMATHPI, angle - 4. / 3. * LINMATHPI};
  	int8_t plane = determine_plane(so, bsd_idx, angle);
  	SV_DATA_LOG("time_since_sync[%d,%d,%d]", &time_since_sync, 1, channel, sensor_id, plane);
  
  	so->stats.hit_from_lhs[bsd_idx]++;
  
  	if (plane >= 0)
  		so->ctx->sweep_angleproc(so, channel, sensor_id, timecode, plane, angle_for_axis[plane]);
  }
  
  SURVIVE_EXPORT void survive_default_sweep_angle_process(SurviveObject *so, survive_channel channel, int sensor_id,
  														survive_timecode timecode, int8_t plane, FLT angle) {
  	struct SurviveContext *ctx = so->ctx;
  	int8_t bsd_idx = survive_get_bsd_idx(ctx, channel);
  	if (bsd_idx == -1) {
  		SV_WARN("Invalid channel requested(%d) for %s", channel, so->codename)
  		return;
  	}
  
  	PoserDataLightGen2 l = {
  		.common =
  			{
  				.hdr =
  					{
  						.pt = POSERDATA_LIGHT_GEN2,
  						.timecode = SurviveSensorActivations_long_timecode_light(&so->activations, timecode),
  					},
  				.sensor_id = sensor_id,
  				.angle = angle,
  				.lh = bsd_idx,
  			},
  		.plane = plane,
  		.sync = so->sync_count[bsd_idx]};
  
  	SV_VERBOSE(500, "%7.3f Sensor ch%2d.%02d.%d %+8.3fdeg", survive_run_time(ctx), channel, sensor_id, plane,
  			   angle / LINMATHPI * 180.);
  
  	// Simulate the use of only one lighthouse in playback mode.
  	if (bsd_idx < ctx->activeLighthouses) {
  		if (SurviveSensorActivations_add_gen2(&so->activations, &l) == false) {
  			so->stats.rejected_data[bsd_idx]++;
  		} else {
  			survive_kalman_tracker_integrate_light(so->tracker, &l.common);
  		}
  	}
  
  	survive_recording_sweep_angle_process(so, channel, sensor_id, timecode, plane, angle);
  	SURVIVE_POSER_INVOKE(so, &l);
  }
  
  SURVIVE_EXPORT void survive_default_gen_detected_process(SurviveObject *so, int lh_version) {
  	SurviveContext *ctx = so->ctx;
  
  	if (ctx->lh_version != -1) {
  		static bool seenWarning = false;
  		if (seenWarning == false) {
  			SV_WARN("Detected both LH gen1 and LH gen2 systems. Mixed mode is only supported for experimentation.");
  			seenWarning = true;
  		}
  
  		ctx->lh_version = 3;
  		return;
  	}
  	assert(ctx->lh_version == -1);
  
  	SV_INFO("Detected LH gen %d system.", lh_version + 1);
  	if (lh_version != ctx->lh_version_configed && ctx->lh_version_configed != -1) {
  		SV_WARN("Configuration was valid for gen %d; resetting BSD positions and OOTX", ctx->lh_version_configed + 1);
  		for (int i = 0; i < NUM_GEN2_LIGHTHOUSES; i++) {
  			ctx->bsd[i].PositionSet = ctx->bsd[i].OOTXSet = 0;
  			ctx->bsd[i].mode = -1;
  		}
  	}
  
  	for (int i = 0; i < ctx->objs_ct; i++) {
  		// When the USB devices transition; they reset their clock or something -- so clear out old data.
  		SurviveSensorActivations_reset(&ctx->objs[i]->activations);
  	}
  
  	ctx->lh_version = lh_version;
  	survive_configi(ctx, "configed-lighthouse-gen", SC_OVERRIDE | SC_SETCONFIG, lh_version + 1);
  	config_save(ctx);
  }
  