/*!
 *  @file Adafruit_LSM6DS.cpp Adafruit LSM6DS 6-DoF Accelerometer
 *  and Gyroscope library
 *
 *  @section intro_sec Introduction
 *
 * 	I2C Driver base for Adafruit LSM6DS 6-DoF Accelerometer
 *      and Gyroscope libraries
 *
 * 	Adafruit invests time and resources providing this open source code,
 *  please support Adafruit and open-source hardware by purchasing products from
 * 	Adafruit!
 *
 *  @section dependencies Dependencies
 *  This library depends on the Adafruit BusIO library
 *
 *  This library depends on the Adafruit Unified Sensor library

 *  @section author Author
 *
 *  Bryan Siepert for Adafruit Industries
 *
 * 	@section license License
 *
 * 	BSD (see license.txt)
 *
 * 	@section  HISTORY
 *
 *     v1.0 - First release
 */

#include "Arduino.h"
#include <Wire.h>

#include "Adafruit_LSM6DS.h"

static const float _data_rate_arr[] = {
    [LSM6DS_RATE_SHUTDOWN] = 0.0f,    [LSM6DS_RATE_12_5_HZ] = 12.5f,
    [LSM6DS_RATE_26_HZ] = 26.0f,      [LSM6DS_RATE_52_HZ] = 52.0f,
    [LSM6DS_RATE_104_HZ] = 104.0f,    [LSM6DS_RATE_208_HZ] = 208.0f,
    [LSM6DS_RATE_416_HZ] = 416.0f,    [LSM6DS_RATE_833_HZ] = 833.0f,
    [LSM6DS_RATE_1_66K_HZ] = 1660.0f, [LSM6DS_RATE_3_33K_HZ] = 3330.0f,
    [LSM6DS_RATE_6_66K_HZ] = 6660.0f,
};

/*!
 *    @brief  Instantiates a new LSM6DS class
 */
Adafruit_LSM6DS::Adafruit_LSM6DS(void) {}

/*!
 *    @brief  Cleans up the LSM6DS
 */
Adafruit_LSM6DS::~Adafruit_LSM6DS(void) { delete temp_sensor; }

/*!  @brief  Unique subclass initializer post i2c/spi init
 *   @param sensor_id Optional unique ID for the sensor set
 *   @returns True if chip identified and initialized
 */
bool Adafruit_LSM6DS::_init(int32_t sensor_id) {
  (void)sensor_id;

  // Enable accelerometer with 104 Hz data rate, 4G
  setAccelDataRate(LSM6DS_RATE_104_HZ);
  setAccelRange(LSM6DS_ACCEL_RANGE_4_G);

  // Enable gyro with 104 Hz data rate, 2000 dps
  setGyroDataRate(LSM6DS_RATE_104_HZ);
  setGyroRange(LSM6DS_GYRO_RANGE_2000_DPS);

  delay(10);

  // delete objects if sensor is reinitialized
  delete temp_sensor;
  delete accel_sensor;
  delete gyro_sensor;

  temp_sensor = new Adafruit_LSM6DS_Temp(this);
  accel_sensor = new Adafruit_LSM6DS_Accelerometer(this);
  gyro_sensor = new Adafruit_LSM6DS_Gyro(this);

  return false;
};

/*!
 *    @brief  Read chip identification register
 *    @returns 8 Bit value from WHOAMI register
 */
uint8_t Adafruit_LSM6DS::chipID(void) {
  Adafruit_BusIO_Register chip_id = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_WHOAMI);
  // Serial.print("Read ID 0x"); Serial.println(chip_id.read(), HEX);

  // make sure we're talking to the right chip
  return chip_id.read();
}

/*!
 *    @brief  Read Status register
 *    @returns 8 Bit value from Status register
 */
uint8_t Adafruit_LSM6DS::status(void) {
  Adafruit_BusIO_Register status_reg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_STATUS_REG);
  return status_reg.read();
}

