#include <Arduino.h>
#include <Wire.h>

#include "gyroscope.hpp"

/*
	Creates a gyroscope communicator.

	@param _gyroscope_id The who ID of the gyroscope to communicate with
*/
Gyroscope::Gyroscope(int _gyroscope_id)
{
	gyroscope_id = _gyroscope_id;

	writeRegistry(RegisterAddresses::LOW_ODR, 0x00);
	writeRegistry(RegisterAddresses::CTRL4, 0x00);

	// 0x6F = 0b01101111
	//
	// DR1 DR0 BW1 BW0 PD Zen Yen Xen
	//  0   1   1   0   1  1   1   1
	writeRegistry(RegisterAddresses::CTRL1, 0x6F);
}

/*
	Reads data from the gyroscope

	@returns Gyroscope data
*/
ReadGyroscopeData Gyroscope::read()
{
	Wire.beginTransmission(gyroscope_id);
	Wire.write(RegisterAddresses::OUT_X_L | (1 << 7));
	Wire.endTransmission();

	Wire.requestFrom(gyroscope_id, (uint8_t)6);

	while (Wire.available() < 6)
	{
		Serial.println("Hej!");
	}

	uint8_t x_low = Wire.read();
	uint8_t x_high = Wire.read();
	uint8_t y_low = Wire.read();
	uint8_t y_high = Wire.read();
	uint8_t z_low = Wire.read();
	uint8_t z_high = Wire.read();

	ReadGyroscopeData data;

	data.x = (int16_t)(x_high << 8 | x_low);
	data.y = (int16_t)(y_high << 8 | y_low);
	data.z = (int16_t)(z_high << 8 | z_low);

	return data;
}

/*
	Writes a value to a gyroscope registry.

	@param registry The address of a registry
	@param value A value that will be written
*/
void Gyroscope::writeRegistry(uint8_t registry, uint8_t value)
{
	Wire.beginTransmission(gyroscope_id);

	Wire.write(registry);
	Wire.write(value);

	Wire.endTransmission();
}

/*
	Initializes the gyroscope.

	@returns The gyroscope's who id or -1 if it's unidentifiable
*/
int Gyroscope::initialize()
{
	Wire.begin();

	auto high_id = testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);

	if (high_id == Addresses::WHO_ID)
	{
		return high_id;
	}

	auto low_id = testRegistry(Addresses::ID_LOW, RegisterAddresses::WHO_AM_I);

	if (low_id == Addresses::WHO_ID)
	{
		return low_id;
	}

	return -1;
}

/*
	Reads from a gyroscope registry safely.

	@param gyroscope_id The who ID of the gyroscope to read from
	@param registry The address of a registry
	@returns Registry data or -1 if there's no response.
*/
int Gyroscope::testRegistry(int gyroscope_id, uint8_t registry)
{
	Wire.beginTransmission(gyroscope_id);

	Wire.write(registry);

	if (Wire.endTransmission() != 0)
	{
		return -1;
	}

	Wire.requestFrom(gyroscope_id, (uint8_t)1);

	if (!Wire.available())
	{
		return -1;
	}

	return Wire.read();
}