#ifndef SENSOR_HPP
#define SENSOR_HPP

#include "Arduino.h"
#include "Wire.h"
#include "time_utils.hpp"

#define SENSOR_WAKEUP 0x00

#define RAD_TO_DEGREES (180.0 / PI)
#define RAW_TO_DPS_FACTOR (1.0 / 131.0)
#define RAW_TO_G_FACTOR (1.0 / 16384.0)

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 throttle_time A minumum time between sensor reads for the sensor to throttle
	 */
	Sensor(uint8_t address, TwoWire *wire, 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();

	/**
	 * Resets the sensor.
	 */
	void reset();

	/**
	 * 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).
	 */
	float getAccelX();

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

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

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

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

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

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

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

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

	/**
	 * Returns the current roll angle.
	 */
	float 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
	float accel_cal_x;
	float accel_cal_y;
	float accel_cal_z;

	// Gyroscope calibration values
	float gyro_cal_x;
	float gyro_cal_y;
	float gyro_cal_z;

private:
	uint8_t _address;

	bool _throttle_enabled;
	unsigned int _throttle_time;

	Time _last_time;

	SensorStatus _status;

	uint8_t _accel_sensitivity;
	float _accel_to_g_force;

	float _accel_raw_x, _accel_raw_y, _accel_raw_z;
	float _accel_x, _accel_y, _accel_z;

	uint8_t _gyro_sensitivity;
	float _ang_rate_to_dps;

	float _gyro_raw_x, _gyro_raw_y, _gyro_raw_z;
	float _gyro_x, _gyro_y, _gyro_z;

	float _pitch, _roll, _yaw;

	int16_t _readTwoBytes();

	TwoWire *_wire;
};

#endif