#include "gps/NavEquations.h"

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

static void test_broadcast_ephemeris_velocity(void)
{
	gps_ephemeris_t ephemeris;
	memset(&ephemeris, 0, sizeof(ephemeris));
	ephemeris.tow.week = 2400;
	ephemeris.tow.sec = 345700.0;
	ephemeris.toe.week = 2400;
	ephemeris.toe.sec = 345600.0;
	ephemeris.toc.week = 2400;
	ephemeris.toc.sec = 345600.0;
	ephemeris.sqrta = 5153.7954775;
	ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
	ephemeris.ecc = 0.0093;
	ephemeris.m0 = 0.75;
	ephemeris.deltan = 4.5e-9;
	ephemeris.cuc = 1.0e-6;
	ephemeris.cus = -3.0e-6;
	ephemeris.crc = 180.0;
	ephemeris.crs = -75.0;
	ephemeris.cic = 1.2e-7;
	ephemeris.cis = -8.0e-8;
	ephemeris.inc0 = 0.96;
	ephemeris.idot = -2.0e-10;
	/* Legacy names: omgdot=Omega0, omg0=omega, omgkdot=Omega-dot. */
	ephemeris.omgdot = 1.1;
	ephemeris.omg0 = -0.4;
	ephemeris.omgkdot = -8.0e-9;
	ephemeris.af0 = 2.3e-5;
	ephemeris.af1 = -4.0e-12;
	ephemeris.af2 = 1.0e-19;
	ephemeris.tgd = -2.0e-9;

	double position[3];
	double velocity[3];
	double clock[2];
	double correction;
	satpos(&ephemeris, position, velocity, clock, &correction);

	const double dt = 0.05;
	gps_ephemeris_t before = ephemeris;
	gps_ephemeris_t after = ephemeris;
	before.tow.sec -= dt;
	after.tow.sec += dt;
	double position_before[3], position_after[3];
	double velocity_unused[3], clock_before[2], clock_after[2];
	double correction_before, correction_after;
	satpos(&before, position_before, velocity_unused,
	       clock_before, &correction_before);
	satpos(&after, position_after, velocity_unused,
	       clock_after, &correction_after);

	double radius_squared = 0.0;
	double speed_squared = 0.0;
	for(unsigned int component = 0u; component < 3u; component++)
	{
		double finite_difference =
			(position_after[component] - position_before[component]) /
			(2.0 * dt);
		assert(fabs(velocity[component] - finite_difference) < 0.05);
		radius_squared += position[component] * position[component];
		speed_squared += velocity[component] * velocity[component];
	}
	double finite_clock_drift =
		(correction_after - correction_before) / (2.0 * dt);
	assert(fabs(clock[1] - finite_clock_drift) < 1.0e-13);
	assert(fabs(clock[0] - correction) < 1.0e-18);
	assert(sqrt(radius_squared) > 25000000.0);
	assert(sqrt(radius_squared) < 28000000.0);
	assert(sqrt(speed_squared) > 1000.0);
	assert(sqrt(speed_squared) < 5000.0);
}

