#include <stdio.h>
#include <stdlib.h>

#include "driver/i2c.h"
#include "embedded_display.c"
#include "esp_log.h"
#include "i2c.c"

#define PCA_9555_TAG "PCA9555"


// Register of command byte
#define PCA95555_CMD_INPUT_PORT_0 0x00
#define PCA95555_CMD_INPUT_PORT_1 0x01
#define PCA95555_CMD_OUTPUT_PORT_0 0x02
#define PCA95555_CMD_OUTPUT_PORT_1 0x03
#define PCA95555_CMD_POLARITY_INVERSION_PORT_0 0x04
#define PCA95555_CMD_POLARITY_INVERSION_PORT_1 0x05
#define PCA95555_CMD_CONFIGURATION_PORT_0 0x06
#define PCA95555_CMD_CONFIGURATION_PORT_1 0x07

esp_err_t pca9555_write_config(uint8_t cmd_byte, uint8_t data) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, PCA9555_ADDRESS << 1 | I2C_MASTER_WRITE,
                        ACK_CHECK_EN);
  i2c_master_write_byte(cmd, cmd_byte, ACK_CHECK_EN);
  i2c_master_write_byte(cmd, data, ACK_CHECK_EN);
  i2c_master_stop(cmd);
  esp_err_t error =
      i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);
  if (error != ESP_OK) {
    ESP_LOGW(PCA_9555_TAG, "write config failed. %d", error);
  }
  return error;
}

esp_err_t pca9555_write_output(uint8_t cmd_byte, uint8_t data) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, PCA9555_ADDRESS << 1 | I2C_MASTER_WRITE,
                        ACK_CHECK_EN);
  i2c_master_write_byte(cmd, cmd_byte, ACK_CHECK_EN);
  i2c_master_write_byte(cmd, data, ACK_CHECK_EN);
  i2c_master_stop(cmd);
  esp_err_t error =
      i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);
  if (error != ESP_OK) {
    ESP_LOGW(PCA_9555_TAG, "write output failed. %d", error);
  }
  return error;
}

esp_err_t pca9555_read_one_input(uint8_t cmd_byte, uint8_t* data) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, PCA9555_ADDRESS << 1 | I2C_MASTER_WRITE,
                        ACK_CHECK_EN);
  i2c_master_write_byte(cmd, cmd_byte, ACK_CHECK_EN);
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, PCA9555_ADDRESS << 1 | I2C_MASTER_READ,
                        ACK_CHECK_EN);
  i2c_master_read_byte(cmd, data, NACK_VAL);
  i2c_master_stop(cmd);
  esp_err_t error =
      i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);
  if (error != ESP_OK) {
    ESP_LOGW(PCA_9555_TAG, "read input failed. %d", error);
  }
  return error;
}

void pca9555_fetch(void* arg) {
  ESP_LOGI(PCA_9555_TAG, "fetching");
  esp_err_t error;
  uint8_t buff;
  char disp_str[17];
  display_fill_rect(0, 6, 128, 8, 0x00);
  for (;;) {
    error = pca9555_read_one_input(PCA95555_CMD_INPUT_PORT_1, &buff);
    if (error != ESP_OK) {
      ESP_LOGW(PCA_9555_TAG, "read failed. %x", error);
    } else {
      sprintf(disp_str, "IO0: 0x%2x  ", buff);
      display_print8_str(8, 6, disp_str);
      ESP_LOGD(PCA_9555_TAG, "IO0: 0x%2x", buff);
    }
    vTaskDelay(pdMS_TO_TICKS(10));
  }
  display_fill_rect(0, 6, 128, 8, 0x00);
}

void pca9555_auto_fetch() {
  if (is_apds_9930_online == 0) {
    return;
  }
  xTaskCreate(pca9555_fetch, "pac05555-fetching", 2048, NULL, 10, NULL);
}

void pca9555_init() {
  if (is_pca9555_online == 0) {
    ESP_LOGE(PCA_9555_TAG, "salve offline");
    return;
  }
  ESP_LOGI(PCA_9555_TAG, "Initializing PCA9555");
  esp_log_level_set(PCA_9555_TAG, ESP_LOG_DEBUG);
  pca9555_write_config(PCA95555_CMD_CONFIGURATION_PORT_1, 0x01);
}