/* MMA8452Q.cpp -- simple interface to MM Accelerometer, stolen from:
 *
 * MMA8452Q Basic Example Code
 * Nathan Seidle SparkFun Electronics
 * November 5, 2012
 *
 * turned into a library with added Output Data Rate setting by
 * Michael Schippling bozotronics Feb 2014
 * 
 * License: This code is public domain but you buy me a beer if you use this
 * and we meet someday (Beerware license).
 * 
 * This example code shows how to read the X/Y/Z accelerations and basic
 * functions of the MMA5842. It leaves out all the neat features this IC is
 * capable of (tap, orientation, and interrupts) and just displays X/Y/Z.
 * See the advanced example code to see more features.
 * (the advanced features set various processing on input data to detect
 *  different common activities which are then signalled on the "int" lines,
 *  but (I don't think) any of it changes the actual accel data filtering).
 * 
 * Hardware setup:
 * MMA8452 Breakout ------------ Arduino
 * 3.3V --------------------- 3.3V
 * SDA -------^^(330)^^------- A4
 * SCL -------^^(330)^^------- A5
 * GND ---------------------- GND
 *
 * The MMA8452 _requires_ 3.3V power.
 * When connecting to a 5v controller the masters
 *  recommend using 330 or 1k resistors on the SDA/SCL signal lines.
 * HOWEVER .... it doesn't matter. The MMA has pullup resistors to 3.3v
 *  which wash out the voltage difference. Just connectem up.
 */
#include "MMA8452Q.h" // our externs
#include <Wire.h>

// The SparkFun breakout board defaults to 1,
//  set to 0 if SA0 jumper on the bottom of the board is set
#define MMA8452_ADDRESS 0x1D  // 0x1D if SA0 is high, 0x1C if low

//Define a few of the registers that we will be accessing on the MMA8452
#define OUT_X_MSB 0x01
#define XYZ_DATA_CFG  0x0E
#define WHO_AM_I   0x0D
#define CTRL_REG1  0x2A

/** default constructor
 */
MMA8452Q::MMA8452Q()
{
}

// Initialize the Wire Library and MMA8452 registers.
//  Return 1 if OK or 0 if failed.
// See the many application notes for more info on
//  setting all of these registers:
//  http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MMA8452Q
byte MMA8452Q::init(void)
{
	Wire.begin(); //Join the I2C bus as a master

	byte c = readRegister(WHO_AM_I);  // Read WHO_AM_I register

	if (c != 0x2A) // WHO_AM_I should always be 0x2A
	{
		// something wrong in I2C Denmark
		this->wego = 0;
		return 0;
	}
	this->wego = 1;

	standby();  // Must be in standby to change registers

	// Set up the full scale range to 2, 4, or 8g.
	byte fsr = GSCALE;
	if(fsr > 8) fsr = 8; //Easy error check
	fsr >>= 2; // Neat trick, see page 22. 00 = 2G, 01 = 4A, 10 = 8G
	writeRegister(XYZ_DATA_CFG, fsr);

	//The default data rate is 800Hz and
	// we (might) modify it in this example code

	active();  // Set to active to start reading

	return 1;
}

/** Read the raw accelleration values into an array of 3 signed ints
 *  returned in destination array: 0=X, 1=Y, 2=Z
 *  returns -- pointer to array on success or NULL on failure
 **/
int* MMA8452Q::readData( int *destination )
{
	byte rawData[6];  // raw x/y/z accel register data

	if( !wego )	return NULL;

	// Read the six raw data registers into data array
	// if we get crap, take one
	if( readRegisters(OUT_X_MSB, 6, rawData) == 0)
		return NULL;

	// Loop to calculate 12-bit ADC and g value for each axis
	for(int i = 0; i < 3 ; i++)
	{
		//Combine the two 8 bit registers into one 12-bit number
		int gCount = (rawData[i*2] << 8) | rawData[(i*2)+1];
		//The registers are left align, here we right align the 12-bit integer
		gCount >>= 4;

		// If the number is negative, we have to make it so manually
		if (rawData[i*2] > 0x7F)
		{  
			gCount = ~gCount + 1;
			gCount *= -1;  // Transform into negative 2's complement #
		}

		destination[i] = gCount; //Record this gCount into the 3 int array
	}

	return destination;
}

// Sets the MMA8452 to standby mode.
// It must be in standby to change most register settings
void MMA8452Q::standby(void)
{
	byte c = readRegister(CTRL_REG1);
	//Clear the active bit to go into standby
	writeRegister(CTRL_REG1, c & ~(0x01));
}

// Sets the MMA8452 to active mode.
// Needs to be in this mode to output data
void MMA8452Q::active(void)
{
	byte c = readRegister(CTRL_REG1);
	// set Output Data Rate -- assume we start with all 0's (800Hz)
	c |= USEODR;
	//Set the active bit to begin detection
	writeRegister(CTRL_REG1, c | 0x01);
}

// Do setup for bytesToRead
//  read bytesToRead from addressToRead
// return true if number of bytes available is what we want or 0 if not
static byte setupRead( byte addressToRead, byte bytesToRead )
{
	byte x=0;

	Wire.beginTransmission(MMA8452_ADDRESS);
	Wire.write(addressToRead);
	//endTransmission but keep the connection active
	Wire.endTransmission(false);

	//Ask for bytes, once done, bus is released by default
	Wire.requestFrom(MMA8452_ADDRESS, (int)bytesToRead);

	//Hang out until we get the # of bytes we expect
	//// NOTE: check how long this takes...
	do
	{
		// give it 255 tries before returning an error
		// because we sometimes hang in here waiting for a missed byte
		// this seems to recover OK if we just redo the whole command
		// so just return an error if we overflow here
	   ++x;
	}
	while( x && (Wire.available() < bytesToRead) ) ;
	
	return x;	// will be 0 if we missed some message
}

// Read number of bytes sequentially,
//  read from addressToRead into the dest byte array for bytesToRead
//  return thenumber of bytes actually read,
//   either 0 on failure or bytesToRead on success...
//
byte MMA8452Q::readRegisters(byte addressToRead, byte bytesToRead, byte * dest)
{
	if( setupRead( addressToRead, bytesToRead ) == 0 )
		return 0;

	// copy bytes into destination
	for( int x = 0 ; x < bytesToRead ; x++ )
		dest[x] = Wire.read();    

	return bytesToRead;
}

// Read a single byte from addressToRead and return it as a byte
//  If something seems to go wrong, return 0xff --
//   this may be useless depending on what we expect to get...
byte MMA8452Q::readRegister(byte addressToRead)
{
	if( setupRead( addressToRead, 1 ) == 0 )
		return (byte)-1;

	//Return this one byte
	return Wire.read();
}

// Write a single byte (dataToWrite) into addressToWrite
void MMA8452Q::writeRegister(byte addressToWrite, byte dataToWrite)
{
	Wire.beginTransmission(MMA8452_ADDRESS);
	Wire.write(addressToWrite);
	Wire.write(dataToWrite);
	Wire.endTransmission(); //Stop transmitting
}