/*
 * app_imu.cpp
 *
 *  Created on: Nov 23, 2023
 *      Author: juraj
 */

#include "app_imu.h"
#include "IcmSpiManager.h"
#include "icm20948.h"


extern SPI_HandleTypeDef hspi1;

nBusAppInterface_t mcu_spi_driver = {
	  mcu_spi_init,
	  mcu_spi_reset,
	  mcu_spi_getType,
	  mcu_spi_getSensorCount,
	  mcu_spi_getData,
	  mcu_spi_setData,
	  mcu_spi_hasParam,
	  mcu_spi_getParam,
	  mcu_spi_setParam,
	  mcu_spi_start,
	  mcu_spi_stop,
	  mcu_spi_readData,
	  NULL, //mcu_spi_store,
	  mcu_spi_calibrate,
	  NULL, //getSensorFormat
	  NULL, //find
	  mcu_spi_deviceReady
};


axisesI *sensor_dataI;  // int16 values

Icm20948 *sensor;
IcmSpiManager *manager;

nBusAppInterface_t *getImuDriver(){
	return &mcu_spi_driver;
}

static accel_samplerate getAccelParamSamplerate(int32_t value){

	switch(value){
	case ACCEL_samplerate_0_27Hz:
	case ACCEL_samplerate_0_55Hz:
	case ACCEL_samplerate_102_3Hz:
	case ACCEL_samplerate_140_6Hz:
	case ACCEL_samplerate_17_6Hz:
	case ACCEL_samplerate_187_5Hz:
	case ACCEL_samplerate_1_1Hz:
	case ACCEL_samplerate_281_3Hz:
	case ACCEL_samplerate_2_2Hz:
	case ACCEL_samplerate_35_2Hz:
	case ACCEL_samplerate_48_9Hz:
	case ACCEL_samplerate_4_4Hz:
	case ACCEL_samplerate_562_5Hz:
	case ACCEL_samplerate_70_3Hz:
	case ACCEL_samplerate_8_8Hz:
		return (accel_samplerate)value;
	}
	return ACCEL_samplerate_None;

}


static accel_full_scale getAccelParamFullScale(int32_t value){
	switch(value){
	case ACCEL_FS_2g:
	case ACCEL_FS_4g:
	case ACCEL_FS_8g:
	case ACCEL_FS_16g:
		return (accel_full_scale)value;
	}
	return ACCEL_FS_NoneG;
}


static accel_dlp_cfg getAccelParamLowPassFilter(int32_t value){
	switch(value){
	case ACCEL_lpf_005_7Hz:
	case ACCEL_lpf_011_5Hz:
	case ACCEL_lpf_023_9Hz:
	case ACCEL_lpf_050_4Hz:
	case ACCEL_lpf_114_4Hz:
	case ACCEL_lpf_246Hz:
	case ACCEL_lpf_473Hz:
	case ACCEL_lpf_OFF:
		return (accel_dlp_cfg)value;
	}
	return ACCEL_lpf_None;
}

static gyro_samplerate getGyroParamSamplerate(int32_t value){

	switch(value){
	case GYRO_samplerate_004_4Hz:
	case GYRO_samplerate_017_3Hz:
	case GYRO_samplerate_017_6Hz:
	case GYRO_samplerate_034_1Hz:
	case GYRO_samplerate_035_2Hz:
	case GYRO_samplerate_048_9Hz:
	case GYRO_samplerate_066_2Hz:
	case GYRO_samplerate_070_3Hz:
	case GYRO_samplerate_102_3Hz:
	case GYRO_samplerate_125_0Hz:
	case GYRO_samplerate_140_6Hz:
	case GYRO_samplerate_187_5Hz:
	case GYRO_samplerate_225_0Hz:
	case GYRO_samplerate_281_3Hz:
	case GYRO_samplerate_375_0Hz:
	case GYRO_samplerate_562_5Hz:
		return (gyro_samplerate)value;
	}
	return GYRO_samplerate_None;
}

