#pragma once

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

// Calibration precision
constexpr double ACCEL_CAL_X_MAX = 0.003;
constexpr double ACCEL_CAL_X_MIN = -0.003;

constexpr double ACCEL_CAL_Y_MAX = 0.003;
constexpr double ACCEL_CAL_Y_MIN = -0.003;

constexpr double ACCEL_CAL_Z_MAX = 0.003;
constexpr double ACCEL_CAL_Z_MIN = -0.003;

constexpr double GYRO_CAL_X_MAX = 0.003;
constexpr double GYRO_CAL_X_MIN = -0.003;

constexpr double GYRO_CAL_Y_MAX = 0.05;
constexpr double GYRO_CAL_Y_MIN = -0.05;

constexpr double GYRO_CAL_Z_MAX = 0.04;
constexpr double GYRO_CAL_Z_MIN = -0.04;

constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // milliseconds

constexpr uint32_t SENSOR_READ_CNT = 20;
constexpr double SENSOR_VAL_ADJUST = 0.05;

class SensorCalibratorValues
{
public:
	double accel_x = 0;
	double accel_y = 0;
	double accel_z = 0;

	double gyro_x = 0;
	double gyro_y = 0;
	double gyro_z = 0;
};

/**
 * Sensor calibrator.
 */
class SensorCalibrator
{
public:
	/**
	 * Sensor calibrator.
	 *
	 * @param sensor A sensor to calibrate
	 * @param sout A Serial output stream
	 */
	SensorCalibrator(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 _updateValues(SensorCalibratorValues &values);
	void _adjustCalibrationWithValues(const SensorCalibratorValues &values);

	static void _adjustValues(SensorCalibratorValues &values);
	static bool _isValuesInRange(const SensorCalibratorValues &values);

	Sensor &_sensor;

	SerialStream _sout;
};