#include "gps/gps_tracking_measurement.h"

#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>

#define TEST_SATELLITES 6u

static bool close_to(double actual, double expected, double tolerance)
{
	return fabs(actual - expected) <= tolerance;
}

static uint64_t cn0_wbp_for_target(
	int32_t sum_i,
	int32_t sum_q,
	unsigned int sample_count,
	double cn0_dbhz)
{
	double cn0_linear = pow(10.0, cn0_dbhz / 10.0);
	double scaled = cn0_linear / 1000.0;
	double ratio =
		(scaled * (double)sample_count + 1.0) / (scaled + 1.0);
	double narrowband =
		(double)sum_i * (double)sum_i +
		(double)sum_q * (double)sum_q;
	return (uint64_t)llround(narrowband / ratio);
}

static void test_cn0_and_mailbox_decode(void)
{
	gps_trck_t registers;
	memset(&registers, 0, sizeof(registers));

	const int32_t sum_i = 20000;
	const int32_t sum_q = 1000;
	const uint64_t wbp =
		cn0_wbp_for_target(sum_i, sum_q, 20u, 45.0);
	const int32_t carrier_q7 =
		(int32_t)llround((GPS_TRCK_IF_CENTER_HZ + 2500.0) *
		                GPS_TRCK_CARRIER_Q7_SCALE);

	registers.PRN_NUMBER = 7u;
	registers.MEASUREMENT_META =
		(GPS_TRCK_MEAS_META_VERSION <<
		 GPS_TRCK_MEAS_META_VERSION_SHIFT) |
		(20u << GPS_TRCK_MEAS_META_COUNT_SHIFT) |
		(1u << GPS_TRCK_MEAS_META_VALID_POS) |
		(1u << GPS_TRCK_MEAS_META_NAV_BIT_POS) |
		(1u << GPS_TRCK_MEAS_META_LOCK_POS);
	registers.MEASUREMENT_SEQUENCE = 0x1234u;
	registers.MEASUREMENT_CARRIER_Q7 = carrier_q7;
	registers.MEASUREMENT_ABS_SAMPLE = 0x89abcdefu;
	registers.MEASUREMENT_REM_CODE_PHASE = 555u;
	registers.MEASUREMENT_SUM_I = sum_i;
	registers.MEASUREMENT_SUM_Q = sum_q;
	registers.MEASUREMENT_WBP_LO = (uint32_t)wbp;
	registers.MEASUREMENT_WBP_HI = (uint32_t)(wbp >> 32);

	gps_tracking_measurement_t measurement;
	assert(gps_tracking_measurement_read(&registers, &measurement));
	assert(measurement.valid);
	assert(measurement.locked);
	assert(measurement.nav_bit);
	assert(measurement.prn == 7u);
	assert(measurement.sequence == 0x1234u);
	assert(measurement.absolute_sample == 0x89abcdefu);
	assert(measurement.remaining_code_phase == 555u);
	assert(measurement.cn0_sample_count == 20u);
	assert(measurement.cn0_wbp == wbp);
	assert(close_to(measurement.cn0_dbhz, 45.0, 0.01));
	assert(close_to(measurement.doppler_hz, 2500.0, 1.0 / 128.0));

	const double expected_range_rate =
		-2500.0 * ((double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ);
	assert(close_to(
		measurement.pseudorange_rate_mps,
		expected_range_rate,
		1.0e-9));

	registers.MEASUREMENT_META &=
		~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
	assert(!gps_tracking_measurement_read(&registers, &measurement));
	assert(!measurement.valid);
}

static void test_velocity_wls(void)
{
	const double receiver_position[3] = {6378137.0, 0.0, 0.0};
	const double directions[TEST_SATELLITES][3] = {
		{ 1.0,  0.0,  0.0},
		{ 0.0,  1.0,  0.0},
		{ 0.0,  0.0,  1.0},
		{-0.5773502691896258,  0.5773502691896258,  0.5773502691896258},
		{ 0.7071067811865475, -0.7071067811865475,  0.0},
		{ 0.4082482904638631,  0.4082482904638631, -0.8164965809277261}
	};
	const double satellite_velocities[TEST_SATELLITES][3] = {
		{  0.0,  2700.0,  100.0},
		{-900.0,     0.0, 2500.0},
		{1200.0, -2100.0,    0.0},
		{1800.0,   400.0, -800.0},
		{-500.0,  1900.0,  600.0},
		{ 700.0, -1200.0, 2200.0}
	};
	const double satellite_clock_drift[TEST_SATELLITES] = {
		1.0e-11, -2.0e-11, 0.5e-11, 1.5e-11, -0.7e-11, 2.2e-11
	};
	const double expected_velocity[3] = {12.0, -4.0, 1.5};
	const double expected_clock_drift_mps = 18.0;

	double satellite_positions[TEST_SATELLITES][3];
	gps_tracking_measurement_t measurements[TEST_SATELLITES];
	memset(measurements, 0, sizeof(measurements));
	for(unsigned int satellite = 0u;
	    satellite < TEST_SATELLITES;
	    satellite++)
	{
		double los_satellite_velocity = 0.0;
		double los_receiver_velocity = 0.0;
		for(unsigned int component = 0u; component < 3u; component++)
		{
			satellite_positions[satellite][component] =
				receiver_position[component] +
				directions[satellite][component] * 21000000.0;
			los_satellite_velocity +=
				directions[satellite][component] *
				satellite_velocities[satellite][component];
			los_receiver_velocity +=
				directions[satellite][component] *
				expected_velocity[component];
		}
		measurements[satellite].valid = true;
		measurements[satellite].locked = true;
		measurements[satellite].cn0_dbhz = 42.0 + (double)satellite;
		measurements[satellite].pseudorange_rate_mps =
			los_satellite_velocity -
			los_receiver_velocity +
			expected_clock_drift_mps -
			(double)SPEED_OF_LIGHT *
			satellite_clock_drift[satellite];
	}

	double solved_velocity[3];
	double solved_clock_drift;
	double residual_rms;
	assert(gps_tracking_velocity_wls(
		receiver_position,
		satellite_positions,
		satellite_velocities,
		satellite_clock_drift,
		measurements,
		TEST_SATELLITES,
		solved_velocity,
		&solved_clock_drift,
		&residual_rms));

	for(unsigned int component = 0u; component < 3u; component++)
	{
		assert(close_to(
			solved_velocity[component],
			expected_velocity[component],
			1.0e-9));
	}
	assert(close_to(
		solved_clock_drift,
		expected_clock_drift_mps,
		1.0e-9));
	assert(residual_rms < 1.0e-9);
}

int main(void)
{
	test_cn0_and_mailbox_decode();
	test_velocity_wls();
	puts("test_gps_tracking_measurement PASS");
	return 0;
}