#include "app_config.h"
#include "cmd_handler.h"
#include "cmd_accel_deadzone.h"
#include "cmd_accel_stream.h"
#include "cmd_tap_notify.h"
#include "cmd_tap_config.h"
#include "cmd_cache_status.h"
#include "cmd_espnow_unicast_test.h"
#include "cmd_espnow_echo_ping.h"
#include "cmd_espnow_find_me.h"
#include "cmd_restart.h"
#include "cmd_client_id_map_reset.h"
#include "cmd_client_info.h"
#include "cmd_version.h"
#include "cmd_ota.h"
#include "cmd_ota_slave_progress.h"
#include "ota_confirm.h"
#include "cmd_led_ring.h"
#include "cmd_battery.h"
#include "cmd_set_log_level.h"
#include "esp_now_comm.h"
#include "powerpod.h"
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "driver/i2c_types.h"
#include "esp_err.h"
#include "esp_flash_partitions.h"
#include "esp_log.h"
#include "esp_ota_ops.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "board_input.h"
#include "bosch456.h"
#include "led_ring.h"
#include "pod_settings.h"
#include "uart.h"
#include "ble_host.h"
#include "battery_uv.h"
#include <stdint.h>
#include <string.h>

enum MASTER_STATES {
  MSTATE_INIT,
  MSTATE_IDLE,
  MSTATE_NORMAL,
  MSTATE_DEEP_SLEEP,
  MSTATE_LOW_POWER,
  MSTATE_FAULT,
  MSTATE_UART_UPDATE,
  MSTATE_OTA_UPDATE,
};

enum SLAVE_STATES {
  SSTATE_INIT,
  SSTATE_IDLE,
  SSTATE_NORMAL,
  SSTATE_DEEP_SLEEP,
  SSTATE_LOW_POWER,
  SSTATE_FAULT,
  SSTATE_UART_UPDATE,
  SSTATE_OTA_UPDATE,
};

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

static i2c_master_bus_handle_t bus_handle;
static i2c_master_dev_handle_t io_expander;

static app_config_t app_config;

const app_config_t *app_config_get(void) { return &app_config; }

static QueueHandle_t cmd_queue;

uint8_t reverse_high_nibble_lut(uint8_t n) {
  static const uint8_t lookup[] = {0x0, 0x8, 0x4, 0xC, 0x2, 0xA, 0x6, 0xE,
                                   0x1, 0x9, 0x5, 0xD, 0x3, 0xB, 0x7, 0xF};

  return (lookup[n >> 4] << 4) | (n & 0x0F);
}

