#include <Wire.h>

enum {x,y,z};
enum {ac1,ac2,ac3,ac4,ac5,ac6,b1,b2,mb,mc,md};
enum {temp,pressure,altitude};
enum {initTemp,getTemp,initPres,getPres};

#define ACCEL_ADDRESS (0x53)     //Accel address, 10 bit res, 13bit at 16g. Setup function needs modification if using 13 bit data
#define ACCEL_ID 0xE5            //Devide ID, used to confirm correct device is attached to address
#define ACCEL_SENSITIVITY 0b01   //Set sensitivity +/-: 0b00 2g, 0b01 4g, 0b10 8g, 0b11 16g
byte accel_buff[6];
int  accel_data[3];

#define GYRO_ADDRESS (0x69)      //Gyro address, 16 bit res
#define GYRO_ID 0b11010011       //Devide ID, used to confirm correct device is attached to address
#define GYRO_SENSITIVITY 0b11    //Set sensitivity in dps: 0b00 250, 0b01 500, 0b10 2000, 0b11 2000
byte gyro_buff[6];
int  gyro_data[3];
byte  gyro_temp;

#define MAG_ADDRESS (0x1E)       //Mag address, 12 bit res
#define MAG_ID1 0b01001000       //Device ID, used to confirm correct device is attached to address. Mag has 3 ID bytes with ACII values: "H4C"
#define MAG_ID2 0b00110100
#define MAG_ID3 0b00110011
#define MAG_GAIN 0x01            //Range(gain control) +/- Ga / LSB per Gauss(0x00-0x07): 0x00 0.88ga/1370, 0x01 1.3ga/1090(default), 0x02 1.9ga/820,0x03 2.5ga/660, 0x04 4.0ga/440, 0x05 4.7ga/390, 0x06 5.6ga/330, 0x07 8.1ga/230
byte mag_buff[6];
int mag_data[3];
int olddata[2];
float heading;

#define BAR_ADDRESS (0b1110111)  //Bar address, 16 bit res(preasure data up to 19 bit)
#define BAR_SENSITIVITY 3        //Bar sensitivity(resolution): 0=ultra low power(4.5ms delay), 1=standard(7.5ms delay), 2=High resolution(13.5ms delay), 3=Ultra high resoltuion(25.5ms delay)
#if (BAR_SENSITIVITY==0)         //Set delay time according to sensitivity, does not need to be changed
  #define BAR_PRESDELAY 5
#elif (BAR_SENSITIVITY==1)
  #define BAR_PRESDELAY 8
#elif (BAR_SENSITIVITY==2)
  #define BAR_PRESDELAY 14
#elif (BAR_SENSITIVITY==3)
  #define BAR_PRESDELAY 26
#endif
#define BAR_TEMPDELAY 5          //4.5ms delay required to measure temp
uint16_t bar_registers[]={0xAA,0xAC,0xAE,0xB0,0xB2,0xB4,0xB6,0xB8,0xBA,0xBC,0xBE};
byte bar_buff[3];
int16_t  bar_calib[11];
int32_t bar_data[3];
//Calculation variables:
int32_t b5;
//Bar state machine variables:
byte bar_state=initTemp;
unsigned long bar_timers[2];

void setup()
{
  Wire.begin();                  // join i2c bus (address optional for master)
  Serial.begin(115200);          // start serial for output. Make sure you set your Serial Monitor to the same!
  setupAccel();
  setupGyro();
  setupMag();
  setupBar();
}

void loop(){
  //Accel:
    readAccel();
    displayAccel();
  //Mag:
    readMagRaw();
    displayMag();
  //Gyro:
    readGyro();
    displayGyro();
  //Bar:
    readBar();
    displayBar();

  Serial.println();
}

