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

#include "peripherals_imu_icm.hpp"

#include <limits>

#include "peripherals_utility.hpp"

namespace
{

namespace _REGS
{

static constexpr std::uint32_t ICM20689_ID =    0x98;

static constexpr std::uint8_t READ_MASK =   0x80;
static constexpr std::uint8_t WRITE_MASK =  0x7F;

static constexpr std::uint8_t SMPLRT_DIV =  0x19;
static constexpr std::uint8_t CONFIG =      0x1A;
static constexpr std::uint8_t GYRO_CONFIG = 0x1B;
static constexpr std::uint8_t ACCEL_CONFIG =0x1C;
static constexpr std::uint8_t ACCEL_CONFIG2=0x1D;

static constexpr std::uint8_t INT_PIN_CFG = 0x37;
static constexpr std::uint8_t INT_ENABLE =  0x38;
static constexpr std::uint8_t INT_STATUS =  0x3A;

static constexpr std::uint8_t GYRO_XOUT_H = 0x43;
static constexpr std::uint8_t GYRO_XOUT_L = 0x44;
static constexpr std::uint8_t GYRO_YOUT_H = 0x45;
static constexpr std::uint8_t GYRO_YOUT_L = 0x46;
static constexpr std::uint8_t GYRO_ZOUT_H = 0x47;
static constexpr std::uint8_t GYRO_ZOUT_L = 0x48;

static constexpr std::uint8_t ACCEL_XOUT_H =0x3B;
static constexpr std::uint8_t ACCEL_XOUT_L =0x3C;
static constexpr std::uint8_t ACCEL_YOUT_H =0x3D;
static constexpr std::uint8_t ACCEL_YOUT_L =0x3E;
static constexpr std::uint8_t ACCEL_ZOUT_H =0x3F;
static constexpr std::uint8_t ACCEL_ZOUT_L =0x40;

static constexpr std::uint8_t USER_CTRL	=   0x6A;
static constexpr std::uint8_t PWR_MGMT_1 =  0x6B;
static constexpr std::uint8_t PWR_MGMT_2 =  0x6C;
static constexpr std::uint8_t WHO_AM_I =    0x75;

static constexpr std::uint8_t XG_OFFSET_H = 0x13;
static constexpr std::uint8_t XG_OFFSET_L = 0x14;
static constexpr std::uint8_t YG_OFFSET_H = 0x15;
static constexpr std::uint8_t YG_OFFSET_L = 0x16;
static constexpr std::uint8_t ZG_OFFSET_H = 0x17;
static constexpr std::uint8_t ZG_OFFSET_L = 0x18;

static constexpr std::uint8_t XA_OFFSET_H = 0x77;
static constexpr std::uint8_t XA_OFFSET_L = 0x78;
static constexpr std::uint8_t YA_OFFSET_H = 0x7A;
static constexpr std::uint8_t YA_OFFSET_L = 0x7B;
static constexpr std::uint8_t ZA_OFFSET_H = 0x7D;
static constexpr std::uint8_t ZA_OFFSET_L = 0x7E;


}

static const float _accel_fs_to_bit_constants[] = {
	(2.0f / 32768.0f),
	(4.0f / 32768.0f),
	(8.0f / 32768.0f),
	(16.0f / 32768.0f)
};

static const float _gyro_fs_to_bit_constants[] = {
	(250.0f / 32768.0f),
	(500.0f / 32768.0f),
	(1000.0f / 32768.0f),
	(2000.0f / 32768.0f)
};

}

namespace Peripherals
{

ImuIcm::ImuIcm(SPI_HandleTypeDef* spi, const IO& cs)
  : _spi(spi, cs)
{ }

ImuIcm::ImuIcm(const Spi& spi)
  : _spi(spi)
{ }

#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U

void ImuIcm::SetSpiRxCallback(pSPI_CallbackTypeDef cb)
{
	_spi.hspi->RxCpltCallback = cb;
}

void ImuIcm::SetSpiTxCallback(pSPI_CallbackTypeDef cb)
{
	_spi.hspi->TxCpltCallback = cb;
}

#endif

void ImuIcm::Setup()
{
	_spi.cs.Set(true);
	HAL_Delay(100);

	_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b10000000);
	HAL_Delay(10);

	_spi.WriteRegister(_REGS::USER_CTRL & _REGS::WRITE_MASK, 0b00010000);
	HAL_Delay(10);

