#include "imu/imu_calibration.h"

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

static bool near(double actual, double expected)
{
	return fabs(actual - expected) < 1.0e-12;
}

static void test_identity_is_noop(void)
{
	imu_calibration_t calibration;
	imu_calibration_default(&calibration);
	imu_sample_t measurement = {
		.accel_mps2 = {1.0, -2.0, 9.7},
		.gyro_rps = {0.01, -0.02, 0.03},
		.temperature_c = 41.0
	};
	imu_sample_t expected = measurement;
	assert(imu_calibration_apply(&calibration, &measurement));
	assert(memcmp(measurement.accel_mps2, expected.accel_mps2,
	              sizeof(measurement.accel_mps2)) == 0);
	assert(memcmp(measurement.gyro_rps, expected.gyro_rps,
	              sizeof(measurement.gyro_rps)) == 0);
}

static void test_bias_transform_and_temperature(void)
{
	imu_calibration_t calibration;
	imu_calibration_default(&calibration);
	for(unsigned int axis = 0 ; axis < 3u ; axis++)
	{
		calibration.accel_bias_mps2[axis] = 1.0;
		calibration.gyro_bias_rps[axis] = 0.1;
		calibration.accel_temperature_coefficient_mps2_per_c[axis] = 0.1;
		calibration.gyro_temperature_coefficient_rps_per_c[axis] = 0.01;
	}
	memset(calibration.accel_transform, 0,
	       sizeof(calibration.accel_transform));
	calibration.accel_transform[0][1] = 1.0;
	calibration.accel_transform[1][0] = -1.0;
	calibration.accel_transform[2][2] = 2.0;
	calibration.reference_temperature_c = 25.0;
	calibration.temperature_compensation_enabled = true;

	imu_sample_t measurement = {
		.accel_mps2 = {3.0, 5.0, 7.0},
		.gyro_rps = {0.4, 0.5, 0.6},
		.temperature_c = 35.0
	};
	assert(imu_calibration_apply(&calibration, &measurement));
	assert(near(measurement.accel_mps2[0], 3.0));
	assert(near(measurement.accel_mps2[1], -1.0));
	assert(near(measurement.accel_mps2[2], 10.0));
	assert(near(measurement.gyro_rps[0], 0.2));
	assert(near(measurement.gyro_rps[1], 0.3));
	assert(near(measurement.gyro_rps[2], 0.4));
}

static void test_singular_transform_is_rejected(void)
{
	imu_calibration_t calibration;
	imu_calibration_default(&calibration);
	memset(calibration.gyro_transform, 0, sizeof(calibration.gyro_transform));
	assert(!imu_calibration_is_valid(&calibration));
}

int main(void)
{
	test_identity_is_noop();
	test_bias_transform_and_temperature();
	test_singular_transform_is_rejected();
	puts("imu_calibration tests passed");
	return 0;
}