/*
 * app_imu_dmp.cpp
 *
 *  Created on: Nov 11, 2025
 *      Author: matus
 */

#include <app_imu_dmp.h>
#include "DMP_ICM20948.h"
#include "string.h"

/* Internal Variables ------------------------------------------------------------*/
DMP_ICM20948 _imuDmpInstance;
DMP_ICM20948Settings _imuDmpSettings = {
  .mode = 1,                            	// 0 = low power mode, 1 = high performance mode
  .enable_gyroscope = true,             	// Enables gyroscope output
  .enable_accelerometer = true,         	// Enables accelerometer output
  .enable_magnetometer = false,         	// Enables magnetometer output // Enables quaternion output
  .enable_gravity = false,              	// Enables gravity vector output
  .enable_linearAcceleration = false,   	// Enables linear acceleration output
  .enable_quaternion6 = true,           	// Enables quaternion 6DOF output
  .enable_quaternion9 = false,          	// Enables quaternion 9DOF output
  .enable_har = false,                  	// Enables activity recognition
  .enable_steps = false,                	// Enables step counter
  .gyroscope_frequency = 225,           	// Max frequency = 225, min frequency = 1
  .accelerometer_frequency = 225,       	// Max frequency = 225, min frequency = 1
  .magnetometer_frequency = 70,         	// Max frequency = 70, min frequency = 1
  .gravity_frequency = 225,             	// Max frequency = 225, min frequency = 1
  .linearAcceleration_frequency = 225,  	// Max frequency = 225, min frequency = 1
  .quaternion6_frequency = 225,         	// Max frequency = 225, min frequency = 50
  .quaternion9_frequency = 225,         	// Max frequency = 225, min frequency = 50
  .har_frequency = 225,                 	// Max frequency = 225, min frequency = 50
  .steps_frequency = 225                	// Max frequency = 225, min frequency = 50
};

nBusAppInterface_t _imuDmpNbusDriver = {
  imuDmp_init,
  imuDmp_reset,
  imuDmp_getType,
  imuDmp_getSensorCount,
  imuDmp_getData,
  imuDmp_setData,
  imuDmp_hasParam,
  imuDmp_getParam,
  imuDmp_setParam,
  imuDmp_start,
  imuDmp_stop,
  imuDmp_readData,
  imuDmp_store,
  imuDmp_calibrate,
  imuDmp_getSensorFormat,
  imuDmp_find,
  imuDmp_device_ready
};


/* Internal Helper Functions ------------------------------------------------------------*/


static inv_sensor_type _convert_sensor_id_to_inv_type(imu_sensorID_t sensor_index)
{
	switch(sensor_index)
		{
		case ID_ACCELEROMETER:
			return INV_SENSOR_TYPE_ACCELEROMETER;
			break;
		case ID_GYROSCOPE:
			return INV_SENSOR_TYPE_GYROSCOPE;
			break;
		case ID_EULER_ANGLES_GAUGE:
			return INV_SENSOR_TYPE_GAME_ROTATION_VECTOR;
			break;
		default:
			return INV_SENSOR_TYPE_RESERVED;
		}
}

static nBus_statusType_t _update_enable_status(DMP_ICM20948 *imuDmpInstance, DMP_ICM20948Settings *imuDmpSettings, imu_sensorID_t sensor_index, uint8_t value)
{
	if (value > 1)
		return STATUS_FAIL;

	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		imuDmpSettings->enable_accelerometer = value;
		break;
	case ID_GYROSCOPE:
		imuDmpSettings->enable_gyroscope = value;
		break;
	case ID_EULER_ANGLES_GAUGE:
		imuDmpSettings->enable_quaternion6 = value;
		break;
	default:
		return STATUS_FAIL;
	}

	imuDmpInstance->enableSensor(_convert_sensor_id_to_inv_type(sensor_index), value);

	return STATUS_SUCCESS;
}

static nBus_statusType_t _update_samplerate(DMP_ICM20948 *imuDmpInstance, DMP_ICM20948Settings *imuDmpSettings, imu_sensorID_t sensor_index, uint32_t value)
{

	if (value < 1 || value > 225)
		return STATUS_FAIL;

	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		imuDmpSettings->accelerometer_frequency = value;
		break;
	case ID_GYROSCOPE:
		imuDmpSettings->gyroscope_frequency = value;
		break;
	case ID_EULER_ANGLES_GAUGE:
		imuDmpSettings->quaternion6_frequency = value;
		break;
	default:
		return STATUS_FAIL;
	}


	imuDmpInstance->setSensorFrequency(_convert_sensor_id_to_inv_type(sensor_index), value);

	return STATUS_SUCCESS;
}

