/**
 @file FaBo9Axis_MPU9250.cpp
 @brief This is a library for the FaBo 9Axis I2C Brick.

   http://fabo.io/202.html

   Released under APACHE LICENSE, VERSION 2.0

   http://www.apache.org/licenses/

 @author FaBo<info@fabo.io>
 */

 #include "FaBo9Axis_MPU9250.h"

/**
 @brief Constructor
 @param [in] addr MPU-9250 I2C slave address
 */
 FaBo9Axis::FaBo9Axis(uint8_t addr) {
  _mpu9250addr = addr;
  Wire.begin();
}

/**
 @brief Begin Device
 @retval true normaly done
 @retval false device error
 */
 bool FaBo9Axis::begin() {
  if (searchDevice()) {
    configMPU9250();
    configAK8963();
    return true;
    } else {
      return false;
    }
  }

/**
 @brief Search Device
 @retval true device connected
 @retval false device error
 */
 bool FaBo9Axis::searchDevice() {
  uint8_t whoami;
  readI2c(_mpu9250addr, MPU9250_WHO_AM_I, 1, &whoami);
  if(whoami == 0x71){
    return true;
    } else{
      return false;
    }
  }

/**
 @brief Configure MPU-9250
 @param [in] gfs Gyro Full Scale Select(default:MPU9250_GFS_250[+250dps])
 @param [in] afs Accel Full Scale Select(default:MPU9250_AFS_2G[2g])
 */
 void FaBo9Axis::configMPU9250(uint8_t gfs, uint8_t afs) {
  switch(gfs) {
    case MPU9250_GFS_250:
    _gres = 250.0/32768.0;
    break;
    case MPU9250_GFS_500:
    _gres = 500.0/32768.0;
    break;
    case MPU9250_GFS_1000:
    _gres = 1000.0/32768.0;
    break;
    case MPU9250_GFS_2000:
    _gres = 2000.0/32768.0;
    break;
  }
  switch(afs) {
    case MPU9250_AFS_2G:
    _ares = 2.0/32768.0;
    break;
    case MPU9250_AFS_4G:
    _ares = 4.0/32768.0;
    break;
    case MPU9250_AFS_8G:
    _ares = 8.0/32768.0;
    break;
    case MPU9250_AFS_16G:
    _ares = 16.0/32768.0;
    break;
  }
  // sleep off
  writeI2c(_mpu9250addr, MPU9250_PWR_MGMT_1, 0x00);
  delay(100);
  // auto select clock source
  writeI2c(_mpu9250addr, MPU9250_PWR_MGMT_1, 0x01);
  delay(200);
  // DLPF_CFG
  writeI2c(_mpu9250addr, MPU9250_CONFIG, 0x03);
  // sample rate divider
  writeI2c(_mpu9250addr, MPU9250_SMPLRT_DIV, 0x04);
  // gyro full scale select
  writeI2c(_mpu9250addr, MPU9250_GYRO_CONFIG, gfs << 3);
  // accel full scale select
  writeI2c(_mpu9250addr, MPU9250_ACCEL_CONFIG, afs << 3);
  // A_DLPFCFG
  writeI2c(_mpu9250addr, MPU9250_ACCEL_CONFIG_2, 0x03);
  // BYPASS_EN
  writeI2c(_mpu9250addr, MPU9250_INT_PIN_CFG, 0x02);
  delay(100);
}

void FaBo9Axis::dumpConfig() {
  uint8_t c;
  readI2c(_mpu9250addr, MPU9250_GYRO_CONFIG, 1, &c);
  Serial.println(c,HEX);
  readI2c(_mpu9250addr, MPU9250_ACCEL_CONFIG, 1, &c);
  Serial.println(c,HEX);
  readI2c(_mpu9250addr, MPU9250_ACCEL_CONFIG_2, 1, &c);
  Serial.println(c,HEX);
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 1, &c);
  Serial.println(c,HEX);
}

