#include "sensor.hpp"

#include "sensor/registers.hpp"

Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout,
			   unsigned int throttle_time)
	: _wire(wire),
	  _sout(sout),
	  _address(address),
	  _throttle_enabled(true),
	  _throttle_time(throttle_time),
	  _last_time(0),
	  _status(SensorStatus::OK),
	  _accel_to_g_force(RAW_TO_G_FACTOR),
	  _ang_rate_to_dps(RAW_TO_DPS_FACTOR)
{
}

bool Sensor::begin() noexcept
{
	_wire.begin();

	if (isConnected())
	{
		_wire.beginTransmission(_address);
		_wire.write(SensorRegisters::PWR_MGMT_1);
		_wire.write(SENSOR_WAKEUP);

		return (_wire.endTransmission() == 0);
	}

	return false;
}

bool Sensor::isConnected() noexcept
{
	_wire.beginTransmission(_address);
	return (_wire.endTransmission() == 0);
}

bool Sensor::read() noexcept
{
	auto now = time_now();

	if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time)
	{
		_status = SensorStatus::THROTTLED;
		return false;
	}

	_wire.beginTransmission(_address);
	_wire.write(SensorRegisters::ACCEL_XOUT_H);

	if (_wire.endTransmission() != 0U)
	{
		_status = SensorStatus::ERR_WRITE;
		return false;
	}

	uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y);

	const uint8_t self_test_success = 14U;

	if (response_length != self_test_success)
	{
		_status = SensorStatus::ERR_READ;
		return false;
	}

	// Accelerometer
	_accel_raw_x = _readHighLow();
	_accel_raw_y = _readHighLow();
	_accel_raw_z = _readHighLow();

	_sout << "\nAccel raw x: " << _accel_raw_x << "\n"
		  << "Accel raw y: " << _accel_raw_y << "\n"
		  << "Accel raw z: " << _accel_raw_z << "\n"
		  << endl;

	// Gyroscope
	_gyro_raw_x = _readHighLow();
	_gyro_raw_y = _readHighLow();
	_gyro_raw_z = _readHighLow();

	_sout << "\nGyro raw x: " << _gyro_raw_x << "\n"
		  << "Gyro raw y: " << _gyro_raw_y << "\n"
		  << "Gyro raw z: " << _gyro_raw_z << "\n"
		  << endl;

	// Duration interval
	now.update();
	auto duration = now.diff(_last_time).secs();
	_last_time = now;

	// Convert raw acceleration to g:s (g-force)
	_accel_raw_x *= _accel_to_g_force;
	_accel_raw_y *= _accel_to_g_force;
	_accel_raw_z *= _accel_to_g_force;

	_sout << "\nAccel raw x g:s: " << _accel_raw_x << "\n"
		  << "Accel raw y g:s: " << _accel_raw_y << "\n"
		  << "Accel raw z g:s: " << _accel_raw_z << "\n"
		  << endl;

	// Error correct raw acceleration
	_accel_raw_x += accel_cal_x;
	_accel_raw_y += accel_cal_y;
	_accel_raw_z += accel_cal_z;

	_sout << "\nAccel raw x correct: " << _accel_raw_x << "\n"
		  << "Accel raw y correct: " << _accel_raw_y << "\n"
		  << "Accel raw z correct: " << _accel_raw_z << "\n"
		  << endl;

	// Prepare for Pitch Roll Yaw
	auto accel_y_pow_two = pow(_accel_raw_y, 2);
	auto accel_z_pow_two = pow(_accel_raw_z, 2);

	_accel_x = atan2(_accel_raw_y, _accel_raw_z) * ONE_EIGHTY / PI;

	_accel_y =
		atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI;

	_sout << "\nAccel x prepared: " << _accel_x << "\n"
		  << "Accel y prepared: " << _accel_y << "\n"
		  << endl;

	// Convert raw Gyro to degrees/s
	_gyro_raw_x *= _ang_rate_to_dps;
	_gyro_raw_y *= _ang_rate_to_dps;
	_gyro_raw_z *= _ang_rate_to_dps;

	_sout << "\nGyro raw x dps: " << _gyro_raw_x << "\n"
		  << "Gyro raw y dps: " << _gyro_raw_y << "\n"
		  << "Gyro raw z dps: " << _gyro_raw_z << "\n"
		  << endl;

	// Error correct raw gyro measurements.
	_gyro_raw_x += gyro_cal_x;
	_gyro_raw_y += gyro_cal_y;
	_gyro_raw_z += gyro_cal_z;

	_sout << "\nGyro raw x correct: " << _gyro_raw_x << "\n"
		  << "Gyro raw y correct: " << _gyro_raw_y << "\n"
		  << "Gyro raw z correct: " << _gyro_raw_z << "\n"
		  << endl;

	_gyro_x += _gyro_raw_x * duration;
	_gyro_y += _gyro_raw_y * duration;
	_gyro_z += _gyro_raw_z * duration;

	_sout << "\nGyro x w/o time: " << _gyro_x << "\n"
		  << "Gyro y w/o time: " << _gyro_y << "\n"
		  << "Gyro z w/o time: " << _gyro_z << "\n"
		  << endl;

	_pitch = _gyro_y + _accel_y;
	_roll = _gyro_x + _accel_x;

	_yaw = _gyro_z;

	return true;
}

