#include "LSM6DSL.h"
 
bool LSM6DSL::begin() {
    Wire.begin(LSM6DSL_SDA_PIN, LSM6DSL_SCL_PIN);
 
    delay(50);
 
    // Enable accelerometer @208 Hz, ±2g, BW ~100 Hz:
    // CTRL1_XL (0x10):
    //   ODR_XL[3:0]=0b0101 => 208 Hz
    //   FS_XL[1:0]=00 => ±2g
    //   BW_XL[1:0]=00 => ~100Hz
    // Combined: 0x50
    if (!writeReg(LSM6DSL_CTRL1_XL, 0x50)) return false;
 
    // Turn gyro off for power saving: CTRL2_G = 0x00 (power-down)
    if (!writeReg(LSM6DSL_CTRL2_G, 0x00)) return false;
 
    // CTRL3_C (0x12):
    // Bit6 BDU=1 (lock data until both bytes read)
    // Bit2 IF_INC=1 (auto-increment register addr)
    // 0x44 = 0b0100_0100
    if (!writeReg(LSM6DSL_CTRL3_C, 0x44)) return false;
 
    delay(50);
    return true;
}
 
bool LSM6DSL::readAccelG(float &ax_g, float &ay_g, float &az_g) {
    uint8_t raw[6];
    if (!readBytes(LSM6DSL_OUTX_L_XL, raw, 6)) return false;
 
    int16_t x_raw = (int16_t)(raw[0] | (raw[1] << 8));
    int16_t y_raw = (int16_t)(raw[2] | (raw[3] << 8));
    int16_t z_raw = (int16_t)(raw[4] | (raw[5] << 8));
 
    ax_g = (float)x_raw * LSM6DSL_ACC_SENS_2G_G_PER_LSB;
    ay_g = (float)y_raw * LSM6DSL_ACC_SENS_2G_G_PER_LSB;
    az_g = (float)z_raw * LSM6DSL_ACC_SENS_2G_G_PER_LSB;
    return true;
}
 
bool LSM6DSL::writeReg(uint8_t reg, uint8_t value) {
    Wire.beginTransmission(LSM6DSL_I2C_ADDR);
    Wire.write(reg);
    Wire.write(value);
    return (Wire.endTransmission() == 0);
}
 
bool LSM6DSL::readBytes(uint8_t startReg, uint8_t *buffer, size_t len) {
    Wire.beginTransmission(LSM6DSL_I2C_ADDR);
    Wire.write(startReg);
    if (Wire.endTransmission(false) != 0) { // repeated start
        return false;
    }
 
    size_t readLen = Wire.requestFrom((int)LSM6DSL_I2C_ADDR, (int)len);
    if (readLen != len) return false;
    for (size_t i = 0; i < len; i++) {
        buffer[i] = Wire.read();
    }
    return true;
}