static gyro_full_scale getGyroParamFullScale(int32_t value){
	switch(value){
	case GYRO_FS_1000dps:
	case GYRO_FS_2000dps:
	case GYRO_FS_250dps:
	case GYRO_FS_500dps:
		return (gyro_full_scale)value;
	}
	return GYRO_FS_None;
}

static gyro_dlp_cfg getGyroParamLowPassFilter(int32_t value){
	switch(value){
	case GYRO_low_pass_OFF:
	case GYRO_lpf_005_7Hz:
	case GYRO_lpf_011_6Hz:
	case GYRO_lpf_023_9Hz:
	case GYRO_lpf_051_2Hz:
	case GYRO_lpf_119_5Hz:
	case GYRO_lpf_151_8Hz:
	case GYRO_lpf_196_6Hz:
	case GYRO_lpf_361_4Hz:
		return (gyro_dlp_cfg)value;
	}
	return GYRO_lpf_None;
}

void mcu_spi_init(void *hw_interface, void *hw_config){
	manager = new IcmSpiManager((SPI_HandleTypeDef*)hw_interface);		// TODO toto ma byt o uroven vyssie, ale je to c subor
	sensor = new Icm20948(manager, (icm20948_Config*)hw_config);
}

void mcu_spi_reset(){
	sensor->Reset();
}

nBus_statusType_t mcu_spi_start(){
	sensor->Start();
	return STATUS_SUCCESS;
}

nBus_statusType_t mcu_spi_stop(){
	sensor->Stop();
	return STATUS_SUCCESS;
}

void mcu_spi_readData(void){
	sensor->Read();
}

nBus_sensorType_t mcu_spi_getType(uint8_t sensor_index){

	nBus_sensorCount_t sc = mcu_spi_getSensorCount();
	if (sensor_index > sc.read_only_count) {
		return TYPE_UNKNOWN;
	}

	if (sensor_index == 1) {
		return TYPE_ACCELEROMETER;
	}

	if (sensor_index == 2) {
		return TYPE_GYROSCOPE;
	}

	return TYPE_UNKNOWN;
}


nBus_sensorCount_t mcu_spi_getSensorCount(){
	nBus_sensorCount_t sc = {2,0};
	return sc;
}


uint8_t mcu_spi_getData(uint8_t sensor_index, uint8_t *data){
	if(sensor_index == 1){
		sensor_dataI = sensor->accSensor->GetData();
	}
	if(sensor_index == 2){
		sensor_dataI = sensor->gyroSensor->GetData();
	}

	data[0] = sensor_index;

	data[1] = (uint8_t)sensor_dataI->x & 0xFF;
	data[2] = (uint8_t)((sensor_dataI->x >> 8) & 0xFF);
	data[3] = (uint8_t)sensor_dataI->y & 0xFF;
	data[4] = (uint8_t)((sensor_dataI->y >> 8) & 0xFF);
	data[5] = (uint8_t)sensor_dataI->z & 0xFF;
	data[6] = (uint8_t)((sensor_dataI->z >> 8) & 0xFF);

	return 7;
}

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

int32_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param){
	uint32_t param_value = PARAM_VALUE_NONE;
	// to module
	if(sensor_index == 0) {
		return PARAM_VALUE_NONE;
	}

	if(sensor_index == 1) {
		switch(param){
		case PARAM_SAMPLERATE:
			param_value = sensor->accSensor->GetSampleRate();
			break;
		case PARAM_RANGE:
			param_value = sensor->accSensor->GetRange();
			break;
		case PARAM_FILTER:
			param_value = sensor->accSensor->GetLowPassFilter();
			break;
		default:
			param_value = PARAM_VALUE_NONE;
		}
	}

	if(sensor_index == 2) {
		switch(param){
		case PARAM_SAMPLERATE:
			param_value = sensor->gyroSensor->GetSampleRate();
			break;
		case PARAM_RANGE:
			param_value = sensor->gyroSensor->GetRange();
			break;
		case PARAM_FILTER:
			param_value = sensor->gyroSensor->GetLowPassFilter();
			break;
		default:
			param_value = PARAM_VALUE_NONE;
		}
	}

	return param_value;
}

