#ifndef CALIBRATION_HPP
#define CALIBRATION_HPP

#include "sensor/sensor.hpp"
#include "utils/memory.hpp"
#include "utils/serial.hpp"

// Calibration precision
#define ACCEL_CAL_X_MAX 0.006
#define ACCEL_CAL_X_MIN -0.006

#define ACCEL_CAL_Y_MAX 0.006
#define ACCEL_CAL_Y_MIN -0.006

#define ACCEL_CAL_Z_MAX 0.006
#define ACCEL_CAL_Z_MIN -0.006

#define GYRO_CAL_X_MAX 0.06
#define GYRO_CAL_X_MIN -0.06

#define GYRO_CAL_Y_MAX 0.06
#define GYRO_CAL_Y_MIN -0.06

#define GYRO_CAL_Z_MAX 0.06
#define GYRO_CAL_Z_MIN -0.06

#define CALIBRATION_TIMEOUT 120000 // Milliseconds

#define SENSOR_READ_CNT 20
#define SENSOR_VAL_ADJUST 0.05

/**
 * Sensor calibrator.
 */
class SensorCalibrator
{
public:
	/**
	 * Sensor calibrator.
	 *
	 * @param sensor A sensor to calibrate
	 * @param sout A Serial output stream
	 */
	SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout);

	/**
	 * Calibrates the sensor.
	 *
	 * @param throttle_time The sensor's throttle time
	 * @returns Whether or not the calibration succeeded. Will return false on
	 * timeout.
	 */
	bool calibrate(unsigned int throttle_time);

private:
	void _resetValues();
	void _updateValues();
	void _adjustValues();
	bool _isValuesInRange();

	void _adjustCalibration();

	unique_ptr<Sensor> &_sensor;

	SerialStream _sout;

	float _accel_x;
	float _accel_y;
	float _accel_z;

	float _gyro_x;
	float _gyro_y;
	float _gyro_z;
};

#endif