#pragma once

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

// Calibration precision
constexpr float ACCEL_CAL_X_MAX = 0.006;
constexpr float ACCEL_CAL_X_MIN = -0.006;

constexpr float ACCEL_CAL_Y_MAX = 0.006;
constexpr float ACCEL_CAL_Y_MIN = -0.006;

constexpr float ACCEL_CAL_Z_MAX = 0.006;
constexpr float ACCEL_CAL_Z_MIN = -0.006;

constexpr float GYRO_CAL_X_MAX = 0.06;
constexpr float GYRO_CAL_X_MIN = -0.06;

constexpr float GYRO_CAL_Y_MAX = 0.06;
constexpr float GYRO_CAL_Y_MIN = -0.06;

constexpr float GYRO_CAL_Z_MAX = 0.06;
constexpr float GYRO_CAL_Z_MIN = -0.06;

constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds

constexpr unsigned int SENSOR_READ_CNT = 20;
constexpr float 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(UniquePtr<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() const;

	void _adjustCalibration();

	UniquePtr<Sensor> &_sensor;

	SerialStream _sout;

	double _accel_x = 0;
	double _accel_y = 0;
	double _accel_z = 0;

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