/*
 * peripherals_imu_icm.hpp
 *
 *  Created on: Mar 5, 2021
 *      Author: erki
 */

#ifndef PERIPHERALS_IMU_ICM_HPP_
#define PERIPHERALS_IMU_ICM_HPP_

#include <array>

#include "spi.h"

#include "peripherals_imu.hpp"
#include "peripherals_spi.hpp"

namespace Peripherals
{

class ImuIcm : public IImu
{
public:
	enum class GyroScale : std::uint32_t
	{
		DPS_250 = 0,
		DPS_500,
		DPS_1000,
		DPS_2000
	};

	enum class AccelerometerScale : std::uint32_t
	{
		G2 = 0,
		G4,
		G8,
		G16
	};

	ImuIcm() = delete;
	ImuIcm(SPI_HandleTypeDef* spi, const IO& cs);
	ImuIcm(const Spi& spi);

#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U

	void SetSpiRxCallback(pSPI_CallbackTypeDef cb);
	void SetSpiTxCallback(pSPI_CallbackTypeDef cb);

#endif

	void Setup() override;
	void Calibrate(const std::uint32_t samples) override;

	void SetGyroscopeScale(const GyroScale scale);
	void SetAccelerometerScale(const AccelerometerScale scale);

	void ReadGyro(float* output) override;
	void ReadGyroRaw(std::int16_t* output) override;

	void ReadAccelerometer(float* output) override;
	void ReadAccelerometerRaw(std::int16_t* output) override;

	std::int16_t AccelerometerReadingToRaw(const float& fs) const;
	std::int16_t GyroReadingToRaw(const float& fs) const;
	float AccelerometerRawToReading(const std::int16_t bit) const;
	float GyroRawToReading(const std::int16_t bit) const;

private:
	Spi _spi;

	GyroScale _scale_gyro = GyroScale::DPS_2000;
	AccelerometerScale _scale_accel = AccelerometerScale::G16;

	std::array<std::int16_t, 3> _bias_gyro;
	std::array<std::int16_t, 3> _bias_accel;
};

}

#endif /* PERIPHERALS_IMU_ICM_HPP_ */