#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c_slave.h"
#include "driver/uart.h"
#include "esp_log.h"

#define I2C_SLAVE_SDA_IO 21
#define I2C_SLAVE_SCL_IO 22
#define I2C_SLAVE_NUM I2C_NUM_0
#define I2C_SLAVE_ADDR 0x61
#define I2C_SLAVE_RX_BUF_LEN 128
#define I2C_SLAVE_TX_BUF_LEN 128

#define UART_NUM UART_NUM_0
#define UART_BAUD_RATE 921600
#define UART_BUF_SIZE 256

#define TAG "I2C2SERIAL"

static void i2c_slave_init(void) {
    i2c_config_t conf_slave = {
        .mode = I2C_MODE_SLAVE,
        .sda_io_num = I2C_SLAVE_SDA_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = I2C_SLAVE_SCL_IO,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .slave = {
            .slave_addr = I2C_SLAVE_ADDR,
            .maximum_speed = 400000,
        },
    };
    ESP_ERROR_CHECK(i2c_param_config(I2C_SLAVE_NUM, &conf_slave));
    ESP_ERROR_CHECK(i2c_driver_install(I2C_SLAVE_NUM, conf_slave.mode, I2C_SLAVE_RX_BUF_LEN, I2C_SLAVE_TX_BUF_LEN, 0));
}

static void uart_init(void) {
    uart_config_t uart_config = {
        .baud_rate = UART_BAUD_RATE,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_APB,
    };
    ESP_ERROR_CHECK(uart_driver_install(UART_NUM, UART_BUF_SIZE, UART_BUF_SIZE, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
}

static void i2c2serial_task(void *arg) {
    uint8_t i2c_rx[I2C_SLAVE_RX_BUF_LEN];
    uint8_t uart_rx[UART_BUF_SIZE];
    uint8_t i2c_tx[I2C_SLAVE_TX_BUF_LEN];
    while (1) {
        // Wait for I2C master write
        int rx_len = i2c_slave_read_buffer(I2C_SLAVE_NUM, i2c_rx, sizeof(i2c_rx), pdMS_TO_TICKS(100));
        if (rx_len > 0) {
            // Print CMD to serial in hex
            printf("CMD:");
            for (int i = 0; i < rx_len; ++i) {
                printf("%02X", i2c_rx[i]);
            }
            printf("\n");

            // Wait for DATA:[hex]\n from serial, with 200ms timeout
            int uart_len = 0;
            int total_len = 0;
            TickType_t start_tick = xTaskGetTickCount();
            bool got_data = false;
            while ((xTaskGetTickCount() - start_tick) < pdMS_TO_TICKS(200)) {
                uart_len = uart_read_bytes(UART_NUM, uart_rx + total_len, UART_BUF_SIZE - total_len, pdMS_TO_TICKS(10));
                if (uart_len > 0) {
                    total_len += uart_len;
                    // Look for a full line
                    char *newline = memchr(uart_rx, '\n', total_len);
                    if (newline) {
                        int line_len = newline - (char *)uart_rx + 1;
                        uart_rx[line_len-1] = 0; // Null-terminate
                        if (strncmp((char *)uart_rx, "DATA:", 5) == 0) {
                            // Parse hex after DATA:
                            char *hexstr = (char *)uart_rx + 5;
                            int tx_len = 0;
                            while (*hexstr && tx_len < I2C_SLAVE_TX_BUF_LEN) {
                                unsigned int val;
                                if (sscanf(hexstr, "%2x", &val) == 1) {
                                    i2c_tx[tx_len++] = val;
                                    hexstr += 2;
                                } else {
                                    break;
                                }
                            }
                            // Write to I2C slave TX buffer
                            i2c_slave_write_buffer(I2C_SLAVE_NUM, i2c_tx, tx_len, 0);
                            got_data = true;
                        }
                        // Remove processed line
                        memmove(uart_rx, newline + 1, total_len - line_len);
                        total_len -= line_len;
                        break;
                    }
                }
            }
            if (!got_data) {
                // Timeout: clear I2C TX buffer
                i2c_slave_write_buffer(I2C_SLAVE_NUM, NULL, 0, 0);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

void app_main(void) {
    i2c_slave_init();
    uart_init();
    xTaskCreate(i2c2serial_task, "i2c2serial_task", 4096, NULL, 10, NULL);
}