static uint8_t _get_enable_status(DMP_ICM20948Settings *imuDmpSettings, imu_sensorID_t sensor_index)
{

	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		return imuDmpSettings->enable_accelerometer;
		break;
	case ID_GYROSCOPE:
		return imuDmpSettings->enable_gyroscope;
		break;
	case ID_EULER_ANGLES_GAUGE:
		return imuDmpSettings->enable_quaternion6;
		break;
	default:
		return STATUS_FAIL;
	}
}

static uint32_t _get_samplerate(DMP_ICM20948Settings *imuDmpSettings, imu_sensorID_t sensor_index)
{
	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		return imuDmpSettings->accelerometer_frequency;
		break;
	case ID_GYROSCOPE:
		return imuDmpSettings->gyroscope_frequency;
		break;
	case ID_EULER_ANGLES_GAUGE:
		return imuDmpSettings->quaternion6_frequency;
		break;
	default:
		return STATUS_FAIL;
	}

	return STATUS_SUCCESS;
}

/* nBUS Interface Implementation  ------------------------------------------------------------*/


nBusAppInterface_t *getImuDriver()
{
	return &_imuDmpNbusDriver;
}

nBus_sensorType_t imuDmp_getType(uint8_t sensor_index)
{
	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		return TYPE_ACCELEROMETER;
		break;
	case ID_GYROSCOPE:
		return TYPE_GYROSCOPE;
		break;
	case ID_EULER_ANGLES_GAUGE:
		return TYPE_EULER_ANGLES_GAUGE;
		break;
	default:
		return TYPE_UNKNOWN;
	}
}

nBus_sensorCount_t imuDmp_getSensorCount()
{
	return (nBus_sensorCount_t){3, 0};
}


uint8_t imuDmp_getData(uint8_t sensor_index, uint8_t *data)
{
	size_t data_offset = 0;
	float values[3];
	int32_t int_value;
	uint8_t use_accel = 0, use_gyro = 0, use_euler = 0;

	switch(sensor_index)
	{
	case ID_ALL:
		use_accel = _imuDmpSettings.enable_accelerometer && _imuDmpInstance.accelDataIsReady();
		use_gyro = _imuDmpSettings.enable_gyroscope && _imuDmpInstance.gyroDataIsReady();
		use_euler = _imuDmpSettings.enable_quaternion6 && _imuDmpInstance.euler6DataIsReady();
		break;
	case ID_ACCELEROMETER:
		use_accel = _imuDmpSettings.enable_accelerometer && _imuDmpInstance.accelDataIsReady();
		break;
	case ID_GYROSCOPE:
		use_gyro = _imuDmpSettings.enable_gyroscope && _imuDmpInstance.gyroDataIsReady();
		break;
	case ID_EULER_ANGLES_GAUGE:
		use_euler = _imuDmpSettings.enable_quaternion6 && _imuDmpInstance.euler6DataIsReady();
		break;
	default:
		break;
	}

	if (use_accel)
	{
		_imuDmpInstance.readAccelData(&values[0], &values[1], &values[2]);

		data[data_offset++] = (uint8_t)ID_ACCELEROMETER;

		for (uint8_t i = 0; i < 3; i++)
		{
			int_value = (int32_t)(values[i] * ACC_MULT_NUM);
			memcpy(data + data_offset, &int_value, ACC_BYTES);
			data_offset += ACC_BYTES;
		}
	}

	if (use_gyro)
	{
		_imuDmpInstance.readGyroData(&values[0], &values[1], &values[2]);

		data[data_offset++] = (uint8_t)ID_GYROSCOPE;

		for (uint8_t i = 0; i < 3; i++)
		{
			int_value = (int32_t)(values[i] * GYR_MULT_NUM);
			memcpy(data + data_offset, &int_value, GYR_BYTES);
			data_offset += GYR_BYTES;

		}
	}

	if (use_euler)
	{
		_imuDmpInstance.readEuler6Data(&values[0], &values[1], &values[2]);

		data[data_offset++] = (uint8_t)ID_EULER_ANGLES_GAUGE;

		for (uint8_t i = 0; i < 3; i++)
		{
			int_value = (int32_t)(values[i] * EUL_MULT_NUM);
			memcpy(data + data_offset, &int_value, EUL_BYTES);
			data_offset += EUL_BYTES;
		}
	}

	return data_offset;
}

