#include "calibration.hpp"

#include "common/time.hpp"
#include "utils.hpp"

SensorCalibrator::SensorCalibrator(Sensor &sensor, SerialStream sout)
	: _sensor(sensor), _sout(sout)
{
}

bool SensorCalibrator::calibrate(unsigned int throttle_time)
{
	bool done = false;
	auto start_time = common::time_now();

	while (!done)
	{
		if (common::time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
		{
			return false;
		}

		delay(throttle_time);

		auto values = SensorCalibratorValues();

		for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++)
		{
			_updateValues(values);
		}

		_adjustValues(values);

		_sout << "Accel X: " << values.accel_x << "	"
			  << "Accel Y: " << values.accel_y << "	"
			  << "Accel Z: " << values.accel_z << "	"
			  << "Gyro X: " << values.gyro_x << "	"
			  << "Gyro Y: " << values.gyro_y << "	"
			  << "Gyro Z: " << values.gyro_z << endl;

		if (_isValuesInRange(values))
		{
			done = true;
		}

		_adjustCalibrationWithValues(values);
	}

	return true;
}

void SensorCalibrator::_updateValues(SensorCalibratorValues &values)
{
	_sensor.read();

	values.accel_x -= _sensor.getAccelX();
	values.accel_y -= _sensor.getAccelY();
	values.accel_z -= _sensor.getAccelZ();

	values.gyro_x -= _sensor.getGyroX();
	values.gyro_y -= _sensor.getGyroY();
	values.gyro_z -= _sensor.getGyroZ();
}

void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values)
{
	_sensor.accel_cal_x += values.accel_x;
	_sensor.accel_cal_y += values.accel_y;
	_sensor.accel_cal_z += values.accel_z;

	_sensor.gyro_cal_x += values.gyro_x;
	_sensor.gyro_cal_y += values.gyro_y;
	_sensor.gyro_cal_z += values.gyro_z;
}

void SensorCalibrator::_adjustValues(SensorCalibratorValues &values)
{
	values.accel_x *= SENSOR_VAL_ADJUST;
	values.accel_y *= SENSOR_VAL_ADJUST;
	values.accel_z *= SENSOR_VAL_ADJUST;

	values.gyro_x *= SENSOR_VAL_ADJUST;
	values.gyro_y *= SENSOR_VAL_ADJUST;
	values.gyro_z *= SENSOR_VAL_ADJUST;
}

bool SensorCalibrator::_isValuesInRange(const SensorCalibratorValues &values)
{
	return (values.accel_x < ACCEL_CAL_X_MAX && values.accel_x > ACCEL_CAL_X_MIN &&
			values.accel_y < ACCEL_CAL_Y_MAX && values.accel_y > ACCEL_CAL_Y_MIN &&
			values.accel_z < ACCEL_CAL_Z_MAX && values.accel_z > ACCEL_CAL_Z_MIN &&
			values.gyro_x < GYRO_CAL_X_MAX && values.gyro_x > GYRO_CAL_X_MIN &&
			values.gyro_y < GYRO_CAL_Y_MAX && values.gyro_y > GYRO_CAL_Y_MIN &&
			values.gyro_z < GYRO_CAL_Z_MAX && values.gyro_z > GYRO_CAL_Z_MIN);
}