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

#ifndef PERIPHERALS_IMU_ICM_HPP_
#define PERIPHERALS_IMU_ICM_HPP_

#include <array>
#include <limits>

#include "peripherals_imu.hpp"
#include "peripherals_utility.hpp"

namespace Peripherals
{

template<typename T, typename HAL>
class ImuIcm : public IImu
{
public:
  using registers_handle = T;
  using hal = HAL;

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

	registers_handle registers;

	ImuIcm() = delete;
	ImuIcm(const registers_handle& registers)
	  : registers(registers)
	{ }

	void Setup() override
	{
	  registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b10000000);
	  hal::Delay(10);

	  registers.WriteRegister(_Registers::USER_CTRL & _Registers::WRITE_MASK, 0b00010000);
	  hal::Delay(10);

	  registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000000);
	  hal::Delay(10);

	  registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000001);
	  hal::Delay(10);

	  registers.WriteRegister(_Registers::CONFIG & _Registers::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
	  registers.WriteRegister(_Registers::ACCEL_CONFIG2 & _Registers::WRITE_MASK, 0x03);
	  hal::Delay(10);

	  // SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
	  registers.WriteRegister(_Registers::SMPLRT_DIV & _Registers::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
	  registers.WriteRegister(_Registers::INT_PIN_CFG & _Registers::WRITE_MASK, 0);
	  hal::Delay(10);

	  registers.WriteRegister(_Registers::INT_ENABLE & _Registers::WRITE_MASK, 1);
	  hal::Delay(10);
	}

	void Calibrate(const std::uint32_t samples) override
	{
	  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 SetGyroscopeScale(const GyroScale scale)
	{
	  const std::uint8_t current_config = registers.ReadRegister(_Registers::GYRO_CONFIG | _Registers::READ_MASK);
	  const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
	  registers.WriteRegister(_Registers::GYRO_CONFIG & _Registers::WRITE_MASK, new_config);

	  _scale_gyro = scale;
	}

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

	  _scale_accel = scale;
	}

	void ReadGyro(float* output) override
	{
	  uint8_t data[6] = { 0 };
	  registers.ReadRegisterMultibyte(_Registers::GYRO_XOUT_H | _Registers::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 ReadGyroRaw(std::int16_t* output) override
	{
	  uint8_t data[6] = { 0 };
	  registers.ReadRegisterMultibyte(_Registers::GYRO_XOUT_H | _Registers::READ_MASK, data, 6);

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

	void ReadAccelerometer(float* output) override
	{
	  uint8_t data[6] = { 0 };
	  registers.ReadRegisterMultibyte(_Registers::ACCEL_XOUT_H | _Registers::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 ReadAccelerometerRaw(std::int16_t* output) override
	{
	  uint8_t data[6] = { 0 };
	  registers.ReadRegisterMultibyte(_Registers::ACCEL_XOUT_H | _Registers::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 AccelerometerReadingToRaw(const float& fs) const
	{
	  return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
	}

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

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

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

private:
	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;

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

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

	struct _Registers
	{
	  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;
	};
};

template<typename T, typename HAL>
constexpr float ImuIcm<T, HAL>::_accel_fs_to_bit_constants[4];

template<typename T, typename HAL>
constexpr float ImuIcm<T, HAL>::_gyro_fs_to_bit_constants[4];

}

#endif /* PERIPHERALS_IMU_ICM_HPP_ */