#include "imu/imu_calibration.h"

#include <math.h>
#include <stddef.h>
#include <string.h>

static bool vector_is_finite(const double vector[3])
{
	return isfinite(vector[0]) && isfinite(vector[1]) && isfinite(vector[2]);
}

static bool matrix_is_finite_and_invertible(const double matrix[3][3])
{
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		if(!vector_is_finite(matrix[row]))
		{
			return false;
		}
	}
	double determinant =
		matrix[0][0] * (matrix[1][1] * matrix[2][2] -
		                matrix[1][2] * matrix[2][1]) -
		matrix[0][1] * (matrix[1][0] * matrix[2][2] -
		                matrix[1][2] * matrix[2][0]) +
		matrix[0][2] * (matrix[1][0] * matrix[2][1] -
		                matrix[1][1] * matrix[2][0]);
	return isfinite(determinant) && (fabs(determinant) > 1.0e-12);
}

void imu_calibration_default(imu_calibration_t* calibration)
{
	if(calibration == NULL)
	{
		return;
	}
	memset(calibration, 0, sizeof(*calibration));
	for(unsigned int axis = 0 ; axis < 3u ; axis++)
	{
		calibration->accel_transform[axis][axis] = 1.0;
		calibration->gyro_transform[axis][axis] = 1.0;
	}
	calibration->reference_temperature_c = 25.0;
}

bool imu_calibration_is_valid(const imu_calibration_t* calibration)
{
	if((calibration == NULL) ||
	   !vector_is_finite(calibration->accel_bias_mps2) ||
	   !vector_is_finite(calibration->gyro_bias_rps) ||
	   !matrix_is_finite_and_invertible(calibration->accel_transform) ||
	   !matrix_is_finite_and_invertible(calibration->gyro_transform) ||
	   !isfinite(calibration->reference_temperature_c) ||
	   !vector_is_finite(calibration->accel_temperature_coefficient_mps2_per_c) ||
	   !vector_is_finite(calibration->gyro_temperature_coefficient_rps_per_c))
	{
		return false;
	}
	return true;
}

bool imu_calibration_apply(const imu_calibration_t* calibration,
	                       imu_sample_t* measurement)
{
	if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
	   !vector_is_finite(measurement->accel_mps2) ||
	   !vector_is_finite(measurement->gyro_rps))
	{
		return false;
	}
	double temperature_delta_c = 0.0;
	if(calibration->temperature_compensation_enabled)
	{
		if(!isfinite(measurement->temperature_c))
		{
			return false;
		}
		temperature_delta_c =
			measurement->temperature_c - calibration->reference_temperature_c;
	}

	double accel_unbiased[3];
	double gyro_unbiased[3];
	for(unsigned int axis = 0 ; axis < 3u ; axis++)
	{
		accel_unbiased[axis] =
			measurement->accel_mps2[axis] - calibration->accel_bias_mps2[axis] -
			calibration->accel_temperature_coefficient_mps2_per_c[axis] *
				temperature_delta_c;
		gyro_unbiased[axis] =
			measurement->gyro_rps[axis] - calibration->gyro_bias_rps[axis] -
			calibration->gyro_temperature_coefficient_rps_per_c[axis] *
				temperature_delta_c;
	}

	double accel_corrected[3] = {0.0};
	double gyro_corrected[3] = {0.0};
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			accel_corrected[row] +=
				calibration->accel_transform[row][col] * accel_unbiased[col];
			gyro_corrected[row] +=
				calibration->gyro_transform[row][col] * gyro_unbiased[col];
		}
	}
	memcpy(measurement->accel_mps2, accel_corrected,
	       sizeof(measurement->accel_mps2));
	memcpy(measurement->gyro_rps, gyro_corrected,
	       sizeof(measurement->gyro_rps));
	return true;
}