bool Sensor::setAccelSensitivity(uint8_t sensitivity) noexcept
{
	if (sensitivity > 3)
	{
		sensitivity = 3;
	}

	_accel_sensitivity = sensitivity;

	auto accel_config = static_cast<int>(getRegister(SensorRegisters::ACCEL_CONFIG));

	if (_status != SensorStatus::OK)
	{
		return false;
	}

	// No need to write same value
	if (((accel_config >> 3) & 3) != _accel_sensitivity)
	{
		const uint8_t magic = 0xE7U;

		accel_config &= magic;
		accel_config |= (_accel_sensitivity << 3U);

		if (!setRegister(SensorRegisters::ACCEL_CONFIG,
						 static_cast<uint8_t>(accel_config)))
		{
			return false;
		}
	}

	// Calculate conversion factor.
	_accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR;

	return true;
}

bool Sensor::setGyroSensitivity(uint8_t sensitivity) noexcept
{
	if (sensitivity > 3)
	{
		sensitivity = 3;
	}

	_gyro_sensitivity = sensitivity;

	auto gyro_config = static_cast<int>(getRegister(SensorRegisters::GYRO_CONFIG));

	if (_status != SensorStatus::OK)
	{
		return false;
	}

	// No need to write same value
	if (((gyro_config >> 3) & 3) != _gyro_sensitivity)
	{
		const uint8_t magic = 0xE7U;

		gyro_config &= magic;
		gyro_config |= (_gyro_sensitivity << 3U);

		if (!setRegister(SensorRegisters::GYRO_CONFIG, static_cast<uint8_t>(gyro_config)))
		{
			return false;
		}
	}

	_ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR;

	return true;
}

double Sensor::getAccelX() const noexcept
{
	return _accel_raw_x;
}

double Sensor::getAccelY() const noexcept
{
	return _accel_raw_y;
}

double Sensor::getAccelZ() const noexcept
{
	return _accel_raw_z;
}

double Sensor::getAngleX() const noexcept
{
	return _accel_x;
}

double Sensor::getAngleY() const noexcept
{
	return _accel_y;
}

double Sensor::getGyroX() const noexcept
{
	return _gyro_raw_x;
}

double Sensor::getGyroY() const noexcept
{
	return _gyro_raw_y;
}

double Sensor::getGyroZ() const noexcept
{
	return _gyro_raw_z;
}

double Sensor::getPitch() const noexcept
{
	return _pitch;
}

double Sensor::getRoll() const noexcept
{
	return _roll;
}

bool Sensor::setRegister(uint8_t reg, uint8_t value) noexcept
{
	_wire.beginTransmission(_address);
	_wire.write(reg);
	_wire.write(value);

	if (_wire.endTransmission() != 0U)
	{
		_status = SensorStatus::ERR_WRITE;
		return false;
	}

	return true;
}

uint8_t Sensor::getRegister(uint8_t reg) noexcept
{
	_wire.beginTransmission(_address);
	_wire.write(reg);

	if (_wire.endTransmission() != 0U)
	{
		_status = SensorStatus::ERR_WRITE;
		return 0U;
	}

	uint8_t response_length = _wire.requestFrom(_address, 1U);
	if (response_length != 1U)
	{
		_status = SensorStatus::ERR_READ;
		return 0U;
	}

	return static_cast<uint8_t>(_wire.read());
}

SensorStatus Sensor::getStatus() noexcept
{
	SensorStatus status = _status;
	_status = SensorStatus::OK;

	return status;
}

int16_t Sensor::_readHighLow() noexcept
{
	const int16_t high = static_cast<int16_t>(_wire.read());
	const int16_t low = static_cast<int16_t>(_wire.read());

	const int8_t bits_in_byte = 8;

	return high << bits_in_byte | low;
}