/*-----------------------------------------------------------------Sensor functions---------------------------------------------------*/
//---------------Barometer
void setupBar(){
  for(int i=0;i<sizeof(bar_registers);i++){
    readFrom(BAR_ADDRESS, bar_registers[i], 2, bar_buff);
    bar_calib[i]=(bar_buff[0]<<8)|bar_buff[1];
  }
}
void readBar(){
  int32_t b3,b6,x1,x2,x3;
  uint32_t b4,b7;

  switch(bar_state){
    case initTemp:
      bar_state=getTemp;
      writeTo(BAR_ADDRESS, 0xF4, 0x2E);
      bar_timers[temp]=millis();
      break;
    case getTemp:
      if(millis()>bar_timers[temp]+BAR_TEMPDELAY){
        bar_state=initPres;
        readFrom(BAR_ADDRESS, 0xF6, 2, bar_buff);
        bar_data[temp]=(bar_buff[0]<<8)|bar_buff[1];
        x1=((bar_data[temp]-(uint16_t)bar_calib[ac6])*(uint16_t)bar_calib[ac5])>>15;
        x2=((int32_t)bar_calib[mc]<<11)/(x1+bar_calib[md]);
        b5=x1+x2;
        bar_data[temp]=(b5+8)>>4;
      }
      break;
    case initPres:
      bar_state=getPres;
      writeTo(BAR_ADDRESS, 0xF4, 0x34+(BAR_SENSITIVITY<<6));
      bar_timers[pressure]=millis();
      break;
    case getPres:
      if(millis()>bar_timers[pressure]+BAR_PRESDELAY){
        if(millis()>bar_timers[temp]+1000)
          bar_state=initTemp;
        else
          bar_state=initPres;
        readFrom(BAR_ADDRESS, 0xF6, 3, bar_buff);
        bar_data[pressure]=((((uint32_t)bar_buff[0])<<16)|((uint32_t)bar_buff[1]<<8)|bar_buff[2])>>(8-BAR_SENSITIVITY);
        b6=b5-4000;
        x1=(bar_calib[b2]*(b6*b6)>>12)>>11;
        x2=(bar_calib[ac2]*b6)>>11;
        x3=x1+x2;
        b3=(((((int32_t)bar_calib[ac1])*4+x3)<<BAR_SENSITIVITY)+2)>>2;
        x1=(bar_calib[ac3]*b6)>>13;
        x2=(bar_calib[b1]*((b6*b6)>>12))>>16;
        x3=((x1+x2)+2)>>2;
        b4=((uint16_t)bar_calib[ac4]*(uint32_t)(x3+32768))>>15;
        b7=((uint32_t)(bar_data[pressure]-b3)*(50000>>BAR_SENSITIVITY));

        if(b7<0x80000000)
          bar_data[pressure]=(b7<<1)/b4;
        else
          bar_data[pressure]=(b7/b4)<<1;
        x1=(bar_data[pressure]>>8)*(bar_data[pressure]>>8);
        x1=(x1*3038)>>16;
        x2=(-7357*bar_data[pressure])>>16;
        bar_data[pressure]+=((x1+x2+3791)>>4);
      }
      break;
    default:
      Serial.println("Error in bar_state!");
  }
}
void displayBar(){
  String s;
  s+="   Bar Temp:";
  s+=pad(bar_data[temp],5);
  s+=" Preasure:";
  s+=bar_data[pressure];
  s+=" Altitude:";
  s+=pad(bar_data[altitude],5);
  Serial.print(s);
}