void app_main(void) {
  if (pod_settings_init() != ESP_OK) {
    ESP_LOGW(TAG, "settings NVS init failed; using defaults");
  }

  if (board_input_init_adc_only() != ESP_OK) {
    ESP_LOGW(TAG, "LiPo ADC init failed");
  }

#if POWERPOD_BATTERY_UV_ENABLE
  if (battery_uv_evaluate_boot()) {
    battery_uv_run_mode();
  }
#endif

  // Get Master Mode Pin
  gpio_reset_pin(DIP_MASTER);
  gpio_set_direction(DIP_MASTER, GPIO_MODE_INPUT);
  uint8_t master = gpio_get_level(DIP_MASTER) ? 0 : 1;
  uint8_t network = 1;
  bool host_ble = false;

  esp_err_t err;

  /* Init I2C IO Expander */
  i2c_master_bus_config_t i2c_mst_config = {
      .clk_source = I2C_CLK_SRC_DEFAULT,
      .i2c_port = I2C_PORT,
      .scl_io_num = I2C_SCL,
      .sda_io_num = I2C_SDA,
      .glitch_ignore_cnt = 7,
      .flags.enable_internal_pullup = true,
  };

  err = i2c_new_master_bus(&i2c_mst_config, &bus_handle);
  if (err != ESP_OK) {
    ESP_LOGI(TAG, "I2C master bus init failed: %s", esp_err_to_name(err));
    bus_handle = NULL;
  }

  i2c_device_config_t dev_cfg = {
      .dev_addr_length = I2C_ADDR_BIT_LEN_7,
      .device_address = IO_EXPANDER_ADDRESS,
      .scl_speed_hz = 100000,
  };
  err = i2c_master_bus_add_device(bus_handle, &dev_cfg, &io_expander);
  if (err != ESP_OK) {
    ESP_LOGI(TAG, "Could not add IO Expnader to Bus %d", err);
  } else {

    uint8_t write_buf[2] = {0x03, 0xFF}; // 0xFF setzt alle Bits auf Input
    err = i2c_master_transmit(io_expander, write_buf, sizeof(write_buf), -1);
    if (err != ESP_OK) {
      ESP_LOGI(TAG, "Could not set IO Expander in input mode %d", err);
    }

    uint8_t reg_addr = 0x00;
    uint8_t read_val = 0;

    err = i2c_master_transmit_receive(io_expander, &reg_addr, 1, &read_val, 1,
                                      -1);
    if (err != ESP_OK) {
      ESP_LOGI(TAG, "Could not read IO Expander input %d", err);
    } else {
      /* High nibble physically reversed on PCB; DIP1..8 after reverse+invert. */
      uint8_t bits = (uint8_t)(~reverse_high_nibble_lut(read_val));
      /* DIP8 (bit7) = host transport: 1 = BLE, 0 = UART */
      host_ble = (bits & 0x80u) != 0;
      uint8_t net_bits = (uint8_t)(bits & 0x7Fu);
      if (net_bits == 0 || (net_bits & (net_bits - 1u)) != 0) {
        ESP_LOGW(TAG, "invalid network DIP one-hot=0x%02x; fallback network=1",
                 net_bits);
        network = 1;
      } else {
        network = (uint8_t)(__builtin_ctz(net_bits) + 1); /* 1..7 */
      }
    }
  }

  if (bus_handle != NULL) {
    esp_err_t bma_err = init_bma456(bus_handle);
    if (bma_err != ESP_OK) {
      ESP_LOGI(TAG, "BMA456 init skipped: %s", esp_err_to_name(bma_err));
    } else {
      pod_settings_apply_accel_deadzone();
      pod_settings_apply_tap_config();
    }
  }

  const esp_partition_t *running = esp_ota_get_running_partition();
  int ota_slot = -1;
  if (running != NULL) {
    if (running->subtype == ESP_PARTITION_SUBTYPE_APP_OTA_0) {
      ota_slot = 0;
    } else if (running->subtype == ESP_PARTITION_SUBTYPE_APP_OTA_1) {
      ota_slot = 1;
    }
  }

  app_config.master = (master != 0);
  app_config.network = network;
  app_config.host_ble = host_ble;
  if (running != NULL) {
    memcpy(app_config.running_partition, running->label,
           sizeof(app_config.running_partition));
  } else {
    app_config.running_partition[0] = '\0';
  }

  ESP_LOGI(TAG, "RUNNING CONFIG:");
  ESP_LOGI(TAG, "Master:            %d", app_config.master);
  ESP_LOGI(TAG, "Network:           %d", app_config.network);
  ESP_LOGI(TAG, "Host:              %s (DIP8; reboot to switch)",
           app_config.host_ble ? "BLE" : "UART");
  ESP_LOGI(TAG, "Running Partition: %s (OTA slot %d)",
           app_config.running_partition, ota_slot);

  board_input_start_lipo_monitor();
  board_input_init_button();

  err = esp_now_comm_init(&app_config);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "ESP-NOW init failed: %s", esp_err_to_name(err));
  }

  led_ring_init();

  if (app_config.master) {
    cmd_queue = xQueueCreate(64, sizeof(generic_msg_t));
    init_cmdHandler(cmd_queue);
    if (app_config.host_ble) {
      err = init_ble_host(cmd_queue);
      if (err != ESP_OK) {
        ESP_LOGE(TAG, "BLE host init failed: %s", esp_err_to_name(err));
      }
    } else {
      init_uart(cmd_queue);
    }
    cmd_version_register();
    cmd_client_info_register();
    cmd_accel_deadzone_register();
    cmd_accel_stream_register();
    cmd_tap_notify_register();
    cmd_tap_config_register();
    cmd_cache_status_register();
    cmd_espnow_unicast_test_register();
    cmd_espnow_echo_ping_register();
    cmd_espnow_find_me_register();
    cmd_restart_register();
    cmd_client_id_map_reset_register();
    cmd_led_ring_register();
    cmd_battery_register();
    cmd_ota_register();
    cmd_ota_slave_progress_register();
    cmd_set_log_level_register();
  }

  ESP_LOGI(TAG, "LED ring: UART LED_RING commands only (no local demo loop)");

  /* After OTA: PENDING_VERIFY until confirm; crash before this → rollback. */
  ota_confirm_schedule();

  while (1) {
    vTaskDelay(portMAX_DELAY);
  }
}