/*
	@author Jonathan Krueger
*/

#include "MPU9250.h"

#pragma region Definitions
#define USER_CTRL 0x6A


#pragma endregion

MPU9250* MPU9250::myInstance = 0;

#pragma region Creation/Deletion
/*
Instance getter for the singleton
Creaates a new instance on first call, future calls just return the created instance
*/
MPU9250* MPU9250::instance()
{
	if (myInstance == 0)
	{
		myInstance = new MPU9250();
	}
	return myInstance;
}

/**
* Constructor for the LSM9DS0 class.
* Sets up the right SPI channels, frequency, mode, and registers.
*/
MPU9250::MPU9250() {

	// set up the mraa library
	mraa_init();

	// set the SPI channels
	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(10);

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

/**
* Deconstructor for the LSM9DS0 class.
* Realeases the SPI channels.
*/
MPU9250::~MPU9250() {
	delete spi;
}

#pragma endregion

#pragma region Read/Write
/**
* Read command for the MPU9250
*
* Input	: Address to read from
* Returns	: Results from chip
*/
uint8_t MPU9250::read(uint8_t address) {

	return spi->write_word(address);
}

/**
* Write command for the MPU9250
* WARNING	: Writing to a reserved register can cause harm to the device.
* 			  Please read documentation before proceeding.
*
* Input	: Address to write to, data to write
*/
void MPU9250::write(uint8_t address, uint8_t data) {

	spi->write_word((data << 8) | address);
}

#pragma endregion