#include "i2cbrn.h"

i2c_master_bus_config_t i2c0_bus_cfg = {
    .clk_source = I2C_CLK_SRC_DEFAULT,
    .i2c_port = I2C_NUM_0,
    .scl_io_num = HWI2C_SCL,
    .sda_io_num = HWI2C_SDA,
    .glitch_ignore_cnt = 7,
    .flags.enable_internal_pullup = true,
};
i2c_master_bus_handle_t i2c0_bus_hdl;

// esp_err_t i2c_master_bus_detect_devices(i2c_master_bus_handle_t handle)
// {
//     const uint16_t probe_timeout_ms = 20; // timeout in milliseconds
//     uint8_t address;

//     printf("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\r\n");

//     for (int i = 0; i < 128; i += 16)
//     {
//         printf("%02x: ", i);

//         for (int j = 0; j < 16; j++)
//         {
//             fflush(stdout);

//             address = i + j;

//             esp_err_t ret = i2c_master_probe(handle, address, probe_timeout_ms);

//             if (ret == ESP_OK)
//             {
//                 printf("%02x ", address);
//             }
//             else if (ret == ESP_ERR_TIMEOUT)
//             {
//                 printf("UU ");
//             }
//             else
//             {
//                 printf("-- ");
//             }
//         }
//         printf("\r\n");
//     }

//     return ESP_OK;
// }


/**
 * i2c master initialization
 */

 esp_err_t i2c_master_bus_detect_devices(i2c_master_bus_handle_t handle)
 {
     uint8_t address;
     bool found_any = false;
 
     printf("Scanning I2C bus...\n");
 
     for (int i = 0; i < 128; i++)
     {
         fflush(stdout);
         address = i;
 
         esp_err_t ret = i2c_master_probe(handle, address, 20);
 
         if (ret == ESP_OK)
         {
             printf("Found device at 0x%02X\n", address);
             found_any = true;
         }
     }
 
     if (!found_any)
     {
         printf("No I2C devices found.\n");
     }
 
     return ESP_OK;
 }

/**
 * @brief Writes data to a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to write to
 * @param data Pointer to the data buffer to write
 * @param len Number of bytes to write
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_write_register(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t *data, size_t len)
{
    if (len == 0) {
        // If no data, send just the register address
        return i2c_master_transmit(dev_handle, &reg_addr, 1, I2C_TIMEOUT_MS_VALUE);
    }

    uint8_t buffer[len + 1];
    buffer[0] = reg_addr;
    memcpy(&buffer[1], data, len);

    esp_err_t ret = i2c_master_transmit(dev_handle, buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        ESP_LOGV(TAG_I2C, "Write to register 0x%02X successful", reg_addr);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Write to register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

/**
 * @brief Reads data from a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to read from
 * @param data Pointer to a buffer to store read data
 * @param len Number of bytes to read
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_read_register(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t *data, size_t len)
{
    esp_err_t ret = i2c_master_transmit_receive(dev_handle, &reg_addr, 1, data, len, I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        ESP_LOGV(TAG_I2C, "Read from register 0x%02X successful", reg_addr);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Read from register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

/**
 * @brief Writes a 16-bit value to a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to write to
 * @param value 16-bit value to write
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_write_register_16(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint16_t value)
{
    uint8_t buffer[3];
    buffer[0] = reg_addr;       // Register address
    buffer[1] = (value >> 8);   // High byte
    buffer[2] = (value & 0xFF); // Low byte

    esp_err_t ret = i2c_master_transmit(dev_handle, buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        ESP_LOGV(TAG_I2C, "Write to register 0x%02X successful (Value: 0x%04X)", reg_addr, value);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Write to register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

/**
 * @brief Reads a 16-bit value from a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to read from
 * @param value Pointer to store the read 16-bit value
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_read_register_16(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint16_t *value)
{
    uint8_t buffer[2];

    esp_err_t ret = i2c_master_transmit_receive(dev_handle, &reg_addr, 1, buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        *value = (buffer[0] << 8) | buffer[1]; // Combine MSB and LSB
        ESP_LOGV(TAG_I2C, "Read from register 0x%02X successful (Value: 0x%04X)", reg_addr, *value);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Read from register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

/**
 * @brief Writes an 8-bit value to a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to write to
 * @param value 8-bit value to write
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_write_register_8(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t value)
{
    uint8_t buffer[2];
    buffer[0] = reg_addr; // Register address
    buffer[1] = value;    // 8-bit value

    esp_err_t ret = i2c_master_transmit(dev_handle, buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        ESP_LOGV(TAG_I2C, "Write to register 0x%02X successful (Value: 0x%02X)", reg_addr, value);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Write to register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

/**
 * @brief Reads an 8-bit value from a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to read from
 * @param value Pointer to store the read 8-bit value
 * @return esp_err_t ESP_OK on success, or an error code
 */
esp_err_t i2c_read_register_8(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t *value)
{
    esp_err_t ret = i2c_master_transmit_receive(dev_handle, &reg_addr, 1, value, 1, I2C_TIMEOUT_MS_VALUE);
    if (ret == ESP_OK)
    {
        ESP_LOGV(TAG_I2C, "Read from register 0x%02X successful (Value: 0x%02X)", reg_addr, *value);
    }
    else
    {
        ESP_LOGE(TAG_I2C, "Read from register 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}