#pragma once

#include "../../core/utils.hpp"

namespace dv::io::camera::imu {
/**
 * List of supported IMU models.
 */
enum class ImuModel : uint8_t {
	IMU_NONE                 = 0,
	IMU_INVENSENSE_6050_6150 = 1,
	IMU_INVENSENSE_6500_9250 = 2,
	IMU_BOSCH_BMI_160        = 3,
	IMU_BOSCH_BMI_270        = 4,
};

inline std::ostream &operator<<(std::ostream &os, const ImuModel &var) {
	switch (var) {
		case ImuModel::IMU_NONE:
			return os << "None";

		case ImuModel::IMU_INVENSENSE_6050_6150:
			return os << "InvenSense MPU 6050 / 6150";

		case ImuModel::IMU_INVENSENSE_6500_9250:
			return os << "InvenSense MPU 6500 / 9250";

		case ImuModel::IMU_BOSCH_BMI_160:
			return os << "Bosch BMI160";

		case ImuModel::IMU_BOSCH_BMI_270:
			return os << "Bosch BMI270";
	}

	return os;
}

/**
 * List of accelerometer scale settings for InvenSense IMUs.
 */
enum class InvensenseAccelRange : uint8_t {
	RANGE_2G  = 0,
	RANGE_4G  = 1,
	RANGE_8G  = 2,
	RANGE_16G = 3,
};

/**
 * List of gyroscope scale settings for InvenSense IMUs.
 */
enum class InvensenseGyroRange : uint8_t {
	RANGE_250DPS  = 0,
	RANGE_500DPS  = 1,
	RANGE_1000DPS = 2,
	RANGE_2000DPS = 3,
};

/**
 * List of accelerometer scale settings for Bosch IMU.
 */
enum class BoschBMI160AccelRange : uint8_t {
	RANGE_2G  = 3,
	RANGE_4G  = 5,
	RANGE_8G  = 8,
	RANGE_16G = 12,
};

/**
 * List of accelerometer data rate settings for Bosch IMU.
 */
enum class BoschBMI160AccelDataRate : uint8_t {
	RATE_12_5HZ = 5,
	RATE_25HZ   = 6,
	RATE_50HZ   = 7,
	RATE_100HZ  = 8,
	RATE_200HZ  = 9,
	RATE_400HZ  = 10,
	RATE_800HZ  = 11,
	RATE_1600HZ = 12,
};

/**
 * List of accelerometer filter settings for Bosch IMU.
 */
enum class BoschBMI160AccelFilter : uint8_t {
	FILTER_OSR4   = 0,
	FILTER_OSR2   = 1,
	FILTER_NORMAL = 2,
};

/**
 * List of gyroscope scale settings for Bosch IMU.
 */
enum class BoschBMI160GyroRange : uint8_t {
	RANGE_2000DPS = 0,
	RANGE_1000DPS = 1,
	RANGE_500DPS  = 2,
	RANGE_250DPS  = 3,
	RANGE_125DPS  = 4,
};

/**
 * List of gyroscope data rate settings for Bosch IMU.
 */
enum class BoschBMI160GyroDataRate : uint8_t {
	RATE_25HZ   = 6,
	RATE_50HZ   = 7,
	RATE_100HZ  = 8,
	RATE_200HZ  = 9,
	RATE_400HZ  = 10,
	RATE_800HZ  = 11,
	RATE_1600HZ = 12,
	RATE_3200HZ = 13,
};

/**
 * List of gyroscope filter settings for Bosch IMU.
 */
enum class BoschBMI160GyroFilter : uint8_t {
	FILTER_OSR4   = 0,
	FILTER_OSR2   = 1,
	FILTER_NORMAL = 2,
};

static float boschBMI160CalculateIMUAccelScale(const uint8_t imuAccelScale) {
	// Accelerometer scale is:
	// 0 - +- 2 g  - 16384 LSB/g
	// 1 - +- 4 g  - 8192 LSB/g
	// 2 - +- 8 g  - 4096 LSB/g
	// 3 - +- 16 g - 2048 LSB/g
	return (65536.0f / static_cast<float>(static_cast<uint32_t>(4 * (1 << imuAccelScale))));
}

static float boschBMI160CalculateIMUGyroScale(const uint8_t imuGyroScale) {
	// Gyroscope scale is:
	// 0 - +- 125 °/s  - 262.4 LSB/°/s
	// 1 - +- 250 °/s  - 131.2 LSB/°/s
	// 2 - +- 500 °/s  - 65.6 LSB/°/s
	// 3 - +- 1000 °/s - 32.8 LSB/°/s
	// 4 - +- 2000 °/s - 16.4 LSB/°/s
	return (65536.0f / static_cast<float>(static_cast<uint32_t>(250 * (1 << imuGyroScale))));
}

static int16_t safeFlip16(const int16_t value) {
	// Safe sign flip considering unequal positive/negative integer ranges.
	if (value == std::numeric_limits<int16_t>::min()) {
		return std::numeric_limits<int16_t>::max();
	}

	return static_cast<int16_t>(-value);
}
}; // namespace dv::io::camera::imu

// fmt compatibility for enum class printing.
template<>
struct fmt::formatter<dv::io::camera::imu::ImuModel> : fmt::ostream_formatter {};
