#include "calibration.hpp"
#include "time_utils.hpp"

SensorCalibrator::SensorCalibrator(Sensor *sensor)
{
	_sensor = sensor;
	_resetValues();
}

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

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

		delay(throttle_time);

		_resetValues();

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

		_adjustValues();

		Serial.print("Accel X: ");
		Serial.print(_accel_x);
		Serial.print("	Accel Y: ");
		Serial.print(_accel_y);
		Serial.print("	Accel Z: ");
		Serial.print(_accel_z);

		Serial.print("	Gyro X: ");
		Serial.print(_gyro_x);
		Serial.print("	Gyro Y: ");
		Serial.print(_gyro_y);
		Serial.print("	Gyro Z: ");
		Serial.println(_gyro_z);

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

		_adjustCalibration();
	}

	return true;
}

void SensorCalibrator::_resetValues()
{
	_accel_x = 0;
	_accel_y = 0;
	_accel_z = 0;

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

void SensorCalibrator::_updateValues()
{
	_sensor->read();

	_accel_x -= _sensor->getAccelX();
	_accel_y -= _sensor->getAccelY();
	_accel_z -= _sensor->getAccelZ();

	_gyro_x -= _sensor->getGyroX();
	_gyro_y -= _sensor->getGyroY();
	_gyro_z -= _sensor->getGyroZ();
}

void SensorCalibrator::_adjustValues()
{
	_accel_x *= SENSOR_VAL_ADJUST;
	_accel_y *= SENSOR_VAL_ADJUST;
	_accel_z *= SENSOR_VAL_ADJUST;

	_gyro_x *= SENSOR_VAL_ADJUST;
	_gyro_y *= SENSOR_VAL_ADJUST;
	_gyro_z *= SENSOR_VAL_ADJUST;
}

bool SensorCalibrator::_isValuesInRange()
{
	return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN &&
			_accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN &&
			_accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN &&
			_gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN &&
			_gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN &&
			_gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN);
}

void SensorCalibrator::_adjustCalibration()
{
	_sensor->accel_cal_x += _accel_x;
	_sensor->accel_cal_y += _accel_y;
	_sensor->accel_cal_z += _accel_z;

	_sensor->gyro_cal_x += _gyro_x;
	_sensor->gyro_cal_y += _gyro_y;
	_sensor->gyro_cal_z += _gyro_z;
}