/*!
 *    @brief  Sets up the hardware and initializes I2C
 *    @param  i2c_address
 *            The I2C address to be used.
 *    @param  wire
 *            The Wire object to be used for I2C connections.
 *    @param  sensor_id
 *            The user-defined ID to differentiate different sensors
 *    @return True if initialization was successful, otherwise false.
 */
boolean Adafruit_LSM6DS::begin_I2C(uint8_t i2c_address, TwoWire *wire,
                                   int32_t sensor_id) {
  delete i2c_dev; // remove old interface

  i2c_dev = new Adafruit_I2CDevice(i2c_address, wire);

  if (!i2c_dev->begin()) {
    return false;
  }

  return _init(sensor_id);
}

/*!
 *    @brief  Sets up the hardware and initializes hardware SPI
 *    @param  cs_pin The arduino pin # connected to chip select
 *    @param  theSPI The SPI object to be used for SPI connections.
 *    @param  frequency The SPI bus frequency
 *    @param  sensor_id
 *            The user-defined ID to differentiate different sensors
 *    @return True if initialization was successful, otherwise false.
 */
bool Adafruit_LSM6DS::begin_SPI(uint8_t cs_pin, SPIClass *theSPI,
                                int32_t sensor_id, uint32_t frequency) {
  i2c_dev = NULL;

  delete spi_dev; // remove old interface

  spi_dev = new Adafruit_SPIDevice(cs_pin,
                                   frequency,             // frequency
                                   SPI_BITORDER_MSBFIRST, // bit order
                                   SPI_MODE0,             // data mode
                                   theSPI);
  if (!spi_dev->begin()) {
    return false;
  }

  return _init(sensor_id);
}

/*!
 *    @brief  Sets up the hardware and initializes software SPI
 *    @param  cs_pin The arduino pin # connected to chip select
 *    @param  sck_pin The arduino pin # connected to SPI clock
 *    @param  miso_pin The arduino pin # connected to SPI MISO
 *    @param  mosi_pin The arduino pin # connected to SPI MOSI
 *    @param  frequency The SPI bus frequency
 *    @param  sensor_id
 *            The user-defined ID to differentiate different sensors
 *    @return True if initialization was successful, otherwise false.
 */
bool Adafruit_LSM6DS::begin_SPI(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin,
                                int8_t mosi_pin, int32_t sensor_id,
                                uint32_t frequency) {
  i2c_dev = NULL;

  delete spi_dev; // remove old interface

  spi_dev = new Adafruit_SPIDevice(cs_pin, sck_pin, miso_pin, mosi_pin,
                                   frequency,             // frequency
                                   SPI_BITORDER_MSBFIRST, // bit order
                                   SPI_MODE0);            // data mode
  if (!spi_dev->begin()) {
    return false;
  }

  return _init(sensor_id);
}

/**************************************************************************/
/*!
    @brief Resets the sensor to its power-on state, clearing all registers and
   memory
*/
void Adafruit_LSM6DS::reset(void) {

  Adafruit_BusIO_Register ctrl3 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL3_C);

  Adafruit_BusIO_RegisterBits sw_reset =
      Adafruit_BusIO_RegisterBits(&ctrl3, 1, 0);

  // Adafruit_BusIO_RegisterBits boot = Adafruit_BusIO_RegisterBits(&ctrl3, 1,
  // 7);

  sw_reset.write(true);

  while (sw_reset.read()) {
    delay(1);
  }
}

/*!
    @brief  Gets an Adafruit Unified Sensor object for the temp sensor component
    @return Adafruit_Sensor pointer to temperature sensor
 */
Adafruit_Sensor *Adafruit_LSM6DS::getTemperatureSensor(void) {
  return temp_sensor;
}

/*!
    @brief  Gets an Adafruit Unified Sensor object for the accelerometer
    sensor component
    @return Adafruit_Sensor pointer to accelerometer sensor
 */
Adafruit_Sensor *Adafruit_LSM6DS::getAccelerometerSensor(void) {
  return accel_sensor;
}

