#include "calibration.hpp"

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

SensorCalibrator::SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout)
	: _sensor(sensor), _sout(sout)
{
	_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();

		_sout << "Accel X: " << _accel_x << "	"
			  << "Accel Y: " << _accel_y << "	"
			  << "Accel Z: " << _accel_z << "	"
			  << "Gyro X: " << _gyro_x << "	"
			  << "Gyro Y: " << _gyro_y << "	"
			  << "Gyro Z: " << _gyro_z << endl;

		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;
}