#include "mcp23018.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "../components/sensors.h"
// Local buffer for tracking GPIO state

i2c_device_config_t MCP23018_DEV_CFG = {
    .dev_addr_length = I2C_ADDR_BIT_LEN_7,
    .device_address = MCP23018_ADDRESS,
    .scl_speed_hz = 100000,
};

i2c_master_dev_handle_t MCP23018_DEV_HANDLE;
uint8_t gpioa_state = 0x00; // All LOW initially
uint8_t gpiob_state = 0x00; // All LOW initially

esp_err_t mcp23018_set_pin(i2c_master_dev_handle_t dev_handle, uint8_t pin, uint8_t value)
{
    ESP_LOGI(TAG_MCP, "Setting %d to %d", pin, value);
    esp_err_t ret = ESP_FAIL;
    if (pin < 8)
    {
        // GPIOA (Pins 0-7)
        if (value)
            gpioa_state |= (1 << pin); // Set bit
        else
            gpioa_state &= ~(1 << pin); // Clear bit

        // Write updated buffer to MCP23018
        ret = i2c_write_register_8(dev_handle, MCP23018_GPIOA, gpioa_state);
    }
    else if (pin < 16)
    {
        // GPIOB (Pins 8-15)
        uint8_t pinB = pin - 8;
        if (value)
            gpiob_state |= (1 << pinB); // Set bit
        else
            gpiob_state &= ~(1 << pinB); // Clear bit

        // Write updated buffer to MCP23018
        ret = i2c_write_register_8(dev_handle, MCP23018_GPIOB, gpiob_state);
    }
    vTaskDelay(1 / portTICK_PERIOD_MS);
    return ret;
}

esp_err_t mcp23018_init()
{
    esp_err_t ret = i2c_master_bus_add_device(i2c0_bus_hdl, &MCP23018_DEV_CFG, &MCP23018_DEV_HANDLE);
    if (ret != ESP_OK) {return ret;}
    i2c_write_register_8(MCP23018_DEV_HANDLE, MCP23018_IODIRA, gpioa_state);
    if (ret != ESP_OK) {return ret;}
    i2c_write_register_8(MCP23018_DEV_HANDLE, MCP23018_IODIRB, gpiob_state);
    if (ret != ESP_OK) {return ret;}
    mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CS_ADC_CO, 1);
    if (ret != ESP_OK) {return ret;}
    mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CS_ADC_NH3, 1);
    if (ret != ESP_OK) {return ret;}
    mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CS_ADC_NO2, 1);
    if (ret != ESP_OK) {return ret;}
    mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CS_ADC_UVC, 1);
    if (ret != ESP_OK) {return ret;}
    mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 1);
    return ret;
}