/*!
    @brief  Gets an Adafruit Unified Sensor object for the gyro sensor component
    @return Adafruit_Sensor pointer to gyro sensor
 */
Adafruit_Sensor *Adafruit_LSM6DS::getGyroSensor(void) { return gyro_sensor; }

/**************************************************************************/
/*!
    @brief  Gets the most recent sensor event, Adafruit Unified Sensor format
    @param  accel
            Pointer to an Adafruit Unified sensor_event_t object to be filled
            with acceleration event data.

    @param  gyro
            Pointer to an Adafruit Unified sensor_event_t object to be filled
            with gyro event data.

    @param  temp
            Pointer to an Adafruit Unified sensor_event_t object to be filled
            with temperature event data.

    @return True on successful read
*/
/**************************************************************************/
bool Adafruit_LSM6DS::getEvent(sensors_event_t *accel, sensors_event_t *gyro,
                               sensors_event_t *temp) {
  uint32_t t = millis();
  _read();

  // use helpers to fill in the events
  fillAccelEvent(accel, t);
  fillGyroEvent(gyro, t);
  fillTempEvent(temp, t);
  return true;
}

void Adafruit_LSM6DS::fillTempEvent(sensors_event_t *temp, uint32_t timestamp) {
  memset(temp, 0, sizeof(sensors_event_t));
  temp->version = sizeof(sensors_event_t);
  temp->sensor_id = _sensorid_temp;
  temp->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
  temp->timestamp = timestamp;
  temp->temperature = temperature;
}

void Adafruit_LSM6DS::fillGyroEvent(sensors_event_t *gyro, uint32_t timestamp) {
  memset(gyro, 0, sizeof(sensors_event_t));
  gyro->version = 1;
  gyro->sensor_id = _sensorid_gyro;
  gyro->type = SENSOR_TYPE_GYROSCOPE;
  gyro->timestamp = timestamp;
  gyro->gyro.x = gyroX;
  gyro->gyro.y = gyroY;
  gyro->gyro.z = gyroZ;
}

void Adafruit_LSM6DS::fillAccelEvent(sensors_event_t *accel,
                                     uint32_t timestamp) {
  memset(accel, 0, sizeof(sensors_event_t));
  accel->version = 1;
  accel->sensor_id = _sensorid_accel;
  accel->type = SENSOR_TYPE_ACCELEROMETER;
  accel->timestamp = timestamp;
  accel->acceleration.x = accX;
  accel->acceleration.y = accY;
  accel->acceleration.z = accZ;
}

/**************************************************************************/
/*!
    @brief Gets the accelerometer data rate.
    @returns The the accelerometer data rate.
*/
lsm6ds_data_rate_t Adafruit_LSM6DS::getAccelDataRate(void) {

  Adafruit_BusIO_Register ctrl1 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL1_XL);

  Adafruit_BusIO_RegisterBits accel_data_rate =
      Adafruit_BusIO_RegisterBits(&ctrl1, 4, 4);

  return (lsm6ds_data_rate_t)accel_data_rate.read();
}

/**************************************************************************/
/*!
    @brief Sets the accelerometer data rate.
    @param  data_rate
            The the accelerometer data rate. Must be a `lsm6ds_data_rate_t`.
*/
void Adafruit_LSM6DS::setAccelDataRate(lsm6ds_data_rate_t data_rate) {

  Adafruit_BusIO_Register ctrl1 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL1_XL);

  Adafruit_BusIO_RegisterBits accel_data_rate =
      Adafruit_BusIO_RegisterBits(&ctrl1, 4, 4);

  accel_data_rate.write(data_rate);
}

