#include "bosch456.h"
#include "bma4.h"
#include "bma456h.h"
#include "bma4_defs.h"
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "hal/gpio_types.h"
#include <rom/ets_sys.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

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

static i2c_master_dev_handle_t bma456_dev_handle;
static bool s_bma456_ready;
static struct bma4_dev bma456_struct;

volatile uint8_t interrupt_status = 0;
uint8_t int_line;
struct bma4_int_pin_config pin_config = {0};
uint16_t int_status = 0;

#define BMA4_READ_WRITE_LEN UINT8_C(46)
#define BMA456W_INT_PIN 10
#define BMA456W_INT_PIN2 9

static void interrupt_callback(void *) {
  interrupt_status = 1;
  // ESP_LOGI("INTERRUPT", "STEP DETECTED");
}

/******************************************************************************/
/*!                User interface functions                                   */

/*!
 * I2C read function map to ESP platform
 */
BMA4_INTF_RET_TYPE bma4_i2c_read(uint8_t reg_addr, uint8_t *reg_data,
                                 uint32_t len, void *intf_ptr) {
  if (bma456_dev_handle == NULL) {
    return BMA4_E_COM_FAIL;
  }
  esp_err_t err = i2c_master_transmit_receive(bma456_dev_handle, &reg_addr, 1,
                                              reg_data, len, -1);
  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
  // return BMA4_OK;
}

/*!
 * I2C write function map to ESP platform
 */
BMA4_INTF_RET_TYPE bma4_i2c_write(uint8_t reg_addr, const uint8_t *reg_data,
                                  uint32_t len, void *intf_ptr) {
  if (bma456_dev_handle == NULL) {
    return BMA4_E_COM_FAIL;
  }
  uint8_t *buffer = malloc(len + 1);
  if (!buffer)
    return BMA4_E_NULL_PTR;

  buffer[0] = reg_addr;

  ESP_LOGI("I2CWrite", "Message Length: %d", len);

  memcpy(&buffer[1], reg_data, len);

  esp_err_t err = i2c_master_transmit(bma456_dev_handle, buffer, len + 1, -1);
  free(buffer);

  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
  // return BMA4_OK;
}

/*!
 * Delay function map to ESP platform
 */
void bma4_delay_us(uint32_t period, void *intf_ptr) {
  uint32_t wait_ms = period / 1000;
  uint32_t wait_us = period % 1000;
  if (wait_ms) {
    vTaskDelay(pdMS_TO_TICKS(wait_ms));
  }
  ets_delay_us(wait_us);
}

/*!
 *  @brief Prints the execution status of the APIs.
 */
void bma4_error_codes_print_result(const char api_name[], int8_t rslt) {
  if (rslt == BMA4_OK) {
    return;
  }

  ESP_LOGI("BMA4_I2C", "%s\t", api_name);

  switch (rslt) {
  case BMA4_E_NULL_PTR:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Null pointer\r\n", rslt);
    break;
  case BMA4_E_COM_FAIL:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Communication failure\r\n", rslt);
    break;
  case BMA4_E_CONFIG_STREAM_ERROR:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Invalid configuration stream\r\n", rslt);
    break;
  case BMA4_E_SELF_TEST_FAIL:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Self test failed\r\n", rslt);
    break;
  case BMA4_E_INVALID_SENSOR:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Device not found\r\n", rslt);
    break;
  case BMA4_E_OUT_OF_RANGE:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Out of Range\r\n", rslt);
    break;
  case BMA4_E_AVG_MODE_INVALID_CONF:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Invalid bandwidth/ODR combination\r\n",
             rslt);
    break;
  default:
    ESP_LOGI("BMA4_I2C", "Error [%d] : Unknown error code\r\n", rslt);
    break;
  }
}

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 void remove_bma456_device(void) {
  if (bma456_dev_handle != NULL) {
    i2c_master_bus_rm_device(bma456_dev_handle);
    bma456_dev_handle = NULL;
  }
  s_bma456_ready = false;
}