/**
 @brief Configure AK8963
 @param [in] mode Magneto Mode Select(default:AK8963_MODE_C8HZ[Continous 8Hz])
 @param [in] mfs Magneto Scale Select(default:AK8963_BIT_16[16bit])
 */
 void FaBo9Axis::configAK8963(uint8_t mode, uint8_t mfs) {
  uint8_t data[3];

  switch(mfs) {
    case AK8963_BIT_14:
    _mres = 4912.0/8190.0;
    break;
    case AK8963_BIT_16:
    _mres = 4912.0/32760.0;
    break;
  }

  // set software reset
//   writeI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL2, 0x01);
//   delay(100);
  // set power down mode
  writeI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x00);
  delay(1);
  // set read FuseROM mode
  writeI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x0F);
  delay(1);
  // read coef data
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_ASAX, 3, data);
  _magXcoef = (float)(data[0] - 128) / 256.0 + 1.0;
  _magYcoef = (float)(data[1] - 128) / 256.0 + 1.0;
  _magZcoef = (float)(data[2] - 128) / 256.0 + 1.0;
  // set power down mode
  writeI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x00);
  delay(1);
  // set scale&continous mode
  writeI2c(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, (mfs<<4|mode));
  delay(1);
}

/**
 @brief Check data ready
 @retval true data is ready
 @retval false data is not ready
 */
 bool FaBo9Axis::checkDataReady() {
  uint8_t drdy;
  readI2c(_mpu9250addr, MPU9250_INT_STATUS, 1, &drdy);
  if ( drdy & 0x01 ) {
    return true;
    } else {
      return false;
    }
  }

/**
 @brief Read accelerometer
 @param [out] ax X-accel(g)
 @param [out] ay Y-accel(g)
 @param [out] az Z-accel(g)
 */
 void FaBo9Axis::readAccelXYZ(float * ax, float * ay, float * az) {
  uint8_t data[6];
  int16_t axc, ayc, azc;
  readI2c(_mpu9250addr, MPU9250_ACCEL_XOUT_H, 6, data);
  axc = ((int16_t)data[0] << 8) | data[1];
  ayc = ((int16_t)data[2] << 8) | data[3];
  azc = ((int16_t)data[4] << 8) | data[5];
  *ax = (float)axc * _ares;
  *ay = (float)ayc * _ares;
  *az = (float)azc * _ares;
}
float FaBo9Axis::readAccelX() {
  uint8_t data[6];
  int16_t axc, ayc, azc;
  readI2c(_mpu9250addr, MPU9250_ACCEL_XOUT_H, 6, data);
  axc = ((int16_t)data[0] << 8) | data[1]; 
  float ax = (float)axc * _ares;
  return ax;
}
float FaBo9Axis::readAccelY() {
  uint8_t data[6];
  int16_t axc, ayc, azc;
  readI2c(_mpu9250addr, MPU9250_ACCEL_XOUT_H, 6, data);
  ayc = ((int16_t)data[2] << 8) | data[3];
  float ay = (float)ayc * _ares;
  return ay;
}
float FaBo9Axis::readAccelZ() {
  uint8_t data[6];
  int16_t axc, ayc, azc;
  readI2c(_mpu9250addr, MPU9250_ACCEL_XOUT_H, 6, data);
  azc = ((int16_t)data[4] << 8) | data[5];
  float az = (float)azc * _ares;
  return az;
}
/**
 @brief Read gyro
 @param [out] gx X-gyro(degrees/sec)
 @param [out] gy Y-gyro(degrees/sec)
 @param [out] gz Z-gyro(degrees/sec)
 */
 void FaBo9Axis::readGyroXYZ(float * gx, float * gy, float * gz) {
  uint8_t data[6];
  int16_t gxc, gyc, gzc;
  readI2c(_mpu9250addr, MPU9250_GYRO_XOUT_H, 6, data);
  gxc = ((int16_t)data[0] << 8) | data[1];
  gyc = ((int16_t)data[2] << 8) | data[3];
  gzc = ((int16_t)data[4] << 8) | data[5];
  *gx = (float)gxc * _gres;
  *gy = (float)gyc * _gres;
  *gz = (float)gzc * _gres;
}
float FaBo9Axis::readGyroX() {
  uint8_t data[6];
  int16_t gxc, gyc, gzc;
  readI2c(_mpu9250addr, MPU9250_GYRO_XOUT_H, 6, data);
  gxc = ((int16_t)data[0] << 8) | data[1];
  float gx = (float)gxc * _gres;
  return gx;
}
float FaBo9Axis::readGyroY() {
  uint8_t data[6];
  int16_t gxc, gyc, gzc;
  readI2c(_mpu9250addr, MPU9250_GYRO_XOUT_H, 6, data);
  gyc = ((int16_t)data[2] << 8) | data[3];
  float gy = (float)gyc * _gres;
  return gy;
}
float FaBo9Axis::readGyroZ() {
  uint8_t data[6];
  int16_t gxc, gyc, gzc;
  readI2c(_mpu9250addr, MPU9250_GYRO_XOUT_H, 6, data);
  gzc = ((int16_t)data[4] << 8) | data[5];
  float gz = (float)gzc * _gres;
  return gz;
}
/**
 @brief Read magneto
 @param [out] mx X-magneto(uT)
 @param [out] my Y-magneto(uT)
 @param [out] mz Z-magneto(uT)
 */
 void FaBo9Axis::readMagnetXYZ(float * mx, float * my, float * mz) {
  uint8_t data[7];
  uint8_t drdy;
  int16_t mxc, myc, mzc;
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_ST1, 1, &drdy);
  if ( drdy & 0x01 ) { // check data ready
    readI2c(AK8963_SLAVE_ADDRESS, AK8963_HXL, 7, data);
    if ( !(data[6] & 0x08) ) { // check overflow
      mxc = ((int16_t)data[1] << 8) | data[0];
      myc = ((int16_t)data[3] << 8) | data[2];
      mzc = ((int16_t)data[5] << 8) | data[4];
      *mx = (float)mxc * _mres * _magXcoef;
      *my = (float)myc * _mres * _magYcoef;
      *mz = (float)mzc * _mres * _magZcoef;
    }
  }
}
float FaBo9Axis::readMagnetX() {
  uint8_t data[7];
  uint8_t drdy;
  int16_t mxc, myc, mzc;
  float mx;
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_ST1, 1, &drdy);
  if ( drdy & 0x01 ) { // check data ready
    readI2c(AK8963_SLAVE_ADDRESS, AK8963_HXL, 7, data);
    if ( !(data[6] & 0x08) ) { // check overflow
      mxc = ((int16_t)data[1] << 8) | data[0];
      mx = (float)mxc * _mres * _magXcoef;
    }
  }
  return mx;
}
float FaBo9Axis::readMagnetY() {
  uint8_t data[7];
  uint8_t drdy;
  float my ;
  int16_t mxc, myc, mzc;
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_ST1, 1, &drdy);
  if ( drdy & 0x01 ) { // check data ready
    readI2c(AK8963_SLAVE_ADDRESS, AK8963_HXL, 7, data);
    if ( !(data[6] & 0x08) ) { // check overflow
     myc = ((int16_t)data[3] << 8) | data[2];
     my = (float)myc * _mres * _magYcoef;
   }
 }
 return my;
}