/**************************************************************************/
/*!
    @brief Gets the accelerometer measurement range.
    @returns The the accelerometer measurement range.
*/
lsm6ds_accel_range_t Adafruit_LSM6DS::getAccelRange(void) {

  Adafruit_BusIO_Register ctrl1 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL1_XL);

  Adafruit_BusIO_RegisterBits accel_range =
      Adafruit_BusIO_RegisterBits(&ctrl1, 2, 2);

  accelRangeBuffered = (lsm6ds_accel_range_t)accel_range.read();

  return accelRangeBuffered;
}
/**************************************************************************/
/*!
    @brief Sets the accelerometer measurement range.
    @param new_range The `lsm6ds_accel_range_t` range to set.
*/
void Adafruit_LSM6DS::setAccelRange(lsm6ds_accel_range_t new_range) {

  Adafruit_BusIO_Register ctrl1 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL1_XL);

  Adafruit_BusIO_RegisterBits accel_range =
      Adafruit_BusIO_RegisterBits(&ctrl1, 2, 2);

  accel_range.write(new_range);

  accelRangeBuffered = new_range;
}

/**************************************************************************/
/*!
    @brief Gets the gyro data rate.
    @returns The the gyro data rate.
*/
lsm6ds_data_rate_t Adafruit_LSM6DS::getGyroDataRate(void) {

  Adafruit_BusIO_Register ctrl2 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL2_G);

  Adafruit_BusIO_RegisterBits gyro_data_rate =
      Adafruit_BusIO_RegisterBits(&ctrl2, 4, 4);

  return (lsm6ds_data_rate_t)gyro_data_rate.read();
}

/**************************************************************************/
/*!
    @brief Sets the gyro data rate.
    @param  data_rate
            The the gyro data rate. Must be a `lsm6ds_data_rate_t`.
*/
void Adafruit_LSM6DS::setGyroDataRate(lsm6ds_data_rate_t data_rate) {

  Adafruit_BusIO_Register ctrl2 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL2_G);

  Adafruit_BusIO_RegisterBits gyro_data_rate =
      Adafruit_BusIO_RegisterBits(&ctrl2, 4, 4);

  gyro_data_rate.write(data_rate);
}

/**************************************************************************/
/*!
    @brief Gets the gyro range.
    @returns The the gyro range.
*/
lsm6ds_gyro_range_t Adafruit_LSM6DS::getGyroRange(void) {

  Adafruit_BusIO_Register ctrl2 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL2_G);

  Adafruit_BusIO_RegisterBits gyro_range =
      Adafruit_BusIO_RegisterBits(&ctrl2, 4, 0);

  gyroRangeBuffered = (lsm6ds_gyro_range_t)gyro_range.read();

  return gyroRangeBuffered;
}

/**************************************************************************/
/*!
    @brief Sets the gyro range.
    @param new_range The `lsm6ds_gyro_range_t` to set.
*/
void Adafruit_LSM6DS::setGyroRange(lsm6ds_gyro_range_t new_range) {

  Adafruit_BusIO_Register ctrl2 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL2_G);

  Adafruit_BusIO_RegisterBits gyro_range =
      Adafruit_BusIO_RegisterBits(&ctrl2, 4, 0);

  gyro_range.write(new_range);

  gyroRangeBuffered = new_range;
}

/**************************************************************************/
/*!
    @brief Enables the high pass filter and/or slope filter
    @param filter_enabled Whether to enable the slope filter (see datasheet)
    @param filter The lsm6ds_hp_filter_t that sets the data rate divisor
*/
/**************************************************************************/
void Adafruit_LSM6DS::highPassFilter(bool filter_enabled,
                                     lsm6ds_hp_filter_t filter) {
  Adafruit_BusIO_Register ctrl8 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL8_XL);
  Adafruit_BusIO_RegisterBits HPF_en =
      Adafruit_BusIO_RegisterBits(&ctrl8, 1, 2);
  Adafruit_BusIO_RegisterBits HPF_filter =
      Adafruit_BusIO_RegisterBits(&ctrl8, 2, 5);
  HPF_en.write(filter_enabled);
  HPF_filter.write(filter);
}

/******************* Adafruit_Sensor functions *****************/
/*!
 *     @brief  Updates the measurement data for all sensors simultaneously
 */
