#include "lsm6ds_base.h"
#include "esphome/core/log.h"
#include "esphome/core/helpers.h"
#include "esphome/core/hal.h"

namespace esphome {
namespace lsm6ds_base {

static const char *const TAG = "lsm6ds";

void LSM6DSComponent::setup() {
  uint8_t who_am_i;
  if (!this->read_register(0x0F, &who_am_i, 1)) {
    this->mark_failed();
    ESP_LOGE(TAG, "Communication failed");
    return;
  }

  if (who_am_i == 0x6C) {
    ESP_LOGI(TAG, "Detected LSM6DSOX");
  } else if (who_am_i == 0x6A) {
    ESP_LOGI(TAG, "Detected LSM6DS3TR-C/LSM6DSM");
  } else if (who_am_i == 0x69) {
    ESP_LOGI(TAG, "Detected LSM6DSO32TR");
  } else {
    this->mark_failed();
    ESP_LOGE(TAG, "Unknown chip ID: 0x%02X", who_am_i);
    return;
  }

  // SW Reset
  this->write_register(0x12, 0x01);
  uint8_t ctrl3;

  // Use esphome:: namespace for framework compatibility
  uint32_t start = esphome::millis();
  do {
    esphome::delay(10);
    this->read_register(0x12, &ctrl3, 1);
  } while ((ctrl3 & 0x01) && (esphome::millis() - start < 100));

  if (ctrl3 & 0x01) {
    ESP_LOGW(TAG, "SW Reset timed out");
  }

  // Enable BDU (Block Data Update)
  this->write_register(0x12, 0x40 | 0x04);  // BDU=1, IF_INC=1

  // Configure Accel
  uint8_t ctrl1_xl = (this->accel_odr_ << 4) | (this->accel_range_ << 2);
  this->write_register(0x10, ctrl1_xl);

  switch (this->accel_range_) {
    case LSM6DS_ACCEL_RANGE_2G:
      this->accel_sensitivity_ = 0.061e-3f;
      break;
    case LSM6DS_ACCEL_RANGE_4G:
      this->accel_sensitivity_ = 0.122e-3f;
      break;
    case LSM6DS_ACCEL_RANGE_8G:
      this->accel_sensitivity_ = 0.244e-3f;
      break;
    case LSM6DS_ACCEL_RANGE_16G:
      this->accel_sensitivity_ = 0.488e-3f;
      break;
    default:
      this->accel_sensitivity_ = 0;
      break;
  }

  // Configure Gyro
  uint8_t ctrl2_g = (this->gyro_odr_ << 4);
  if (this->gyro_range_ == LSM6DS_GYRO_RANGE_125DPS) {
    ctrl2_g |= 0x02;  // FS_125 = 1
    this->gyro_sensitivity_ = 4.375e-3f;
  } else {
    ctrl2_g |= (this->gyro_range_ << 2);
    switch (this->gyro_range_) {
      case LSM6DS_GYRO_RANGE_250DPS:
        this->gyro_sensitivity_ = 8.75e-3f;
        break;
      case LSM6DS_GYRO_RANGE_500DPS:
        this->gyro_sensitivity_ = 17.50e-3f;
        break;
      case LSM6DS_GYRO_RANGE_1000DPS:
        this->gyro_sensitivity_ = 35.0e-3f;
        break;
      case LSM6DS_GYRO_RANGE_2000DPS:
        this->gyro_sensitivity_ = 70.0e-3f;
        break;
      default:
        this->gyro_sensitivity_ = 0;
        break;
    }
  }
  this->write_register(0x11, ctrl2_g);
}

void LSM6DSComponent::update() {
  uint8_t status;
  if (!this->read_register(0x1E, &status, 1))
    return;

  if (this->gyro_odr_ != LSM6DS_ODR_OFF && (status & 0x01)) {  // GDA - Gyroscope data available
    uint8_t data[6];
    if (this->read_register(0x22, data, 6)) {
      int16_t gx = encode_uint16(data[1], data[0]);
      int16_t gy = encode_uint16(data[3], data[2]);
      int16_t gz = encode_uint16(data[5], data[4]);
      if (this->gyro_x_sensor_)
        this->gyro_x_sensor_->publish_state(gx * this->gyro_sensitivity_);
      if (this->gyro_y_sensor_)
        this->gyro_y_sensor_->publish_state(gy * this->gyro_sensitivity_);
      if (this->gyro_z_sensor_)
        this->gyro_z_sensor_->publish_state(gz * this->gyro_sensitivity_);
    }
  }

  if (this->accel_odr_ != LSM6DS_ODR_OFF && (status & 0x02)) {  // XLDA - Accelerometer data available
    uint8_t data[6];
    if (this->read_register(0x28, data, 6)) {
      int16_t ax = encode_uint16(data[1], data[0]);
      int16_t ay = encode_uint16(data[3], data[2]);
      int16_t az = encode_uint16(data[5], data[4]);
      float grav = 9.80665f;
      if (this->accel_x_sensor_)
        this->accel_x_sensor_->publish_state(ax * this->accel_sensitivity_ * grav);
      if (this->accel_y_sensor_)
        this->accel_y_sensor_->publish_state(ay * this->accel_sensitivity_ * grav);
      if (this->accel_z_sensor_)
        this->accel_z_sensor_->publish_state(az * this->accel_sensitivity_ * grav);
    }
  }

  if (status & 0x04) {  // TDA - Temperature data available
    uint8_t data[2];
    if (this->read_register(0x20, data, 2)) {
      int16_t temp = encode_uint16(data[1], data[0]);
      if (this->temperature_sensor_)
        this->temperature_sensor_->publish_state(25.0f + (temp / 256.0f));
    }
  }
}

void LSM6DSComponent::dump_config() {
  ESP_LOGCONFIG(TAG, "LSM6DS (LSM6DSOX/LSM6DS3TR-C/LSM6DSO32TR):");
  LOG_UPDATE_INTERVAL(this);
  if (this->is_failed()) {
    ESP_LOGCONFIG(TAG, "  Connection failed!");
    return;
  }
  LOG_SENSOR("  ", "Accel X", this->accel_x_sensor_);
  LOG_SENSOR("  ", "Accel Y", this->accel_y_sensor_);
  LOG_SENSOR("  ", "Accel Z", this->accel_z_sensor_);
  LOG_SENSOR("  ", "Gyro X", this->gyro_x_sensor_);
  LOG_SENSOR("  ", "Gyro Y", this->gyro_y_sensor_);
  LOG_SENSOR("  ", "Gyro Z", this->gyro_z_sensor_);
  LOG_SENSOR("  ", "Temperature", this->temperature_sensor_);
}

}  // namespace lsm6ds_base
}  // namespace esphome