/* 
 Trying to convert from wire.h library to I2C.lib

 The MMA8452 has built in pull-up resistors for I2C so you do not need additional pull-ups.
 Source: http://github.com/sparkfun/MMA8452_Accelerometer/tree/master/Firmware/MMA8452Q_BasicExample
 Sparkfun link: http://www.sparkfun.com/products/10955
*/

#include <I2C.h> 

// The SparkFun breakout board defaults to 1, set to 0 if SA0 jumper on the bottom of the board is set
const uint8_t MMA8452_ADDRESS = 0x1D;  // 0x1D (29 dec) if SA0 is high, 0x1C if low

#define ACCELTHRSHOLD 400    // Accelerometer register value that has to be exceded to assume there is movement
uint32_t getAccelTimer;     // timer to get accelerometer values every GETACCELDELAY milliseconds
uint32_t accel1SecTmr;    // print accelerometer register once a second
#define GETACCELDELAY 100   // get acceleromoter every 100 mS


//-------------------------------------------------------
void setup()
{
  Serial.begin(9600);
  delay(3000);
  while (!Serial && millis() < 8000) {}
  Serial.println(F("MMA8452 Basic Example"));
  pinMode(PCBLED, OUTPUT);
  digitalWrite(PCBLED, LOW);
  
  I2c.begin();   //Join the bus as a master
  initMMA8452(); // Test and intialize the MMA8452
  getAccelTimer = millis() + GETACCELDELAY;
  accel1SecTmr = millis() + 1000;
}

//-------------------------------------------------------
void loop()
{  
  int accelCount[] = {0,0,0};  // Stores the 12-bit signed value

  if((long)(millis() - getAccelTimer) > 0)
  {
    readAccelData(accelCount);  // Read the x/y/z adc values
    getAccelTimer = millis() + GETACCELDELAY;

    // Is accelerometer moving
    if(abs(accelCount[0]) > ACCELTHRSHOLD || abs(accelCount[1]) > ACCELTHRSHOLD )
    { 
      PintAccelValues(accelCount); 
      accel1SecTmr = millis() + 1000;  // just printed values, so set printout timer to 1 second
    }
  }
  
  // print every second if it's not moving
  if((long)(millis() - accel1SecTmr) > 0)
  {
    readAccelData(accelCount);  // Read the x/y/z adc values
    accel1SecTmr = millis() + 1000;
    PintAccelValues(accelCount);
  }

}  


//-------------------------------------------------------
void PintAccelValues(int *accelValues)
{
  // Print out values
  Serial.print(accelValues[0]);  
  Serial.print("\t");  
  Serial.print(accelValues[1]);  
  Serial.print("\t");  
  Serial.println(accelValues[2]);  
}


//-------------------------------------------------------
void readAccelData(int *destination)
{
  uint8_t rawData[6];  // x/y/z accel register data stored here

  readRegisters(0x01, 6, rawData);  // Read the six raw data registers into data array

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

    // If the number is negative, we have to make it so manually (no 12-bit data type)
    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
  }
}

//-------------------------------------------------------
// Initialize the MMA8452 registers 
// 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
void initMMA8452()
{
  byte c = readRegister(0x0D);  // Read WHO_AM_I register
  if (c == 0x2A) // WHO_AM_I should always be 0x2A
  {  
    Serial.println("MMA8452Q is online...");
  }
  else
  {
    Serial.print("Could not connect to MMA8452Q: 0x");
    Serial.println(c, HEX);
//    while(1) ; // Loop forever if communication doesn't happen
  }

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

  // Set up the full scale range to 2, 4, or 8g.
  writeRegister(0x0E, 0);  // 00 for 2G

  //The default data rate is 800Hz and we don't modify it in this example code

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

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

//-------------------------------------------------------
// Sets the MMA8452 to active mode. Needs to be in this mode to output data
void MMA8452Active()
{
  byte c = readRegister(0x2A);
  writeRegister(0x2A, c | 0x01); //Set the active bit to begin detection
}

//-------------------------------------------------------
// Read bytesToRead sequentially, starting at addressToRead into the dest byte array
void readRegisters(uint8_t addressToRead, uint8_t bytesToRead, uint8_t * dest)
{
/*
  Wire.beginTransmission(MMA8452_ADDRESS);
  Wire.write(addressToRead);
  Wire.endTransmission(false); //endTransmission but keep the connection active

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

  while(Wire.available() < bytesToRead); //Hang out until we get the # of bytes we expect

  for(int x = 0 ; x < bytesToRead ; x++)
    dest[x] = Wire.read();    
*/
  I2c.read(MMA8452_ADDRESS, addressToRead, bytesToRead, dest);

}

//-------------------------------------------------------
// Read a single byte from addressToRead and return it as a byte
uint8_t readRegister(uint8_t addressToRead)
{
/*  
  Wire.beginTransmission(MMA8452_ADDRESS);
  Wire.write(addressToRead);
  Wire.endTransmission(false); //endTransmission but keep the connection active
  Wire.requestFrom(MMA8452_ADDRESS, 1); //Ask for 1 byte, once done, bus is released by default
  while(!Wire.available()) ; //Wait for the data to come back
  return Wire.read(); //Return this one byte
*/

  uint8_t getByte;
  I2c.read(addressToRead, getByte, getByte);
  return getByte;
  
}

//-------------------------------------------------------
// Writes a single byte (dataToWrite) into addressToWrite
void writeRegister(uint8_t addressToWrite, uint8_t dataToWrite)
{
/*
  Wire.beginTransmission(MMA8452_ADDRESS);
  Wire.write(addressToWrite);
  Wire.write(dataToWrite);
  Wire.endTransmission(); //Stop transmitting
*/

  I2c.write(MMA8452_ADDRESS, addressToWrite, dataToWrite);
  
}