/**************************************************************************/
void Adafruit_LSM6DS::_read(void) {
  // get raw readings
  Adafruit_BusIO_Register data_reg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_OUT_TEMP_L, 14);

  uint8_t buffer[14];
  data_reg.read(buffer, 14);

  rawTemp = buffer[1] << 8 | buffer[0];
  temperature = (rawTemp / temperature_sensitivity) + 25.0;

  rawGyroX = buffer[3] << 8 | buffer[2];
  rawGyroY = buffer[5] << 8 | buffer[4];
  rawGyroZ = buffer[7] << 8 | buffer[6];

  rawAccX = buffer[9] << 8 | buffer[8];
  rawAccY = buffer[11] << 8 | buffer[10];
  rawAccZ = buffer[13] << 8 | buffer[12];

  float gyro_scale = 1; // range is in milli-dps per bit!
  switch (gyroRangeBuffered) {
  case ISM330DHCX_GYRO_RANGE_4000_DPS:
    gyro_scale = 140.0;
    break;
  case LSM6DS_GYRO_RANGE_2000_DPS:
    gyro_scale = 70.0;
    break;
  case LSM6DS_GYRO_RANGE_1000_DPS:
    gyro_scale = 35.0;
    break;
  case LSM6DS_GYRO_RANGE_500_DPS:
    gyro_scale = 17.50;
    break;
  case LSM6DS_GYRO_RANGE_250_DPS:
    gyro_scale = 8.75;
    break;
  case LSM6DS_GYRO_RANGE_125_DPS:
    gyro_scale = 4.375;
    break;
  }

  gyroX = rawGyroX * gyro_scale * SENSORS_DPS_TO_RADS / 1000.0;
  gyroY = rawGyroY * gyro_scale * SENSORS_DPS_TO_RADS / 1000.0;
  gyroZ = rawGyroZ * gyro_scale * SENSORS_DPS_TO_RADS / 1000.0;

  float accel_scale = 1; // range is in milli-g per bit!
  switch (accelRangeBuffered) {
  case LSM6DS_ACCEL_RANGE_16_G:
    accel_scale = 0.488;
    break;
  case LSM6DS_ACCEL_RANGE_8_G:
    accel_scale = 0.244;
    break;
  case LSM6DS_ACCEL_RANGE_4_G:
    accel_scale = 0.122;
    break;
  case LSM6DS_ACCEL_RANGE_2_G:
    accel_scale = 0.061;
    break;
  }

  accX = rawAccX * accel_scale * SENSORS_GRAVITY_STANDARD / 1000;
  accY = rawAccY * accel_scale * SENSORS_GRAVITY_STANDARD / 1000;
  accZ = rawAccZ * accel_scale * SENSORS_GRAVITY_STANDARD / 1000;
}

/**************************************************************************/
/*!
    @brief Sets the INT1 and INT2 pin activation mode
    @param active_low true to set the pins  as active high, false to set the
   mode to active low
    @param open_drain true to set the pin mode as open-drain, false to set the
   mode to push-pull
*/
void Adafruit_LSM6DS::configIntOutputs(bool active_low, bool open_drain) {

  Adafruit_BusIO_Register ctrl3 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL3_C);
  Adafruit_BusIO_RegisterBits ppod_bits =
      Adafruit_BusIO_RegisterBits(&ctrl3, 2, 4);

  ppod_bits.write((active_low << 1) | open_drain);
}