nBus_statusType_t imuDmp_setData(uint8_t *data, uint8_t count, uint8_t *response)
{
	return STATUS_FAIL;
}

uint8_t imuDmp_hasParam(uint8_t sensor_index, nBus_param_t param_name)
{
	if (sensor_index == 0)
		return param_name == PARAM_MODE;

	switch(param_name)
	{
	case PARAM_ENABLE:
	case PARAM_SAMPLERATE:
		return 1;
		break;
	default:
		return 0;
	}
}

int32_t imuDmp_getParam(uint8_t sensor_index, nBus_param_t param_name)
{
	if (imuDmp_hasParam(sensor_index, param_name))
	{
		switch(param_name)
		{
		case PARAM_ENABLE:
			return _get_enable_status(&_imuDmpSettings, (imu_sensorID_t)sensor_index);
			break;

		case PARAM_SAMPLERATE:
			return _get_samplerate(&_imuDmpSettings, (imu_sensorID_t)sensor_index);
			break;

		case PARAM_MODE:
			return _imuDmpSettings.mode;
			break;

		default:
			return 0;
		}
	}

	return 0;
}

nBus_statusType_t imuDmp_setParam(uint8_t sensor_index, nBus_param_t param_name, int32_t param_value)
{
	if (imuDmp_hasParam(sensor_index, param_name))
	{
		switch(param_name)
		{
		case PARAM_ENABLE:
			return _update_enable_status(&_imuDmpInstance, &_imuDmpSettings, (imu_sensorID_t)sensor_index, param_value);
			break;

		case PARAM_SAMPLERATE:
			return _update_samplerate(&_imuDmpInstance, &_imuDmpSettings, (imu_sensorID_t)sensor_index, param_value);
			break;

		case PARAM_MODE:
			_imuDmpInstance.setOperatingMode(param_value);
			return STATUS_SUCCESS;
			break;

		default:
			return STATUS_FAIL;
		}
	}

	return STATUS_NOT_SUPPORTED;
}

void imuDmp_init(void *hw_interface, void *hw_config)
{
	_imuDmpInstance.init(_imuDmpSettings);
}

void imuDmp_reset()
{
  // not implemented
}

nBus_statusType_t imuDmp_start(void)
{
	return STATUS_SUCCESS;
}

nBus_statusType_t imuDmp_stop(void)
{
	return STATUS_SUCCESS;
}


void imuDmp_readData(void)
{
	_imuDmpInstance.task();
}

uint8_t imuDmp_store(void)
{
	return 0; 	// not implemented
}

nBus_statusType_t imuDmp_calibrate(uint8_t sensor_index)
{
	return STATUS_NOT_SUPPORTED;
}

nBus_sensorFormat_t imuDmp_getSensorFormat(uint8_t sensor_index)
{
	switch(sensor_index)
	{
	case ID_ACCELEROMETER:
		return (nBus_sensorFormat_t){.sign = 1, .unit_multiplier = 0, .value_multiplier = ACC_MULT_LOG, .byte_length = ACC_BYTES, .samples = 3};
		break;
	case ID_GYROSCOPE:
		return (nBus_sensorFormat_t){.sign = 1, .unit_multiplier = 0, .value_multiplier = GYR_MULT_LOG, .byte_length = GYR_BYTES, .samples = 3};
		break;
	case ID_EULER_ANGLES_GAUGE:
		return (nBus_sensorFormat_t){.sign = 1, .unit_multiplier = 0, .value_multiplier = EUL_MULT_LOG, .byte_length = EUL_BYTES, .samples = 3};
		break;
	default:
		return (nBus_sensorFormat_t){0, 0, 0, 0, 0};
		break;
	}
}

nBus_statusType_t imuDmp_find(uint8_t enable)
{
	return STATUS_NOT_SUPPORTED;
}

uint8_t imuDmp_device_ready()
{
	return 1;
}