uint8_t mcu_spi_hasParam(uint8_t sensor_index, nBus_param_t param){
	if(sensor_index == 1 || sensor_index == 2){
		switch(param){
		case PARAM_SAMPLERATE:
		case PARAM_RANGE:
		case PARAM_FILTER:
			return 1;
		default:
			return 0;
		}
		return 0;
	}
	return 0;
}

nBus_statusType_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, int32_t value) {
	// to module
	if(sensor_index == 0) {
		return STATUS_NOT_SUPPORTED;
	}

	if(sensor_index == 1) {
		switch(param){
		case PARAM_SAMPLERATE:
		{
			accel_samplerate sr = getAccelParamSamplerate(value);
			if(sr != ACCEL_samplerate_None) {
				sensor->accSensor->SetSampleRate(sr);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		case PARAM_RANGE:
		{
			accel_full_scale fs = getAccelParamFullScale(value);
			if(fs != ACCEL_FS_NoneG) {
				sensor->accSensor->SetRange(fs);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		case PARAM_FILTER:
		{
			accel_dlp_cfg lpf = getAccelParamLowPassFilter(value);
			if(lpf != ACCEL_lpf_None) {
				sensor->accSensor->SetLowPassFilter(lpf);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		default:
			return STATUS_NOT_SUPPORTED;
		}
	}

	if(sensor_index == 2) {
		switch(param){
		case PARAM_SAMPLERATE:
		{
			gyro_samplerate sr = getGyroParamSamplerate(value);
			if(sr != GYRO_samplerate_None) {
				sensor->gyroSensor->SetSampleRate(sr);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		case PARAM_RANGE:
		{
			gyro_full_scale fs = getGyroParamFullScale(value);
			if(fs != GYRO_FS_None) {
				sensor->gyroSensor->SetRange(fs);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		case PARAM_FILTER:
		{
			gyro_dlp_cfg lpf = getGyroParamLowPassFilter(value);
			if(lpf != GYRO_lpf_None) {
				sensor->gyroSensor->SetLowPassFilter(lpf);
			} else {
				return STATUS_NOT_SUPPORTED;
			}

		}
			break;
		default:
			return STATUS_NOT_SUPPORTED;
		}
	}

	return STATUS_SUCCESS;

}

/**
 * @param sebslaveIndex sensorAccIndex, sensorGyroIndex, sensorAll
 */
nBus_statusType_t mcu_spi_calibrate(uint8_t subslaveIndex){

	uint8_t wasReady = sensor->IsReady();
	sensor->Stop();
	nBus_statusType_t success = STATUS_SUCCESS;
	if (subslaveIndex == (uint8_t) sensorAccIndex){
		sensor->accSensor->Calibrate();
		success = STATUS_FAIL;
	}

	if (subslaveIndex == (uint8_t) sensorGyroIndex){
		sensor->gyroSensor->Calibrate();
		success = STATUS_FAIL;
	}

//	if (subslaveIndex == sensorMagIndex){
//		sensor->magSensor->Calibrate();
//	}

	if (subslaveIndex == (uint8_t) sensorAll){
		sensor->accSensor->Calibrate();
		sensor->gyroSensor->Calibrate();
//		sensor->magSensor->Calibrate();
		success = STATUS_FAIL;
	}

	if (wasReady !=0 ) {
		sensor->Start();
	}

	return success;
}

uint8_t mcu_spi_deviceReady()
{
    return 1;
}
/*
uint8_t mcu_spi_store(void){
	return 0;
}
*/