/**
 * BMA456H integration for Powerpod (ESP-IDF I2C master + Bosch SensorAPI).
 *
 * Polls accelerometer at 10 Hz; tap events arrive on BMA456_INT_GPIO.
 * Accel logging is filtered in software (deadzone); slaves stream samples via ESP-NOW.
 */

#include "bosch456.h"
#include "bma4.h"
#include "bma4_defs.h"
#include "bma456h.h"
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "freertos/semphr.h"
#include <rom/ets_sys.h>
#include <string.h>

static const char *TAG = "[BMA456]";

#define BMA4_READ_WRITE_LEN UINT8_C(46)
#define BMA4_I2C_MAX_WRITE (1u + BMA4_READ_WRITE_LEN)

#define SENSOR_POLL_MS 100

static i2c_master_dev_handle_t s_bma456_dev;
static bool s_bma456_ready;
static struct bma4_dev s_bma456;
static uint32_t s_accel_deadzone = BMA456_DEFAULT_ACCEL_DEADZONE;
static int16_t s_last_x;
static int16_t s_last_y;
static int16_t s_last_z;
static bool s_have_last_sample;

static volatile bool s_int_pending;
static SemaphoreHandle_t s_accel_mutex;
static bma456_tap_handler_t s_tap_handler;
static void *s_tap_handler_ctx;
static const bma456_tap_config_t s_tap_config = BMA456_TAP_CONFIG_DEFAULT;

static esp_err_t check_bma4(const char *api_name, int8_t rslt);

/* Bosch SensorAPI platform hooks (intf_ptr → i2c_master_dev_handle_t *). */

BMA4_INTF_RET_TYPE bma4_i2c_read(uint8_t reg_addr, uint8_t *reg_data,
                                 uint32_t len, void *intf_ptr) {
  if (s_bma456_dev == NULL) {
    return BMA4_E_COM_FAIL;
  }
  esp_err_t err = i2c_master_transmit_receive(s_bma456_dev, &reg_addr, 1,
                                              reg_data, len, -1);
  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
}

BMA4_INTF_RET_TYPE bma4_i2c_write(uint8_t reg_addr, const uint8_t *reg_data,
                                  uint32_t len, void *intf_ptr) {
  (void)intf_ptr;
  if (s_bma456_dev == NULL || reg_data == NULL) {
    return BMA4_E_COM_FAIL;
  }
  if (len > BMA4_READ_WRITE_LEN) {
    return BMA4_E_COM_FAIL;
  }

  uint8_t buffer[BMA4_I2C_MAX_WRITE];
  buffer[0] = reg_addr;
  memcpy(&buffer[1], reg_data, len);

  esp_err_t err =
      i2c_master_transmit(s_bma456_dev, buffer, (size_t)(len + 1), -1);
  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
}

void bma4_delay_us(uint32_t period, void *intf_ptr) {
  (void)intf_ptr;
  uint32_t wait_ms = period / 1000;
  uint32_t wait_us = period % 1000;
  if (wait_ms > 0) {
    vTaskDelay(pdMS_TO_TICKS(wait_ms));
  }
  if (wait_us > 0) {
    ets_delay_us(wait_us);
  }
}

void bma4_error_codes_print_result(const char api_name[], int8_t rslt) {
  if (rslt == BMA4_OK) {
    return;
  }
  ESP_LOGW(TAG, "%s failed: %d", api_name, (int)rslt);
}

static esp_err_t check_bma4(const char *api_name, int8_t rslt) {
  if (rslt == BMA4_OK) {
    return ESP_OK;
  }
  bma4_error_codes_print_result(api_name, rslt);
  return ESP_FAIL;
}

static int16_t axis_delta(int16_t a, int16_t b) {
  int32_t d = (int32_t)a - (int32_t)b;
  return (int16_t)(d < 0 ? -d : d);
}

static bool sample_exceeds_deadzone(int16_t x, int16_t y, int16_t z) {
  if (!s_have_last_sample) {
    return true;
  }
  return axis_delta(x, s_last_x) > (int16_t)s_accel_deadzone ||
         axis_delta(y, s_last_y) > (int16_t)s_accel_deadzone ||
         axis_delta(z, s_last_z) > (int16_t)s_accel_deadzone;
}

