#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 "nvs.h"
#include "nvs_flash.h"
#include <stdint.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,
};

struct app_config_t {
  uint8_t Master;
  uint8_t Network;
  char RunningPartition[17];
};

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

static i2c_master_bus_handle_t bus_handle;
static i2c_master_dev_handle_t io_expander;

static struct app_config_t App_Config;

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) {
  // 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 = 0;

  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 Could not Init Bus");
  }

  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);
    }
    // Build NetworkNumber because Pin 5-8 are reversed
    network = reverse_high_nibble_lut(read_val);
    network = ~network; //reverse bits
  }

  esp_err_t ret = nvs_flash_init();
  if (ret == ESP_ERR_NVS_NO_FREE_PAGES ||
      ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
    ESP_ERROR_CHECK(nvs_flash_erase());
    ret = nvs_flash_init();
  }
  ESP_ERROR_CHECK(ret);

  const esp_partition_t *running = esp_ota_get_running_partition();

  App_Config.Master = (master == true);
  App_Config.Network = network;
  memcpy(App_Config.RunningPartition, running->label,
         sizeof(App_Config.RunningPartition));

  ESP_LOGI(TAG, "RUNNING CONFIG:");
  ESP_LOGI(TAG, "Master:            %d", App_Config.Master);
  ESP_LOGI(TAG, "Network:           %d", App_Config.Network);
  ESP_LOGI(TAG, "Running Partition: %s", App_Config.RunningPartition);
}