/**
 * CSCE236 Spring 2012
 * LSM303DLM Sample interface code
 * Carrick Detweiler
 **/
 
#include <Wire.h>
 
//The 7 bit addresses of the two devices
#define MAG_ADDR 0x1E
#define ACCEL_ADDR 0x18

//Global variables to hold the mag and accel values
int magX = 0, magY = 0, magZ = 0;
int accelX = 0, accelY = 0, accelZ = 0;

/**
 * Write value to the specified register on the lsm303 Accelerometer.
 **/
void writeRegAccel(uint8_t reg, uint8_t value){
     Wire.beginTransmission(ACCEL_ADDR);
     //First specify the register to write
     Wire.write(reg); 
     //Next write the value
     Wire.write(value);
     Wire.endTransmission();
}
 
/**
 * Write value to the specified register on the lsm303 magnetometer.
 **/
void writeRegMag(uint8_t reg, uint8_t value){
     Wire.beginTransmission(MAG_ADDR);
     //First specify the register to write to
     Wire.write(reg); 
     //Next write the value to the register
     Wire.write(value);
     Wire.endTransmission();
}
 
/**
 * This function updates the global varaibles that contain
 * the most recent magnetometer values.
 **/
void updateMag(){
  //Before we can read, we must tell it which register
  //to read by issuing a write with that address as an argument
  Wire.beginTransmission(MAG_ADDR);
  //Address 0x03 is the OUT_X_H_M reg
  Wire.write(0x03);
  Wire.endTransmission();

  //Now request a bunch of bytes from the mag. As specified in
  //the datasheet, each read causes the address pointer to be
  //updated, so no further writes are needed to specify the address
  Wire.requestFrom(MAG_ADDR, 6);
  //Zero them first
  magX = 0; magY = 0; magZ = 0;
  
  //Make sure all 6 bytes are available.
  if(Wire.available() != 6){
    Serial.print("ERROR: not enough bytes returned by compass, only: ");
    Serial.println(Wire.available());
    return;
  }

 //========================================================
 //STUDENT CODE
 //Determine the order that the 6 bytes are recieved from 
 //the magnotometer to properly update magX, magY, magZ.  
 //To do this, refer to Table 15 in the LSM303DLM datahseet.
}
 
/**
 * This function updates the global varaibles that contain
 * the most recent accelerometer values.
 **/
void updateAccel(){
  //Before we can read, we must tell it which register
  //to read by issuing a write with that address as an argument
  Wire.beginTransmission(ACCEL_ADDR);
  //Address 0x28 is the OUT_X_L_A reg
  //Unlike the magnetometer, the memory pointer will only be 
  //incremented if the high bit of the address is set.  Hence the (1<<7).
  Wire.write(0x28 | (1<<7));
  Wire.endTransmission();

  //Now request a bunch of bytes from the mag. Since we set the
  //high bit above when setting the register we wante to read,
  //each read will cause the address pointer to be updated, 
  //so no further writes are needed to specify the address
  Wire.requestFrom(ACCEL_ADDR, 6);
  //Zero them first
  accelX = 0; accelY = 0; accelZ = 0;
  
  //Make sure all 6 bytes are available.
  if(Wire.available() != 6){
    Serial.print("ERROR: not enough bytes returned by accel, only: ");
    Serial.println(Wire.available());
    return;
  }

 //========================================================
 //STUDENT CODE
 //Determine the order that the 6 bytes are recieved from 
 //the accelerometer to properly update accelX, accelY, accelZ.  
 //To do this, refer to Table 15 in the LSM303DLM datahseet.
}
  
void printMag(){
  Serial.print("Mag: ");
  Serial.print(magX);  Serial.print(",");
  Serial.print(magY);  Serial.print(",");
  Serial.print(magZ);
  Serial.println();
 
}

void printAccel(){
  Serial.print("Accel: ");
  Serial.print(accelX);  Serial.print(",");
  Serial.print(accelY);  Serial.print(",");
  Serial.print(accelZ);
  Serial.println();  
}
 
 void setup(){
   //Init the i2c bus as master (no address specified)
   Wire.begin();

   //Setup serial
   Serial.begin(9600);
   
   //////Start the compass/accelerometer
   //CTRL_REG1_A: put in Normal power mode and enable XYZ axes
   writeRegAccel(0x20,(1<<5) | (0x7));
   //MR_REG_M: put in continuous conversion mode
   writeRegMag(0x02,0);
}
 
 
 
 void loop(){
   delay(500);
   updateMag();
   updateAccel();
   Serial.println("-----------------------");
   printMag();
   printAccel();  
 }