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

#ifndef SKULLC_PERIPHERALS_IMU_ICM_HPP_
#define SKULLC_PERIPHERALS_IMU_ICM_HPP_

#include <array>
#include <limits>

#include "peripherals_imu.hpp"

#include "utility_logging.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);

    setAccelerometerScale(scale_accel_);

    setGyroscopeScale(scale_gyro_);

    // 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, 0);
    hal::delay(10);
  }

  void calibrate(const std::uint32_t samples) override
  {
	const auto prev_accel_scale = scale_accel_;
	const auto prev_gyro_scale = scale_gyro_;

	setAccelerometerScale(AccelerometerScale::G2);
	setGyroscopeScale(GyroScale::DPS_250);

    std::array<std::int32_t, 3> avg_gyro = {0, 0, 0};
    std::array<std::int32_t, 3> avg_accel = {0, 0, 0};
    std::array<std::int32_t, 3> bias_gyro = {0, 0, 0};
    std::array<std::int32_t, 3> bias_accel = {0, 0, 0};

    [[maybe_unused]] const std::uint16_t gyro_sensitivity = 131;   // = 131 LSB/degrees/sec
    const std::uint16_t accel_sensitivity = 16384; // = 16384 LSB/g

    for (std::uint32_t i = 0; i < samples; i++)
    {
      std::array<std::int16_t, 3> raw;

      readGyroRaw(raw.data());
      for (std::uint32_t j = 0; j < 3; j++)
        avg_gyro[j] += raw[j];

      readAccelerometerRaw(raw.data());
      for (std::uint32_t j = 0; j < 3; j++)
        avg_accel[j] += raw[j];

      hal::delay(2);
    }

    for (std::uint32_t i = 0; i < 3; i++)
    {
      bias_gyro[i] = avg_gyro[i] / std::int32_t(samples);
      bias_accel[i] = avg_accel[i] / std::int32_t(samples);
    }

    if (bias_accel[2] > 0)
    	bias_accel[2] -= accel_sensitivity;
    else
    	bias_accel[2] += accel_sensitivity;

    std::array<std::uint8_t, 6> push_data;

    // Construct the gyro biases for push to the hardware gyro bias registers,
    // which are reset to zero upon device startup.
    // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
    // format.
    push_data[0] = (-bias_gyro[0]/4 >> 8) & 0xFF;
    push_data[1] = (-bias_gyro[0]/4) & 0xFF;
    push_data[2] = (-bias_gyro[1]/4 >> 8) & 0xFF;
    push_data[3] = (-bias_gyro[1]/4) & 0xFF;
    push_data[4] = (-bias_gyro[2]/4 >> 8) & 0xFF;
    push_data[5] = (-bias_gyro[2]/4) & 0xFF;

    registers.writeRegisterMultibyte(Registers_::XG_OFFSET_H & Registers_::WRITE_MASK,
    		push_data.data(), push_data.size());

    std::array<std::int32_t, 3> factory_values = {0, 0, 0};
//    registers.readRegisterMultibyte(Registers_::XA_OFFSET_H | Registers_::READ_MASK,
//    		push_data.data(), push_data.size());
//    factory_values[0] = std::int32_t((std::int16_t(push_data[0]) << 8) | push_data[1]);
//    factory_values[1] = std::int32_t((std::int16_t(push_data[2]) << 8) | push_data[3]);
//    factory_values[2] = std::int32_t((std::int16_t(push_data[4]) << 8) | push_data[5]);

    // Construct total accelerometer bias, including calculated average
    // accelerometer bias from above
    // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
    // (16 g full scale)
    bias_accel[0] = factory_values[0] - (bias_accel[0] / 8);
    bias_accel[1] = factory_values[1] - (bias_accel[1] / 8);
    bias_accel[1] = factory_values[1] - (bias_accel[1] / 8);

    push_data[0] = ((bias_accel[0] >> 8) & 0xFF);
    push_data[1] = ((bias_accel[0]) & 0xFF) | (factory_values[0] & 0x1);
    push_data[2] = ((bias_accel[1] >> 8) & 0xFF);
    push_data[3] = ((bias_accel[1]) & 0xFF) | (factory_values[1] & 0x1);
    push_data[4] = ((bias_accel[2] >> 8) & 0xFF);
    push_data[5] = ((bias_accel[2]) & 0xFF) | (factory_values[2] & 0x1);

    registers.writeRegisterMultibyte(Registers_::XA_OFFSET_H & Registers_::WRITE_MASK,
			push_data.data(), push_data.size());

   // __asm__("BKPT");

	setAccelerometerScale(prev_accel_scale);
	setGyroscopeScale(prev_gyro_scale);

	SKULLC_LOG_DEBUG("ICM: Accelerometer bias: %d, %d, %d; accel total: %d, %d, %d.",
			bias_accel[0], bias_accel[1], bias_accel[2],
			avg_accel[0], avg_accel[1], avg_accel[2]);
	// TODO: remove manual bias calculations. These are now done in hardware.
  }

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

    output[0] = gyroRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
    output[1] = gyroRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
    output[2] = gyroRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
  }

  void readGyroRaw(std::int16_t* output) override
  {
    uint8_t data[6] = {0};
    registers.readRegisterMultibyte(
            Registers_::GYRO_XOUT_H | Registers_::READ_MASK, data, 6);

    output[0] = std::int16_t((data[0] << 8) | data[1]);
    output[1] = std::int16_t((data[2] << 8) | data[3]);
    output[2] = std::int16_t((data[4] << 8) | data[5]);
  }

  void readAccelerometer(float* output) override
  {
    uint8_t data[6] = {0};
    registers.readRegisterMultibyte(
            Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);

    output[0] = accelerometerRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
    output[1] = accelerometerRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
    output[2] = accelerometerRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
  }

  void readAccelerometerRaw(std::int16_t* output) override
  {
    uint8_t data[6] = {0};
    registers.readRegisterMultibyte(
            Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);


    output[0] = std::int16_t((data[0] << 8) | data[1]);
    output[1] = std::int16_t((data[2] << 8) | data[3]);
    output[2] = std::int16_t((data[4] << 8) | data[5]);
  }

  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 std::size_t axis) const
  {
    return (float(bit) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f;
  }

  float gyroRawToReading(const std::int16_t bit, const std::size_t axis) const
  {
    return float(bit) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
  }

  std::uint8_t readProductId()
  {
    return registers.readRegister(Registers_::WHO_AM_I | Registers_::READ_MASK);
  }

private:
  GyroScale scale_gyro_ = GyroScale::DPS_2000;
  AccelerometerScale scale_accel_ = AccelerometerScale::G16;

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

}// namespace Peripherals

#endif /* SKULLC_PERIPHERALS_IMU_ICM_HPP_ */