	_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000000);
	HAL_Delay(10);

	_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000001);
	HAL_Delay(20);

	_spi.WriteRegister(_REGS::CONFIG & _REGS::WRITE_MASK, 0x03); // DLPF_CFG = 3, gyro filter = 41/59.0, gyro rate = 1KHz, temp filter = 42
	HAL_Delay(10);

	SetGyroscopeScale(_scale_gyro);

	SetAccelerometerScale(_scale_accel);

	// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
	_spi.WriteRegister(_REGS::ACCEL_CONFIG2 & _REGS::WRITE_MASK, 0x03);
	HAL_Delay(10);

	// SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
	_spi.WriteRegister(_REGS::SMPLRT_DIV & _REGS::WRITE_MASK, 0);
	HAL_Delay(10);

	// Enable interrupt

	// The logic level for INT/DRDY pin is active high.
	// INT/DRDY pin is configured as push-pull.
	// INT/DRDY pin indicates interrupt pulse's width is 50us.
	// Interrupt status is cleared only by reading INT_STATUS register
	_spi.WriteRegister(_REGS::INT_PIN_CFG & _REGS::WRITE_MASK, 0);
	HAL_Delay(10);

	_spi.WriteRegister(_REGS::INT_ENABLE & _REGS::WRITE_MASK, 1);
	HAL_Delay(10);
}

void ImuIcm::Calibrate(const std::uint32_t samples)
{
	std::array<std::int32_t, 3> avg_gyro;
	std::array<std::int32_t, 3> avg_accel;

	for (std::uint32_t i = 0; i < samples; i++)
	{
		std::array<std::int16_t, 3> raw;
		auto add_to_avg = [&raw](std::array<std::int32_t, 3>& out)
		{
			for (std::uint32_t j = 0; j < 3; j++)
				out[j] += raw[j];
		};

		ReadGyroRaw(raw.data());
		add_to_avg(avg_gyro);

		ReadAccelerometerRaw(raw.data());
		add_to_avg(avg_accel);
	}

	for (std::uint32_t i = 0; i < 3; i++)
	{
		const std::int32_t max = std::numeric_limits<std::int16_t>::max();
		const std::int32_t min = std::numeric_limits<std::int16_t>::min();

		_bias_gyro[i] = Clamp(avg_gyro[i], max, min);
		_bias_accel[i] = Clamp(avg_accel[i], max, min);
	}

	_bias_accel[2] -= AccelerometerReadingToRaw(1);
}

void ImuIcm::SetGyroscopeScale(const ImuIcm::GyroScale scale)
{
	const std::uint8_t current_config = _spi.ReadRegister(_REGS::GYRO_CONFIG | _REGS::READ_MASK);
	const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
	_spi.WriteRegister(_REGS::GYRO_CONFIG & _REGS::WRITE_MASK, new_config);

	_scale_gyro = scale;
}

void ImuIcm::SetAccelerometerScale(const ImuIcm::AccelerometerScale scale)
{
	const std::uint8_t current_config = _spi.ReadRegister(_REGS::ACCEL_CONFIG | _REGS::READ_MASK);
	const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
	_spi.WriteRegister(_REGS::ACCEL_CONFIG & _REGS::WRITE_MASK, new_config);

	_scale_accel = scale;
}

void ImuIcm::ReadGyro(float* output)
{
	uint8_t data[6] = { 0 };
	_spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::READ_MASK, data, 6);

	for (std::uint32_t i = 0; i < 3; i++)
	{
		const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
		output[i] = GyroRawToReading(bit);
	}
}

void ImuIcm::ReadGyroRaw(std::int16_t* output)
{
	uint8_t data[6] = { 0 };
	_spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::READ_MASK, data, 6);

	for (std::uint32_t i = 0; i < 3; i++)
	{
		output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
	}
}

void ImuIcm::ReadAccelerometer(float* output)
{
	uint8_t data[6] = { 0 };
	_spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::READ_MASK, data, 6);

	for (std::uint32_t i = 0; i < 3; i++)
	{
		const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
		output[i] = AccelerometerRawToReading(bit);
	}
}

void ImuIcm::ReadAccelerometerRaw(std::int16_t* output)
{
	uint8_t data[6] = { 0 };
	_spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::READ_MASK, data, 6);

	for (std::uint32_t i = 0; i < 3; i++)
	{
		output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
	}
}

std::int16_t ImuIcm::AccelerometerReadingToRaw(const float& fs) const
{
	return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
}

std::int16_t ImuIcm::GyroReadingToRaw(const float& fs) const
{
	return fs / _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
}

float ImuIcm::AccelerometerRawToReading(const std::int16_t bit) const
{
	return float(bit) * _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
}

float ImuIcm::GyroRawToReading(const std::int16_t bit) const
{
	return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
}

}