/**************************************************************************/
/*!
    @brief Enables and disables the data ready interrupt on INT 1.
    @param drdy_temp true to output the data ready temperature interrupt
    @param drdy_g true to output the data ready gyro interrupt
    @param drdy_xl true to output the data ready accelerometer interrupt
    @param step_detect true to output the step detection interrupt (default off)
    @param wakeup true to output the wake up interrupt (default off)
*/
void Adafruit_LSM6DS::configInt1(bool drdy_temp, bool drdy_g, bool drdy_xl,
                                 bool step_detect, bool wakeup) {

  Adafruit_BusIO_Register int1_ctrl = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_INT1_CTRL);

  int1_ctrl.write((step_detect << 7) | (drdy_temp << 2) | (drdy_g << 1) |
                  drdy_xl);

  Adafruit_BusIO_Register md1cfg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_MD1_CFG);

  Adafruit_BusIO_RegisterBits wu = Adafruit_BusIO_RegisterBits(&md1cfg, 1, 5);
  wu.write(wakeup);
}

/**************************************************************************/
/*!
    @brief Enables and disables the data ready interrupt on INT 2.
    @param drdy_temp true to output the data ready temperature interrupt
    @param drdy_g true to output the data ready gyro interrupt
    @param drdy_xl true to output the data ready accelerometer interrupt
*/
void Adafruit_LSM6DS::configInt2(bool drdy_temp, bool drdy_g, bool drdy_xl) {

  Adafruit_BusIO_Register int2_ctrl = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_INT2_CTRL);

  Adafruit_BusIO_RegisterBits int2_drdy_bits =
      Adafruit_BusIO_RegisterBits(&int2_ctrl, 3, 0);

  int2_drdy_bits.write((drdy_temp << 2) | (drdy_g << 1) | drdy_xl);
}

/**************************************************************************/
/*!
    @brief  Gets the sensor_t data for the LSM6DS's gyroscope sensor
*/
/**************************************************************************/
void Adafruit_LSM6DS_Gyro::getSensor(sensor_t *sensor) {
  /* Clear the sensor_t object */
  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */
  strncpy(sensor->name, "LSM6DS_G", sizeof(sensor->name) - 1);
  sensor->name[sizeof(sensor->name) - 1] = 0;
  sensor->version = 1;
  sensor->sensor_id = _sensorID;
  sensor->type = SENSOR_TYPE_GYROSCOPE;
  sensor->min_delay = 0;
  sensor->min_value = -34.91; /* -2000 dps -> rad/s (radians per second) */
  sensor->max_value = +34.91;
  sensor->resolution = 7.6358e-5; /* 4.375 mdps -> rad/s */
}

/**************************************************************************/
/*!
    @brief  Gets the gyroscope as a standard sensor event
    @param  event Sensor event object that will be populated
    @returns True
*/
/**************************************************************************/
bool Adafruit_LSM6DS_Gyro::getEvent(sensors_event_t *event) {
  _theLSM6DS->_read();
  _theLSM6DS->fillGyroEvent(event, millis());

  return true;
}

/**************************************************************************/
/*!
    @brief  Gets the sensor_t data for the LSM6DS's accelerometer
*/
/**************************************************************************/
void Adafruit_LSM6DS_Accelerometer::getSensor(sensor_t *sensor) {
  /* Clear the sensor_t object */
  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */
  strncpy(sensor->name, "LSM6DS_A", sizeof(sensor->name) - 1);
  sensor->name[sizeof(sensor->name) - 1] = 0;
  sensor->version = 1;
  sensor->sensor_id = _sensorID;
  sensor->type = SENSOR_TYPE_ACCELEROMETER;
  sensor->min_delay = 0;
  sensor->min_value = -156.9064F; /*  -16g = 156.9064 m/s^2  */
  sensor->max_value = 156.9064F;  /* 16g = 156.9064 m/s^2  */
  sensor->resolution = 0.061;     /* 0.061 mg/LSB at +-2g */
}

/**************************************************************************/
/*!
    @brief  Gets the accelerometer as a standard sensor event
    @param  event Sensor event object that will be populated
    @returns True
*/
/**************************************************************************/
bool Adafruit_LSM6DS_Accelerometer::getEvent(sensors_event_t *event) {
  _theLSM6DS->_read();
  _theLSM6DS->fillAccelEvent(event, millis());

  return true;
}

