#pragma once

#include "utils/serial.hpp"
#include "utils/time.hpp"

#include <Arduino.h>
#include <Wire.h>

constexpr uint8_t SENSOR_WAKEUP = 0x00U;

constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0;
constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0;

constexpr unsigned int ONE_EIGHTY = 180;

enum class SensorStatus
{
	OK = 0,
	THROTTLED = 1,
	ERR_READ = -1,
	ERR_WRITE = -2,
	ERR_NOT_CONNECTED = -3
};

/**
 * A GY521 gyro and accelerometer sensor.
 */
class Sensor
{
public:
	/**
	 * A GY521 gyro and accelerometer sensor.
	 *
	 * @param address The address of the sensor
	 * @param wire A Wire instance
	 * @param sout A serial output stream
	 * @param throttle_time A minumum time between sensor reads for the sensor to throttle
	 */
	Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time);

	/**
	 * Initializes communication with the sensor.
	 *
	 * @returns Whether or not the sensor can be communicated with.
	 */
	bool begin();

	/**
	 * Returns whether or not the sensor is connected.
	 */
	bool isConnected();

	/**
	 * Reads from the sensor.
	 *
	 * @returns Whether or not it succeeded.
	 */
	bool read();

	/**
	 * Sets the accelerometer sensitivity.
	 *
	 * @param sensitivity Accelerometer sensitivity. 0, 1, 2, 3 => 2g 4g 8g 16g
	 * @returns Whether or not it succeeded.
	 */
	bool setAccelSensitivity(uint8_t sensitivity);

	/**
	 * Sets the gyro sensitivity.
	 *
	 * @param sensitivity Gyro sensitivity.
	 * 	0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
	 * @returns Whether or not it succeeded.
	 */
	bool setGyroSensitivity(uint8_t sensitivity);

	/**
	 * Returns the current X axis acceleration in g:s (g-force).
	 */
	double getAccelX();

	/**
	 * Returns the current Y axis acceleration in g:s (g-force).
	 */
	double getAccelY();

	/**
	 * Returns the current Z axis acceleration in g:s (g-force).
	 */
	double getAccelZ();

	/**
	 * Returns the current X angle.
	 */
	double getAngleX();

	/**
	 * Returns the current Y angle.
	 */
	double getAngleY();

	/**
	 * Returns the current X axis in degrees/s.
	 */
	double getGyroX();

	/**
	 * Returns the current Y axis in degrees/s.
	 */
	double getGyroY();

	/**
	 * Returns the current Z axis in degrees/s.
	 */
	double getGyroZ();

	/**
	 * Returns the current pitch angle.
	 */
	double getPitch();

	/**
	 * Returns the current roll angle.
	 */
	double getRoll();

	/**
	 * Sets the value of a key in the sensor's register.
	 *
	 * @param reg The address of a registry key
	 * @param value A new value
	 * @returns Whether or not it succeeded.
	 */
	bool setRegister(uint8_t reg, uint8_t value);

	/**
	 * Returns the value of a key in the sensor's registry.
	 *
	 * @param reg The address of a registry key
	 */
	uint8_t getRegister(uint8_t reg);

	/**
	 * Returns the last sensor status.
	 */
	SensorStatus getStatus();

	// Accelerometer calibration values
	double accel_cal_x = 0.0F;
	double accel_cal_y = 0.0F;
	double accel_cal_z = 0.0F;

	// Gyroscope calibration values
	double gyro_cal_x = 0.0F;
	double gyro_cal_y = 0.0F;
	double gyro_cal_z = 0.0F;

private:
	TwoWire _wire;

	uint8_t _address;

	bool _throttle_enabled;
	unsigned int _throttle_time;

	Time _last_time;

	SensorStatus _status;

	uint8_t _accel_sensitivity = 0U;
	float _accel_to_g_force;

	double _accel_raw_x = 0;
	double _accel_raw_y = 0;
	double _accel_raw_z = 0;

	double _accel_x = 0;
	double _accel_y = 0;

	uint8_t _gyro_sensitivity = 0U;
	float _ang_rate_to_dps;

	double _gyro_raw_x = 0;
	double _gyro_raw_y = 0;
	double _gyro_raw_z = 0;

	double _gyro_x = 0;
	double _gyro_y = 0;
	double _gyro_z = 0;

	double _pitch = 0;
	double _roll = 0;
	double _yaw = 0;

	int16_t _readHighLow();

	SerialStream _sout;
};