//---------------Accelerometer
void setupAccel(){
  readFrom(ACCEL_ADDRESS, 0x00, 1, accel_buff);                                                                          //Read ID of accel
  checkID(ACCEL_ID,accel_buff[0]);                                                                                       //Verify match of expected to received
  //                             x-------   -x------   --x-----   ----x---   -----x--     ------xx
  writeTo(ACCEL_ADDRESS, 0x31, 0b00000000|0b00000000|0b00000000|0b00000000|0b00000000|ACCEL_SENSITIVITY);                //DATA_FORMAT register: First bit is self test: 0=disabled, 1=enable(offset output data). Second bit is spi interface: 0=4 wire, 1=3 wire. Third bit inverts INT active state: 0=active high, 1=active low. Fourth bit unused. Fifth bit is full resolution mode: 0=10 bit mode where scale and g-range change with sensitivity(range), 1=output resolution increases with range to maintain 4mg/LSB scale factor. Sixth bit is for justify: 0=right justified with sign extension, 1=left(MSB) justified. Last two bits set sensitivity(range).
  //                             --x-----   ---x----   ----x---   -----x--   ------xx
  writeTo(ACCEL_ADDRESS, 0x2D, 0b00000000|0b00000000|0b00001000|0b00000000|0b00000000);                                  //POWER_CTL register: First two bits are unused. Third bit is link state: Reference datasheet. Fourth bit is AUTO_SLEEP mode: 0=disabled, 1=reference datasheet.Fifth bit is Measure mode: 0=standby, 1=measurement mode. Sixth bit is sleep mode: 0=normal mode,1=sleep mode. Last two bits are for WAKUP: sample rate to set when waking up.
}
void readAccel() {
  readFrom(ACCEL_ADDRESS, 0x32, 6, accel_buff);                                   //read the acceleration data from the ADXL345
  accel_data[x]=(((int)(accel_buff[1]<<6))<<2)|accel_buff[0];                     //10 bit numbers, LSB first in array. Could <<8 but this prevents rollover and bad data on unused bits.
  accel_data[y]=(((int)(accel_buff[3]<<6))<<2)|accel_buff[2];
  accel_data[z]=(((int)(accel_buff[5]<<6))<<2)|accel_buff[4];
}
void displayAccel(){
  String s;
  s+="   Accel X:";
  s+=pad(accel_data[x]);
  s+=" Y:";
  s+=pad(accel_data[y]);
  s+=" Z:";
  s+=pad(accel_data[z]);
  Serial.print(s);
}
//---------------Gyro
void setupGyro(){
  readFrom(GYRO_ADDRESS, 0x0F, 1, gyro_buff);                                                            //Read ID of Gyro
  checkID(GYRO_ID,gyro_buff[0]);                                                                         //Verify match of expected to received
  //                            xx------   --xx----   ----x---   -----xxx
  writeTo(GYRO_ADDRESS, 0x20, 0b00000000|0b00110000|0b00001000|0b00000111);                              //CTRL_REG1: First two bits are data rate in Hz: 00=100,01=200,10=400,11=800. Second two bits are bandwidth(look in datasheet for details). Fith bit is Power Down Enable: 0=power down mode, 1=Normal/sleep mode.Last 3 bits are ZYX Axis enable bits: 0=disabled,1=Enabled.
  readFrom(GYRO_ADDRESS, 0x21, 1, gyro_buff);                                                            //CTRL_REG2 writes first two bits on boot that shouldn't be changed, so let's first read them so when we write new data we don't change those bits. Recyling the gyro_buffer since there is no need to waste memory on more variables.
  //                              xx------   --xx----   ----xxxx
  writeTo(GYRO_ADDRESS, 0x21, gyro_buff[0]|0b00000000|0b00000000);                                       //CTRL_REG2: First two bits should remain unchanged so write them as we read them. Second two bits are high pass filter mode selection: 00=Normal mode(reset reading HP_RESET_FILTER),01=reference signal for filtering, 10=normal mode, 11=autoreset on interrupt event. Last 4 bits are high pass filter cut off frequency selection(refer to datasheet for table).
  //                            x-------   -x------         --xx----      -----xx-   -------x
  writeTo(GYRO_ADDRESS, 0x23, 0b00000000|0b00000000|GYRO_SENSITIVITY<<4|0b00000000|0b00000000);          //CTRL_REG4: First bit is block data update: 0=continuous, 1=output registers not updated until MSG and LCB reading. Second bit bit/little endian data selection: 0=data LSB at lower address,1=data MSB at lower address. Next two bits are full scale selection(sensitivity in dps. Fith bit is unused. Sixth and Seventh bit are self test enable: 00=disabled, others=see datasheet table. Last bit is SPI interface mode(not used since we're using i2c): 0=4 wire, 1=3 wire.
  //                            x-------   -x------   ---x----   ----xx--   ------xx
  writeTo(GYRO_ADDRESS, 0x24, 0b00000000|0b00000000|0b00000000|0b00000000|0b00000000);                   //CTRL_REG5:  First bit is reboot memeory content: 0=normal mode, 1=reboot memory content. Second bit is FIFO enable. Third bit unused. Fourth bit is high pass filter enable: 0=disabled, 1=enabled. Bits five and six used for INT1 selection configuration(see datasheet for details). Last two bits are for Out selection configuration(see datasheet for details).
}
void readGyro() {
  readFrom(GYRO_ADDRESS, 0x26, 1, gyro_buff);                                    //Read temp for compensation or other uses
  gyro_temp=gyro_buff[0];
  readFrom(GYRO_ADDRESS, 0b10000000|0x28, 6, gyro_buff);                         //Read the rate data from the L3G4200D, 0b10000000 is require for multi byte readout
  gyro_data[x]=((((int)(gyro_buff[1]))<<8)|gyro_buff[0])-1;                      //Full 16 bit numbers, LSB first, goes out of range for signed int so need to subtract 1 to keep it within –32768 to 32767
  gyro_data[y]=((((int)(gyro_buff[3]))<<8)|gyro_buff[2])-1;
  gyro_data[z]=((((int)(gyro_buff[5]))<<8)|gyro_buff[4])-1;
}
void displayGyro(){
  String s;
  s+="   Gyro X:";
  s+=pad(gyro_data[x],5);
  s+=" Y:";
  s+=pad(gyro_data[y],5);
  s+=" Z:";
  s+=pad(gyro_data[z],5);
  s+=" Temp:";
  s+=pad(gyro_temp);
  Serial.print(s);
}