void read_sensor_task(void *params) {
  int8_t ret;
  struct bma4_accel sens_data = {0};

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

  while (1) {
    ret = bma4_read_accel_xyz(&sens_data, &bma456_struct);
    bma4_error_codes_print_result("bma4_read_accel_xyz", ret);

    ESP_LOGI("ACC", "X: %d, Y: %d, Z: %d", sens_data.x, sens_data.y,
             sens_data.z);

    if (interrupt_status) {
      ESP_LOGI("INTERRUPT", "Da war der Interrupt resetting");
      interrupt_status = 0;
      ret = bma456h_read_int_status(&int_status, &bma456_struct);
      bma4_error_codes_print_result("bma456w_step_counter_output status", ret);

      int8_t rslt;
      struct bma456h_out_state tap_out = {0};

      rslt = bma456h_output_state(&tap_out, &bma456_struct);

      if (BMA4_OK == rslt) {
        /* Enters only if the obtained interrupt is single-tap */
        if (tap_out.single_tap) {
          ESP_LOGI("INTERRUPT", "Single Tap interrupt occurred\n");
        }
        /* Enters only if the obtained interrupt is double-tap */
        else if (tap_out.double_tap) {
          ESP_LOGI("INTERRUPT", "Double Tap interrupt occurred\n");
        }
        /* Enters only if the obtained interrupt is triple-tap */
        else if (tap_out.triple_tap) {
          ESP_LOGI("INTERRUPT", "Triple Tap interrupt occurred\n");
        }
      }
    }

    // ESP_LOGI("i2c", "X:%d, Y%d, Z%d", sens_data.x, sens_data.y, sens_data.z);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

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

  s_bma456_ready = false;
  bma456_dev_handle = NULL;

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

  i2c_device_config_t dev_cfg_bma456 = {
      .dev_addr_length = I2C_ADDR_BIT_LEN_7,
      .device_address = BMA456_ADDRESS,
      .scl_speed_hz = 100000,
  };

  err = i2c_master_bus_add_device(bus_handle, &dev_cfg_bma456, &bma456_dev_handle);
  if (err != ESP_OK) {
    return err;
  }

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

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

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

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

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

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

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

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

  struct bma456h_multitap_settings tap_settings = {0};
  ret = bma456h_tap_get_parameter(&tap_settings, &bma456_struct);
  if (check_bma4("bma456h_tap_get_parameter", ret) != ESP_OK) {
    goto fail;
  }
  tap_settings.tap_sens_thres = 0;
  ret = bma456h_tap_set_parameter(&tap_settings, &bma456_struct);
  if (check_bma4("bma456h_tap_set_parameter", ret) != ESP_OK) {
    goto fail;
  }

  ret = bma456h_feature_enable(
      (BMA456H_SINGLE_TAP_EN | BMA456H_DOUBLE_TAP_EN | BMA456H_TRIPLE_TAP_EN),
      BMA4_ENABLE, &bma456_struct);
  if (check_bma4("bma456h_feature_enable", ret) != ESP_OK) {
    goto fail;
  }

  ret = bma456h_step_counter_set_watermark(1, &bma456_struct);
  if (check_bma4("bma456h_step_counter_set_watermark", ret) != ESP_OK) {
    goto fail;
  }

  int_line = BMA4_INTR2_MAP;

  ret = bma4_get_int_pin_config(&pin_config, int_line, &bma456_struct);
  if (check_bma4("bma4_get_int_pin_config", ret) != ESP_OK) {
    goto fail;
  }

  ret = bma456h_map_interrupt(int_line, BMA456H_TAP_OUT_INT, BMA4_ENABLE,
                              &bma456_struct);
  if (check_bma4("bma456h_map_interrupt", ret) != ESP_OK) {
    goto 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, &bma456_struct);
  if (check_bma4("bma4_set_int_pin_config", ret) != ESP_OK) {
    goto fail;
  }

  gpio_reset_pin(BMA456W_INT_PIN);
  gpio_set_direction(BMA456W_INT_PIN, GPIO_MODE_INPUT);
  gpio_set_pull_mode(BMA456W_INT_PIN, GPIO_PULLDOWN_ENABLE);
  gpio_set_intr_type(BMA456W_INT_PIN, GPIO_INTR_POSEDGE);
  gpio_intr_enable(BMA456W_INT_PIN);

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

  err = gpio_isr_handler_add(BMA456W_INT_PIN, interrupt_callback,
                             (void *)BMA456W_INT_PIN);
  if (err != ESP_OK) {
    goto fail;
  }

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

  s_bma456_ready = true;
  ESP_LOGI(TAG, "initialized");
  return ESP_OK;

fail:
  remove_bma456_device();
  return ESP_FAIL;
}