#include <stdio.h>
#include <iostream>
#include <string>
#include <stdint.h>
#include <unistd.h>
#include "mraa.hpp"

using namespace mraa;

using namespace std;

#pragma region Definitions

#define USER_CTRL 0x6A
#define WHO_AM_I 0x75
#define AM_I_TRUE true

#pragma endregion

int main(int argc, char **argv)
{
	// set up the mraa library
	mraa_init();

	// set the SPI channels
	Spi* spi = new Spi(0);

	// set the frequency ( 10Mhz )
	spi->frequency(1000000);

	// set the mode
	spi->mode(MRAA_SPI_MODE1);

	// ensure MSB is sent first
	spi->lsbmode(false);

	usleep(1000000);

	// This disables I2C from the start
	spi->write_word(0x10 | USER_CTRL);

	usleep(1000000);

	while (AM_I_TRUE)
	{
		uint8_t result = spi->write_word(WHO_AM_I);

		cout << "I should be 0x71, I am " << hex << result << endl;

		usleep(1000000);
	}

	while (!AM_I_TRUE)
	{
		uint16_t gyroX = spi->write_word(0x43) | spi->write_word(0x44);
		uint16_t gyroY = spi->write_word(0x45) | spi->write_word(0x46);
		uint16_t gyroZ = spi->write_word(0x47) | spi->write_word(0x48);

		cout << "X: " << hex << gyroX << " Y: " << hex << gyroY << " Z: " << hex << gyroZ << endl;

		usleep(1000000);
	}

	delete spi;
}