bool bma456_is_ready(void) { return s_bma456_ready; }

void bma456_set_accel_deadzone(uint32_t deadzone_lsb) {
  s_accel_deadzone = deadzone_lsb;
  s_have_last_sample = false;
  if (s_bma456_ready) {
    ESP_LOGI(TAG, "accel deadzone %lu LSB", (unsigned long)deadzone_lsb);
  }
}

uint32_t bma456_get_accel_deadzone(void) { return s_accel_deadzone; }

void bma456_set_tap_handler(bma456_tap_handler_t handler, void *ctx) {
  s_tap_handler = handler;
  s_tap_handler_ctx = ctx;
}

esp_err_t bma456_read_accel(int16_t *x, int16_t *y, int16_t *z) {
  if (!s_bma456_ready || x == NULL || y == NULL || z == NULL) {
    return ESP_ERR_INVALID_STATE;
  }
  if (s_accel_mutex == NULL ||
      xSemaphoreTake(s_accel_mutex, pdMS_TO_TICKS(500)) != pdTRUE) {
    return ESP_ERR_TIMEOUT;
  }

  struct bma4_accel sens_data = {0};
  int8_t ret = bma4_read_accel_xyz(&sens_data, &s_bma456);
  xSemaphoreGive(s_accel_mutex);

  if (ret != BMA4_OK) {
    bma4_error_codes_print_result("bma4_read_accel_xyz", ret);
    return ESP_FAIL;
  }

  *x = sens_data.x;
  *y = sens_data.y;
  *z = sens_data.z;
  return ESP_OK;
}

void bma456_report_accel_if_changed(int16_t x, int16_t y, int16_t z) {
  if (!s_bma456_ready || !sample_exceeds_deadzone(x, y, z)) {
    return;
  }

  s_last_x = x;
  s_last_y = y;
  s_last_z = z;
  s_have_last_sample = true;

  ESP_LOGI(TAG, "ACC X=%d Y=%d Z=%d (deadzone %lu)", x, y, z,
           (unsigned long)s_accel_deadzone);
}

static void IRAM_ATTR bma456_int_isr(void *arg) {
  (void)arg;
  s_int_pending = true;
}

static void handle_tap_interrupt(void) {
  uint16_t int_status = 0;
  int8_t ret = bma456h_read_int_status(&int_status, &s_bma456);
  if (ret != BMA4_OK) {
    bma4_error_codes_print_result("bma456h_read_int_status", ret);
    return;
  }

  struct bma456h_out_state tap_out = {0};
  ret = bma456h_output_state(&tap_out, &s_bma456);
  if (ret != BMA4_OK) {
    bma4_error_codes_print_result("bma456h_output_state", ret);
    return;
  }

  if (tap_out.single_tap) {
    ESP_LOGI(TAG, "tap: single");
    if (s_tap_handler != NULL) {
      s_tap_handler(BMA456_TAP_SINGLE, s_tap_handler_ctx);
    }
  } else if (tap_out.double_tap) {
    ESP_LOGI(TAG, "tap: double");
    if (s_tap_handler != NULL) {
      s_tap_handler(BMA456_TAP_DOUBLE, s_tap_handler_ctx);
    }
  } else if (tap_out.triple_tap) {
    ESP_LOGI(TAG, "tap: triple");
    if (s_tap_handler != NULL) {
      s_tap_handler(BMA456_TAP_TRIPLE, s_tap_handler_ctx);
    }
  }
}

static void remove_bma456_device(void) {
  if (s_bma456_ready) {
    gpio_isr_handler_remove(BMA456_INT_GPIO);
  }
  if (s_bma456_dev != NULL) {
    i2c_master_bus_rm_device(s_bma456_dev);
    s_bma456_dev = NULL;
  }
  s_bma456_ready = false;
  s_int_pending = false;
}