float FaBo9Axis::readMagnetZ() {
  uint8_t data[7];
  uint8_t drdy;
  float mz ;
  int16_t mxc, myc, mzc;
  readI2c(AK8963_SLAVE_ADDRESS, AK8963_ST1, 1, &drdy);
  if ( drdy & 0x01 ) { // check data ready
    readI2c(AK8963_SLAVE_ADDRESS, AK8963_HXL, 7, data);
    if ( !(data[6] & 0x08) ) { // check overflow
     mzc = ((int16_t)data[5] << 8) | data[4];
     mz = (float)mzc * _mres * _magZcoef;
   }
 }
 return mz;
}
/**
 @brief Read temperature
 @param [out] temperature temperature(degrees C)
 */
 void FaBo9Axis::readTemperature(float * temperature) {
  uint8_t data[2];
  int16_t tmc;
  readI2c(_mpu9250addr, MPU9250_TEMP_OUT_H, 2, data);
  tmc = ((int16_t)data[0] << 8) | data[1];
  *temperature = ((float)tmc) / 333.87 + 21.0;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////

/**
 @brief Write I2C
 @param [in] address I2C slave address
 @param [in] register_addr register address
 @param [in] data write data
 */
 void FaBo9Axis::writeI2c(uint8_t address, uint8_t register_addr, uint8_t data) {
  Wire.beginTransmission(address);
  Wire.write(register_addr);
  Wire.write(data);
  Wire.endTransmission();
}

/**
 @brief Read I2C
 @param [in] address I2C slave address
 @param [in] register_addr register address
 @param [in] num read length
 @param [out] buffer read data
 */
 void FaBo9Axis::readI2c(uint8_t address, uint8_t register_addr, uint8_t num, uint8_t * buffer) {
  Wire.beginTransmission(address);
  Wire.write(register_addr);
  Wire.endTransmission();
  uint8_t i = 0;
  Wire.requestFrom(address, num);
  while( Wire.available() ) {
    buffer[i++] = Wire.read();
  }
}