//---------------Magnetometer
void setupMag(){
  readFrom(MAG_ADDRESS, 0x0A, 3, mag_buff);                                  //Read the 3 ID bytes of Mag
  checkID(MAG_ID1,mag_buff[0]);                                              //Verify first ID byte.
  checkID(MAG_ID2,mag_buff[1]);                                              //Verify second ID byte.
  checkID(MAG_ID3,mag_buff[2]);                                              //Verify third ID byte.
  //                           x-------   -xx-----   ---xxx--   ------xx
  writeTo(MAG_ADDRESS, 0x00, 0b10000000|0b01100000|0b00011000|0b00000000);   //First bit needed to clear, next 2 bits sample average: 00=1 01=2 10=4 11=8, next 3 bits data output rate 0b000-0b110(0.75,1.5,3,7.5,15,30,75 Hz) 160 Hz can be achieve in single shot sample mode, last 2 bits measurement config bits 0b00-0b11(normal, posative bias, negative bias, reserved-do not use)
  writeTo(MAG_ADDRESS, 0x01, MAG_GAIN<<5);                                   //Set first 3 bits for gain, last 5 need to be 0 to clear
  writeTo(MAG_ADDRESS, 0x02, 0x00);                                          //Set sampling mode: 0x00 Continuous, 0x01 single shot, 0x03 idle
}
void readMagRaw(){
  readFrom(MAG_ADDRESS, 0x03, 6, mag_buff);                                  //read data from the HMC5883L
  mag_data[x]=(((int)(mag_buff[0]<<4))<<4)|mag_buff[1];                      //12 bit numbers. Could <<8 but this prevents bad data on unused bits.
  mag_data[z]=(((int)(mag_buff[2]<<4))<<4)|mag_buff[3];
  mag_data[y]=(((int)(mag_buff[4]<<4))<<4)|mag_buff[5];
}

void displayMag(){
  String s;
  s+="   Mag X:";
  s+=pad(mag_data[x]);
  s+=" Y:";
  s+=pad(mag_data[y]);
  s+=" Z:";
  s+=pad(mag_data[z]);
  s+=" H:";
  s+=pad(heading);
  Serial.print(s);
}

/*------------------------------------------------------------------i2c functions------------------------------------------------------*/
void writeTo(byte device, byte address, byte val) {
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(val);
  Wire.endTransmission();
}

void readFrom(byte device, byte address, byte num, byte buff[]) {
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();

  Wire.beginTransmission(device);
  Wire.requestFrom(device, num);

  for(int i=0;i<num;i++){
    if(Wire.available())
      buff[i]=Wire.read();
    else
      buff[i]=0;
  }
  Wire.endTransmission();
}

/*------------------------------------------------------Utility functions-----------------------------------------------------------*/
void checkID(uint16_t expected, uint16_t received){
  if(received!=expected){
    Serial.print("Wrong device ID. ");
    Serial.print("Expected: 0x");
    Serial.print(expected,HEX);
    Serial.print("(0b");
    Serial.print(expected,BIN);
    Serial.print(") but received: 0x");
    Serial.print(received,HEX);
    Serial.print("(0b");
    Serial.print(received,BIN);
    Serial.println(")");
    while(true);
  }
}
String pad(int number){
  return pad(number,4);
}
String pad(int number,int digits){
  String ret="";
  if(number<0){
    number*=-1;
    ret+="-";
  }
  else{
    ret+=" ";
  }
  for(int i=0; i < (digits-1) - byte(log10(number)); i++)
    ret+='0';
  ret+=number;
  return ret;
}