#pragma once
#include "driver/i2c.h"

#include "esp_log.h"

#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_SDA_IO CONFIG_I2C_SDA
#define I2C_MASTER_SCL_IO CONFIG_I2C_SCL

#define ACK_CHECK_EN 0x1  /*!< I2C master will check ack from slave*/
#define ACK_CHECK_DIS 0x0 /*!< I2C master will not check ack from slave */
#define ACK_VAL 0x0       /*!< I2C ack value */
#define NACK_VAL 0x1      /*!< I2C nack value */

#define I2C_MASTER_FREQ_HZ 400000 /*!< I2C master clock frequency */
#define I2C_MASTER_NUM CONFIG_I2C_NUM

// 0 1 0 0 A2 A1 A0
#define PCA9555_ADDRESS 0x20
#define GX21M15_ADDRESS 0x48
#define APDS_9930_ADDRESS 0x39
#define I2C_ADDRESS 0x3c

static const char *I2C_TAG = "APP_I2C";

static uint8_t is_temperature_online = 0;
static uint8_t is_pca9555_online = 0;
static uint8_t is_apds_9930_online = 0;
static uint8_t is_embedded_display_online = 0;

void init_i2c() {
  i2c_config_t conf = {
      .mode = I2C_MODE_MASTER,
      .sda_io_num = I2C_MASTER_SDA_IO,
      .scl_io_num = I2C_MASTER_SCL_IO,
      .sda_pullup_en = GPIO_PULLUP_ENABLE,
      .scl_pullup_en = GPIO_PULLUP_ENABLE,
      .master.clk_speed = I2C_MASTER_FREQ_HZ,
  };
  ESP_ERROR_CHECK(i2c_driver_install(I2C_MASTER_NUM, conf.mode,
                                     I2C_MASTER_RX_BUF_DISABLE,
                                     I2C_MASTER_TX_BUF_DISABLE, 0));
  i2c_param_config(I2C_MASTER_NUM, &conf);

  ESP_LOGI(I2C_TAG, "I2C initialized");
}

uint8_t i2c_check_slave_exists(uint8_t address) {
  ESP_LOGI(I2C_TAG, "Checking if slave 0x%2x exists", address);
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
  i2c_master_stop(cmd);
  esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 50 / portTICK_RATE_MS);
  i2c_cmd_link_delete(cmd);

  if (ret == ESP_OK) {
    ESP_LOGW(I2C_TAG, "Slave 0x%2x found", address);
    return 1;
  } else if (ret == ESP_ERR_TIMEOUT) {
    ESP_LOGW(I2C_TAG, "Slave 0x%2x TIMEOUT", address);
    return 1;
  } else {
    ESP_LOGW(I2C_TAG, "Slave 0x%2x not found", address);
    return 0;
  }
}

void i2c_check_slaves() {
  is_temperature_online = i2c_check_slave_exists(GX21M15_ADDRESS);
  is_pca9555_online = i2c_check_slave_exists(PCA9555_ADDRESS);
  is_apds_9930_online = i2c_check_slave_exists(APDS_9930_ADDRESS);
  is_embedded_display_online = i2c_check_slave_exists(I2C_ADDRESS);
}