static void test_cold_wls_accepts_differential_timer_datum(void)
{
	const unsigned int satellite_count = 6u;
	/* Keep this convergence test above the atmospheric-model ceiling; the
	 * timer datum and cold WLS state are the variables under test here. */
	const double receiver[3] = {6398137.0, 0.0, 0.0};
	const double range_m = 21000000.0;
	const double directions[6][3] = {
		{ 1.0,  0.0,  0.0},
		{ 0.2,  0.9,  0.3872983346207417},
		{ 0.3, -0.4,  0.8660254037844386},
		{-0.2,  0.5,  0.8426149773176358},
		{ 0.4, -0.8, -0.4472135954999579},
		{-0.5, -0.5,  0.7071067811865475}
	};
	mat_type* satellite_positions =
		create_matrix((int)satellite_count, 3, "coldDifferentialSatPos");
	mat_type* observations =
		create_matrix(1, (int)satellite_count, "coldDifferentialObs");
	mat_type* az = create_matrix(1, (int)satellite_count, "coldDifferentialAz");
	mat_type* el = create_matrix(1, (int)satellite_count, "coldDifferentialEl");
	assert(satellite_positions != NULL);
	assert(observations != NULL);
	assert(az != NULL);
	assert(el != NULL);
	initialize_all(0.0, satellite_positions);
	initialize_all(0.0, observations);
	initialize_all(0.0, az);
	initialize_all(0.0, el);
	double modeled_range_m[6];

	for(unsigned int satellite = 0u;
	    satellite < satellite_count;
	    satellite++)
	{
		for(unsigned int component = 0u; component < 3u; component++)
		{
			satellite_positions->matrix[satellite][component] =
				receiver[component] +
				directions[satellite][component] * range_m;
		}
		double earth_angle = OMEGA_EARTH * range_m / SPEED_OF_LIGHT;
		double rotated[3] = {
			cos(earth_angle) * satellite_positions->matrix[satellite][0] +
			sin(earth_angle) * satellite_positions->matrix[satellite][1],
			-sin(earth_angle) * satellite_positions->matrix[satellite][0] +
			cos(earth_angle) * satellite_positions->matrix[satellite][1],
			satellite_positions->matrix[satellite][2]
		};
		modeled_range_m[satellite] = sqrt(
			(rotated[0] - receiver[0]) * (rotated[0] - receiver[0]) +
			(rotated[1] - receiver[1]) * (rotated[1] - receiver[1]) +
			(rotated[2] - receiver[2]) * (rotated[2] - receiver[2]));
	}
	for(unsigned int satellite = 0u;
	    satellite < satellite_count;
	    satellite++)
	{
		observations->matrix[0][satellite] =
			modeled_range_m[satellite] - modeled_range_m[0];
	}

	unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u};
	double cn0_dbhz[6] = {45.0, 45.0, 45.0, 45.0, 45.0, 45.0};
	gps_nav_solution_t solution;
	assert(leastSquaresSTD(
		satellite_positions, observations, (int)satellite_count,
		az, el, ura, cn0_dbhz, NULL, 345700.0, &solution));
	double error_m = sqrt(
		(solution.ecef_m[0] - receiver[0]) *
		(solution.ecef_m[0] - receiver[0]) +
		(solution.ecef_m[1] - receiver[1]) *
		(solution.ecef_m[1] - receiver[1]) +
		(solution.ecef_m[2] - receiver[2]) *
		(solution.ecef_m[2] - receiver[2]));
	assert(solution.solution_valid);
	assert(error_m < 10.0);
	assert(solution.clock_bias_m < -20000000.0);
	assert(solution.clock_bias_m > -22000000.0);

	free_matrix(satellite_positions);
	free_matrix(observations);
	free_matrix(az);
	free_matrix(el);
}

static void test_cn0_changes_position_weight(void)
{
	const unsigned int satellite_count = 6u;
	const double receiver[3] = {6378137.0, 0.0, 0.0};
	const double directions[6][3] = {
		{ 1.0,  0.0,  0.0},
		{ 0.2,  0.9,  0.3872983346207417},
		{ 0.3, -0.4,  0.8660254037844386},
		{-0.2,  0.5,  0.8426149773176358},
		{ 0.4, -0.8, -0.4472135954999579},
		{-0.5, -0.5,  0.7071067811865475}
	};
	mat_type* satellite_positions =
		create_matrix((int)satellite_count, 3, "testSatPos");
	mat_type* observations =
		create_matrix(1, (int)satellite_count, "testObs");
	mat_type* az = create_matrix(1, (int)satellite_count, "testAz");
	mat_type* el = create_matrix(1, (int)satellite_count, "testEl");
	assert(satellite_positions != NULL);
	assert(observations != NULL);
	assert(az != NULL);
	assert(el != NULL);
	initialize_all(0.0, satellite_positions);
	initialize_all(0.0, observations);
	initialize_all(0.0, az);
	initialize_all(0.0, el);

	unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u};
	double cn0_dbhz[6] = {52.0, 22.0, 45.0, 45.0, 45.0, 45.0};
	for(unsigned int satellite = 0u;
	    satellite < satellite_count;
	    satellite++)
	{
		double range_squared = 0.0;
		for(unsigned int component = 0u; component < 3u; component++)
		{
			satellite_positions->matrix[satellite][component] =
				receiver[component] +
				directions[satellite][component] * 21000000.0;
			double difference =
				satellite_positions->matrix[satellite][component] -
				receiver[component];
			range_squared += difference * difference;
		}
		observations->matrix[0][satellite] =
			sqrt(range_squared) + 125.0;
	}

	gps_nav_solution_t solution;
	assert(leastSquaresSTD(
		satellite_positions, observations, (int)satellite_count,
		az, el, ura, cn0_dbhz, NULL, 345700.0, &solution));
	assert(isfinite(solution.measurement_sigma_m[0]));
	assert(isfinite(solution.measurement_sigma_m[1]));
	assert(solution.measurement_sigma_m[1] >
	       solution.measurement_sigma_m[0] + 20.0);

	free_matrix(satellite_positions);
	free_matrix(observations);
	free_matrix(az);
	free_matrix(el);
}

int main(void)
{
	test_broadcast_ephemeris_velocity();
	test_cold_wls_accepts_differential_timer_datum();
	test_cn0_changes_position_weight();
	puts("test_gps_satellite_velocity PASS");
	return 0;
}