#include <Wire.h>
#include <math.h>

// LSM303DLH 的 i2c 地址。由於 Arduino i2c 資料空間是 7 bit 所以要右移一個位元
#define ACC_ADDRESS (0x30 >> 1)
#define MAG_ADDRESS (0x3C >> 1)

#define LSM303DLH_CTRL_REG1_A       0x20
#define LSM303DLH_CTRL_REG2_A       0x21
#define LSM303DLH_CTRL_REG3_A       0x22
#define LSM303DLH_CTRL_REG4_A       0x23
#define LSM303DLH_CTRL_REG5_A       0x24
#define LSM303DLH_HP_FILTER_RESET_A 0x25
#define LSM303DLH_REFERENCE_A       0x26
#define LSM303DLH_STATUS_REG_A      0x27

#define LSM303DLH_OUT_X_L_A         0x28
#define LSM303DLH_OUT_X_H_A         0x29
#define LSM303DLH_OUT_Y_L_A         0x2A
#define LSM303DLH_OUT_Y_H_A         0x2B
#define LSM303DLH_OUT_Z_L_A         0x2C
#define LSM303DLH_OUT_Z_H_A         0x2D

#define LSM303DLH_INT1_CFG_A        0x30
#define LSM303DLH_INT1_SRC_A        0x31
#define LSM303DLH_INT1_THS_A        0x32
#define LSM303DLH_INT1_DURATION_A   0x33
#define LSM303DLH_INT2_CFG_A        0x34
#define LSM303DLH_INT2_SRC_A        0x35
#define LSM303DLH_INT2_THS_A        0x36
#define LSM303DLH_INT2_DURATION_A   0x37

#define LSM303DLH_CRA_REG_M         0x00
#define LSM303DLH_CRB_REG_M         0x01
#define LSM303DLH_MR_REG_M          0x02

#define LSM303DLH_OUT_X_H_M         0x03
#define LSM303DLH_OUT_X_L_M         0x04
#define LSM303DLH_OUT_Y_H_M         0x05
#define LSM303DLH_OUT_Y_L_M         0x06
#define LSM303DLH_OUT_Z_H_M         0x07
#define LSM303DLH_OUT_Z_L_M         0x08

#define LSM303DLH_SR_REG_M          0x09
#define LSM303DLH_IRA_REG_M         0x0A
#define LSM303DLH_IRB_REG_M         0x0B
#define LSM303DLH_IRC_REG_M         0x0C


int ax=0,ay=0,az=0;
int mx=0,my=0,mz=0;

void setup(){
  Serial.begin(9600);
  Wire.begin();
  enableDefault();
}

void loop(){
  readdata();
  Serial.println("Accelerometer:\t\t\tMagnetometer:");
  Serial.print("X:");
  Serial.print(ax);
  Serial.print("\t\t\t\tX:");
  Serial.println(mx);
  Serial.print("Y:");
  Serial.print(ay);
  Serial.print("\t\t\t\tY:");
  Serial.println(my); 
  Serial.print("Z:");
  Serial.print(az);
  Serial.print("\t\t\t\tZ:");
  Serial.println(mz);

  delay(500);
}


void enableDefault()
{
// Enable Accelerometer, register 0x20
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
  writeAccReg(0x20, 0x27);

// Enable Magnetometer, register 0x02
// 0x00 = 0b00000000
// Continuous conversion mode
  writeMagReg(0x02, 0x00);
}

// Writes an accelerometer register
void writeAccReg(byte reg, byte value)
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

// Writes a magnetometer register
void writeMagReg(byte reg, byte value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
}

// Reads all 6 channels of the LSM303DLH and stores them in the object variables
void readdata()
{
  readAcc();
  readMag();
}

// Reads the 3 accelerometer channels and stores them in vector a
void readAcc()
{
  Wire.beginTransmission(ACC_ADDRESS);
// assert the MSB of the address to get the accelerometer
// to do slave-transmit subaddress updating.
  Wire.write(0x28 | (1 << 7)); 
  Wire.endTransmission();   
  Wire.requestFrom(ACC_ADDRESS, 6); 
  
  while (Wire.available() < 6); 
  
  uint8_t xla =  Wire.read(); 
  uint8_t xha = Wire.read(); 
  uint8_t yla = Wire.read(); 
  uint8_t yha = Wire.read(); 
  uint8_t zla = Wire.read(); 
  uint8_t zha = Wire.read(); 
  ax = (xha << 8 | xla) >> 4;
  ay = (yha << 8 | yla) >> 4;
  az = (zha << 8 | zla) >> 4;
}

// Reads the 3 magnetometer channels and stores them in vector m
void readMag()
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303DLH_OUT_X_H_M);
  Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 6);

  while (Wire.available() < 6);          
 
  uint8_t xhm = Wire.read();         
  uint8_t xlm = Wire.read();         
  uint8_t yhm = Wire.read();         
  uint8_t ylm = Wire.read();         
  uint8_t zhm = Wire.read();         
  uint8_t zlm = Wire.read();          
  mx = (xhm << 8 | xlm);        
  my = (yhm << 8 | ylm);         
  mz = (zhm << 8 | zlm); 
}