/**************************************************************************/
/*!
    @brief  Gets the sensor_t data for the LSM6DS's tenperature
*/
/**************************************************************************/
void Adafruit_LSM6DS_Temp::getSensor(sensor_t *sensor) {
  /* Clear the sensor_t object */
  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */
  strncpy(sensor->name, "LSM6DS_T", sizeof(sensor->name) - 1);
  sensor->name[sizeof(sensor->name) - 1] = 0;
  sensor->version = 1;
  sensor->sensor_id = _sensorID;
  sensor->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
  sensor->min_delay = 0;
  sensor->min_value = -40;
  sensor->max_value = 85;
  sensor->resolution = 1; /* not a great sensor */
}

/**************************************************************************/
/*!
    @brief  Gets the temperature as a standard sensor event
    @param  event Sensor event object that will be populated
    @returns True
*/
/**************************************************************************/
bool Adafruit_LSM6DS_Temp::getEvent(sensors_event_t *event) {
  _theLSM6DS->_read();
  _theLSM6DS->fillTempEvent(event, millis());

  return true;
}

/**************************************************************************/
/*!
    @brief Enables and disables the pedometer function
    @param enable True to turn on the pedometer function, false to turn off
*/
/**************************************************************************/
void Adafruit_LSM6DS::enablePedometer(bool enable) {
  // enable or disable step counter
  Adafruit_BusIO_Register tapcfg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_TAP_CFG);
  Adafruit_BusIO_RegisterBits pedo_en =
      Adafruit_BusIO_RegisterBits(&tapcfg, 1, 6);
  pedo_en.write(enable);

  // enable or disable functionality
  Adafruit_BusIO_Register ctrl10 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL10_C);
  Adafruit_BusIO_RegisterBits func_en =
      Adafruit_BusIO_RegisterBits(&ctrl10, 1, 2);
  func_en.write(enable);

  resetPedometer();
}

/**************************************************************************/
/*!
    @brief Enables and disables the wakeup function
    @param enable True to turn on the wakeup function, false to turn off
    @param duration How many > threshold readings to generate a wakeup
    @param thresh The threshold (sensitivity)
*/
/**************************************************************************/
void Adafruit_LSM6DS::enableWakeup(bool enable, uint8_t duration,
                                   uint8_t thresh) {
  // enable or disable functionality
  Adafruit_BusIO_Register tapcfg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_TAP_CFG);
  Adafruit_BusIO_RegisterBits slope_en =
      Adafruit_BusIO_RegisterBits(&tapcfg, 1, 4);
  Adafruit_BusIO_RegisterBits timer_en =
      Adafruit_BusIO_RegisterBits(&tapcfg, 1, 7);
  slope_en.write(enable);
  timer_en.write(enable);
  if (enable) {
    Adafruit_BusIO_Register wake_dur = Adafruit_BusIO_Register(
        i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_WAKEUP_DUR);
    Adafruit_BusIO_RegisterBits durbits =
        Adafruit_BusIO_RegisterBits(&wake_dur, 2, 5);
    durbits.write(duration);

    Adafruit_BusIO_Register wake_ths = Adafruit_BusIO_Register(
        i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_WAKEUP_THS);
    Adafruit_BusIO_RegisterBits thsbits =
        Adafruit_BusIO_RegisterBits(&wake_ths, 6, 0);
    thsbits.write(thresh);
  }
}

/**************************************************************************/
/*!
    @brief Checks interrupt register to see if we have a wake signal
    @returns True if wake event bit is set in WAKEUP_SRC (cleared on read)
*/
/**************************************************************************/
bool Adafruit_LSM6DS::awake(void) {
  Adafruit_BusIO_Register wakesrc = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_WAKEUP_SRC);
  Adafruit_BusIO_RegisterBits wake_evt =
      Adafruit_BusIO_RegisterBits(&wakesrc, 1, 3);
  return wake_evt.read();
}

