#ifndef SENSOR_HPP
#define SENSOR_HPP

#include "Arduino.h"
#include "Wire.h"

#define DEFAULT_THROTTLE_TIME 10 // milliseconds

// Status codes
#define SENSOR_OK 0
#define SENSOR_THROTTLED 1
#define SENSOR_ERR_READ -1
#define SENSOR_ERR_WRITE -2
#define SENSOR_ERR_NOT_CONNECTED -3

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

// Automatic calibration precision
#define ACCEL_CAL_X_MAX 0.002
#define ACCEL_CAL_X_MIN -0.002

#define ACCEL_CAL_Y_MAX 0.002
#define ACCEL_CAL_Y_MIN -0.002

#define ACCEL_CAL_Z_MAX 0.002
#define ACCEL_CAL_Z_MIN -0.002

#define GYRO_CAL_X_MAX 0.2
#define GYRO_CAL_X_MIN -0.2

#define GYRO_CAL_Y_MAX 0.02
#define GYRO_CAL_Y_MIN -0.02

#define GYRO_CAL_Z_MAX 0.08
#define GYRO_CAL_Z_MIN -0.08

#define CALIBRATION_TIMEOUT 120000 // Milliseconds

class Sensor
{
public:
	Sensor(uint8_t address, TwoWire *wire);

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

	/**
	 * Returns whether or not throttling is enabled.
	 */
	bool getThrottleEnabled();

	/**
	 * Sets whether or not throttling is enabled.
	 * 
	 * @param throttle Throttling enable/disable
	 */
	void setThrottleEnabled(bool throttle);

	/**
	 * Returns the throttle time.
	 */
	uint16_t getThrottleTime();

	/**
	 * Sets the throttle time.
	 * 
	 * @param time_ms Time in milliseconds to throttle
	 */
	void setThrottleTime(uint16_t time_ms);

	/**
	 * Reads from the sensor.
	 * 
	 * @returns The sensor status.
	 */
	int16_t read();

	/**
	 * Returns the accelerometer sensitivity.
	 * 
	 * 0,1,2,3 ==> 2g 4g 8g 16g
	 */
	uint8_t getAccelSensitivity();

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

	/**
	 * Returns the gyro sensitivity.
	 * 
	 * 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
	 */
	uint8_t getGyroSensitivity();

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

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

	/**
	 * Returns the last time the sensor was read.
	 */
	uint32_t lastTime();

	/**
	 * 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 The sensor status
	 */
	uint8_t 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.
	 */
	int16_t getStatus();

	/**
	 * Automatically calibrates the sensor.
	 *
	 * @returns Whether or not the calibration succeeded.
	 * 	Will return false if it times out.
	 */
	bool autoCalibrate();

	bool is_calibrated;

	// 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;
	uint16_t _throttleTime;

	uint32_t _lastTime;
	uint32_t _lastMicros;

	int16_t _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;

	float _temperature;

	int16_t _readTwoBytes();

	TwoWire *_wire;
};

#endif