/*
 * 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
};


axisesI *sensor_dataI;  // int16 values

Icm20948 *sensor;
IcmSpiManager *manager;

nBusAppInterface_t *getImuDriver(){
	return &mcu_spi_driver;
}

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();
}

void mcu_spi_start(){
	sensor->Start();
}

void mcu_spi_stop(){
	sensor->Stop();
}

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

nBus_sensorType_t mcu_spi_getType(uint8_t sensor_index){

	if (sensor_index > mcu_spi_getSensorCount())
		return TYPE_UNKNOWN;

	if (sensor_index == 1)
		return TYPE_ACCELEROMETER;

	if (sensor_index == 2)
		return TYPE_GYROSCOPE;

	return TYPE_UNKNOWN;
}


uint8_t mcu_spi_getSensorCount(){
	return 2;
}


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;
}

uint8_t mcu_spi_setData(uint8_t *data){
	return 0;
}

uint8_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param){
	return 0x00;
}

uint8_t mcu_spi_hasParam(uint8_t sensor_index, nBus_param_t param){
	return 0;
}

nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, uint8_t value){
	return PARAM_NONE;
}