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

#include "gyroscope.hpp"

void Gyroscope::setup(int id)
{
	_gyroscope_id = 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 coordinates from the gyroscope

	@param x A pointer to a variable to store the x coordinate in
	@param y A pointer to a variable to store the y coordinate in
	@param z A pointer to a variable to store the z coordinate in
*/
void Gyroscope::readCoordinates(int16_t *x, int16_t *y, int16_t *z)
{
	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)
		;

	*x = _read_next_coordinate();
	*y = _read_next_coordinate();
	*z = _read_next_coordinate();
}

/*
	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(int8_t registry, int8_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 = Gyroscope::testRegistry(Addresses::ID_HIGH, RegisterAddresses::WHO_AM_I);

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

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

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

	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, int8_t registry)
{
	Wire.beginTransmission(gyroscope_id);

	Wire.write(registry);

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

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

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

	return Wire.read();
}

/*
	Reads the next coordinate given by the gyroscope.

	@returns A coordinate (x, y or z)
*/
int16_t Gyroscope::_read_next_coordinate()
{
	int8_t low = Wire.read();
	int8_t high = Wire.read();

	return (int16_t)(high << 8 | low);
}