static void read_sensor_task(void *param) {
  (void)param;

  if (!s_bma456_ready) {
    vTaskDelete(NULL);
    return;
  }

  struct bma4_accel sens_data = {0};

  while (1) {
    bool got_sample = false;
    if (s_accel_mutex != NULL &&
        xSemaphoreTake(s_accel_mutex, pdMS_TO_TICKS(500)) == pdTRUE) {
      int8_t ret = bma4_read_accel_xyz(&sens_data, &s_bma456);
      xSemaphoreGive(s_accel_mutex);
      if (ret == BMA4_OK) {
        got_sample = true;
      } else {
        bma4_error_codes_print_result("bma4_read_accel_xyz", ret);
      }
    }
    if (got_sample) {
      bma456_report_accel_if_changed(sens_data.x, sens_data.y, sens_data.z);
    }

    if (s_int_pending) {
      s_int_pending = false;
      handle_tap_interrupt();
    }

    vTaskDelay(pdMS_TO_TICKS(SENSOR_POLL_MS));
  }
}

static esp_err_t configure_tap_interrupt(void) {
  esp_err_t err;
  int8_t ret;
  const uint8_t int_line = BMA4_INTR2_MAP;

  struct bma456h_multitap_settings tap_settings = {0};
  ret = bma456h_tap_get_parameter(&tap_settings, &s_bma456);
  if (check_bma4("bma456h_tap_get_parameter", ret) != ESP_OK) {
    return ESP_FAIL;
  }
  tap_settings.tap_sens_thres = s_tap_config.tap_sens_thres;
  tap_settings.max_gest_dur = s_tap_config.max_gest_dur;
  tap_settings.tap_shock_dur = s_tap_config.tap_shock_dur;
  tap_settings.quite_time_after_gest = s_tap_config.quite_time_after_gest;
  tap_settings.wait_for_timeout = s_tap_config.wait_for_timeout;
  tap_settings.axis_sel = s_tap_config.axis_sel;
  ret = bma456h_tap_set_parameter(&tap_settings, &s_bma456);
  if (check_bma4("bma456h_tap_set_parameter", ret) != ESP_OK) {
    return ESP_FAIL;
  }

  uint16_t tap_features = 0;
  if (s_tap_config.enable_single) {
    tap_features |= BMA456H_SINGLE_TAP_EN;
  }
  if (s_tap_config.enable_double) {
    tap_features |= BMA456H_DOUBLE_TAP_EN;
  }
  if (s_tap_config.enable_triple) {
    tap_features |= BMA456H_TRIPLE_TAP_EN;
  }
  if (tap_features == 0) {
    ESP_LOGW(TAG, "tap config: no tap kinds enabled");
    return ESP_ERR_INVALID_ARG;
  }

  ret = bma456h_feature_enable(tap_features, BMA4_ENABLE, &s_bma456);
  if (check_bma4("bma456h_feature_enable", ret) != ESP_OK) {
    return ESP_FAIL;
  }

  ESP_LOGI(TAG,
           "tap config sens=%u max_gest=%u shock=%u quiet=%u wait=%u axis=%u "
           "(single=%d double=%d triple=%d)",
           (unsigned)s_tap_config.tap_sens_thres,
           (unsigned)s_tap_config.max_gest_dur,
           (unsigned)s_tap_config.tap_shock_dur,
           (unsigned)s_tap_config.quite_time_after_gest,
           (unsigned)s_tap_config.wait_for_timeout,
           (unsigned)s_tap_config.axis_sel, s_tap_config.enable_single,
           s_tap_config.enable_double, s_tap_config.enable_triple);

  ret = bma456h_map_interrupt(int_line, BMA456H_TAP_OUT_INT, BMA4_ENABLE,
                              &s_bma456);
  if (check_bma4("bma456h_map_interrupt", ret) != ESP_OK) {
    return ESP_FAIL;
  }

  struct bma4_int_pin_config pin_config = {0};
  ret = bma4_get_int_pin_config(&pin_config, int_line, &s_bma456);
  if (check_bma4("bma4_get_int_pin_config", ret) != ESP_OK) {
    return ESP_FAIL;
  }

  pin_config.edge_ctrl = BMA4_EDGE_TRIGGER;
  pin_config.output_en = BMA4_OUTPUT_ENABLE;
  pin_config.lvl = BMA4_ACTIVE_HIGH;
  pin_config.od = BMA4_PUSH_PULL;
  pin_config.input_en = BMA4_INPUT_DISABLE;

  ret = bma4_set_int_pin_config(&pin_config, int_line, &s_bma456);
  if (check_bma4("bma4_set_int_pin_config", ret) != ESP_OK) {
    return ESP_FAIL;
  }

  gpio_reset_pin(BMA456_INT_GPIO);
  gpio_set_direction(BMA456_INT_GPIO, GPIO_MODE_INPUT);
  gpio_set_pull_mode(BMA456_INT_GPIO, GPIO_PULLDOWN_ONLY);
  gpio_set_intr_type(BMA456_INT_GPIO, GPIO_INTR_POSEDGE);

  err = gpio_install_isr_service(0);
  if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
    return err;
  }

  err = gpio_isr_handler_add(BMA456_INT_GPIO, bma456_int_isr, NULL);
  if (err != ESP_OK) {
    return err;
  }

  return ESP_OK;
}