/**************************************************************************/
/*!
    @brief Simple shake detection. Must call enableWakeup() first.
    @returns True if shake (wake) detected, otherwise false.
*/
/**************************************************************************/
bool Adafruit_LSM6DS::shake(void) {
  Adafruit_BusIO_Register tapcfg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_TAP_CFG);
  Adafruit_BusIO_RegisterBits slope_en =
      Adafruit_BusIO_RegisterBits(&tapcfg, 1, 4);
  Adafruit_BusIO_RegisterBits timer_en =
      Adafruit_BusIO_RegisterBits(&tapcfg, 1, 7);
  // only check if enabled
  if (slope_en.read() && timer_en.read()) {
    return awake();
  }
  return false;
}

/**************************************************************************/
/*!
    @brief Reset the pedometer count
*/
/**************************************************************************/
void Adafruit_LSM6DS::resetPedometer(void) {
  // reset bit to clear counter
  Adafruit_BusIO_Register ctrl10 = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL10_C);
  Adafruit_BusIO_RegisterBits ped_rst =
      Adafruit_BusIO_RegisterBits(&ctrl10, 1, 1);
  ped_rst.write(true);
}

/**************************************************************************/
/*!
    @brief Read the 16-bit pedometer count
    @returns The value from the step counter
*/
/**************************************************************************/
uint16_t Adafruit_LSM6DS::readPedometer(void) {
  Adafruit_BusIO_Register steps_reg = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_STEPCOUNTER, 2);
  return steps_reg.read();
}

/**************************************************************************/
/*!
    @brief Gets the accelerometer data rate.
    @returns The data rate in float
*/
float Adafruit_LSM6DS::accelerationSampleRate(void) {
  return _data_rate_arr[this->getAccelDataRate()];
}

/**************************************************************************/
/*!
    @brief Check for available data from accelerometer
    @returns 1 if available, 0 if not
*/
int Adafruit_LSM6DS::accelerationAvailable(void) {
  return (this->status() & 0x01) ? 1 : 0;
}

/**************************************************************************/
/*!
    @brief Read accelerometer data
    @param x reference to x axis
    @param y reference to y axis
    @param z reference to z axis
    @returns 1 if success, 0 if not
*/
int Adafruit_LSM6DS::readAcceleration(float &x, float &y, float &z) {
  int16_t data[3];

  Adafruit_BusIO_Register accel_data = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_OUTX_L_A, 6);

  if (!accel_data.read((uint8_t *)data, sizeof(data))) {
    x = y = z = NAN;
    return 0;
  }

  // scale to range of -4 – 4
  x = data[0] * 4.0 / 32768.0;
  y = data[1] * 4.0 / 32768.0;
  z = data[2] * 4.0 / 32768.0;

  return 1;
}

/**************************************************************************/
/*!
    @brief Get the gyroscope data rate.
    @returns The data rate in float
*/
float Adafruit_LSM6DS::gyroscopeSampleRate(void) {
  return _data_rate_arr[this->getGyroDataRate()];
}

/**************************************************************************/
/*!
    @brief Check for available data from gyroscope
    @returns 1 if available, 0 if not
*/
int Adafruit_LSM6DS::gyroscopeAvailable(void) {
  return (this->status() & 0x02) ? 1 : 0;
}

/**************************************************************************/
/*!
    @brief Read gyroscope data
    @param x reference to x axis
    @param y reference to y axis
    @param z reference to z axis
    @returns 1 if success, 0 if not
*/
int Adafruit_LSM6DS::readGyroscope(float &x, float &y, float &z) {
  int16_t data[3];

  Adafruit_BusIO_Register gyro_data = Adafruit_BusIO_Register(
      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_OUTX_L_G, 6);

  if (!gyro_data.read((uint8_t *)data, sizeof(data))) {
    x = y = z = NAN;
    return 0;
  }

  // scale to range of -2000 – 2000
  x = data[0] * 2000.0 / 32768.0;
  y = data[1] * 2000.0 / 32768.0;
  z = data[2] * 2000.0 / 32768.0;

  return 1;
}