esp_err_t init_bma456(i2c_master_bus_handle_t bus_handle) {
  int8_t ret;
  esp_err_t err;

  s_bma456_ready = false;
  s_bma456_dev = NULL;
  s_int_pending = false;

  if (bus_handle == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  i2c_device_config_t dev_cfg = {
      .dev_addr_length = I2C_ADDR_BIT_LEN_7,
      .device_address = BMA456_I2C_ADDR,
      .scl_speed_hz = 100000,
  };

  err = i2c_master_bus_add_device(bus_handle, &dev_cfg, &s_bma456_dev);
  if (err != ESP_OK) {
    return err;
  }

  s_bma456.intf = BMA4_I2C_INTF;
  s_bma456.bus_read = bma4_i2c_read;
  s_bma456.bus_write = bma4_i2c_write;
  s_bma456.delay_us = bma4_delay_us;
  s_bma456.read_write_len = BMA4_READ_WRITE_LEN;
  s_bma456.intf_ptr = &s_bma456_dev;
  s_bma456.chip_id = 0;

  ret = bma456h_init(&s_bma456);
  if (check_bma4("bma456h_init", ret) != ESP_OK) {
    remove_bma456_device();
    return ESP_ERR_NOT_FOUND;
  }

  ESP_LOGI(TAG, "chip id 0x%02x", s_bma456.chip_id);

  ret = bma4_soft_reset(&s_bma456);
  if (check_bma4("bma4_soft_reset", ret) != ESP_OK) {
    goto fail;
  }
  vTaskDelay(pdMS_TO_TICKS(20));

  ret = bma4_set_advance_power_save(BMA4_DISABLE, &s_bma456);
  if (check_bma4("bma4_set_advance_power_save", ret) != ESP_OK) {
    goto fail;
  }
  vTaskDelay(pdMS_TO_TICKS(10));

  ret = bma456h_write_config_file(&s_bma456);
  if (check_bma4("bma456h_write_config_file", ret) != ESP_OK) {
    goto fail;
  }

  struct bma4_accel_config accel_config = {0};
  ret = bma4_get_accel_config(&accel_config, &s_bma456);
  if (check_bma4("bma4_get_accel_config", ret) != ESP_OK) {
    goto fail;
  }
  accel_config.range = BMA4_ACCEL_RANGE_2G;
  ret = bma4_set_accel_config(&accel_config, &s_bma456);
  if (check_bma4("bma4_set_accel_config", ret) != ESP_OK) {
    goto fail;
  }

  ret = bma4_set_accel_enable(BMA4_ENABLE, &s_bma456);
  if (check_bma4("bma4_set_accel_enable", ret) != ESP_OK) {
    goto fail;
  }

  if (configure_tap_interrupt() != ESP_OK) {
    goto fail;
  }

  if (s_accel_mutex == NULL) {
    s_accel_mutex = xSemaphoreCreateMutex();
    if (s_accel_mutex == NULL) {
      goto fail;
    }
  }

  if (xTaskCreate(read_sensor_task, "bma456_poll", 4096, NULL, 1, NULL) !=
      pdPASS) {
    goto fail;
  }

  s_bma456_ready = true;
  ESP_LOGI(TAG, "ready (I2C 0x%02x, INT GPIO%d, poll %d ms)", BMA456_I2C_ADDR,
           BMA456_INT_GPIO, SENSOR_POLL_MS);
  return ESP_OK;

fail:
  remove_bma456_device();
  return ESP_FAIL;
}