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

// I2C Slave Configuration
#define I2C_SLAVE_SDA_IO 21
#define I2C_SLAVE_SCL_IO 22
#define I2C_SLAVE_FREQ_HZ 100000
#define I2C_SLAVE_ADDR 0x61
#define I2C_SLAVE_RX_BUF_LEN 256
#define I2C_SLAVE_TX_BUF_LEN 256

// UART Configuration
#define UART_NUM UART_NUM_0
#define UART_BAUD_RATE 921600
#define UART_BUF_SIZE 512

static const char *TAG = "I2C_SLAVE";

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 esp_err_t i2c_slave_init(void) {
    int i2c_slave_port = I2C_NUM_0;
    i2c_config_t conf_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,
        .mode = I2C_MODE_SLAVE,
        .slave.addr_10bit_en = 0,
        .slave.slave_addr = I2C_SLAVE_ADDR,
        .clk_flags = 0,
    };
    esp_err_t err = i2c_param_config(i2c_slave_port, &conf_slave);
    if (err != ESP_OK) return err;
    return i2c_driver_install(i2c_slave_port, conf_slave.mode, I2C_SLAVE_RX_BUF_LEN, I2C_SLAVE_TX_BUF_LEN, 0);
}

void app_main(void) {
    uint8_t i2c_rx_buffer[I2C_SLAVE_RX_BUF_LEN];
    uint8_t uart_rx_buffer[UART_BUF_SIZE];
    uint8_t i2c_tx_buffer[I2C_SLAVE_TX_BUF_LEN];

    uart_init();
    ESP_ERROR_CHECK(i2c_slave_init());
    ESP_LOGI(TAG, "I2C Slave initialized successfully (Legacy Driver)");

    while (1) {
        // 1. Wait for data from Master (Blocking with timeout)
        int len = i2c_slave_read_buffer(I2C_NUM_0, i2c_rx_buffer, I2C_SLAVE_RX_BUF_LEN, 50 / portTICK_PERIOD_MS);
        
        if (len > 0) {
            ESP_LOGI(TAG, "Received %d bytes from Master", len);
            
            // 2. Send CMD to Python
            uart_write_bytes(UART_NUM, "CMD:", 4);
            for (int i = 0; i < len; i++) {
                char hex[3];
                sprintf(hex, "%02X", i2c_rx_buffer[i]);
                uart_write_bytes(UART_NUM, hex, 2);
            }
            uart_write_bytes(UART_NUM, "\n", 1);
            
            // 3. Wait for DATA from Python (Blocking with timeout)
            int total_rx = 0;
            bool data_received = false;
            // Simple timeout for Python response
            for (int i=0; i<100; i++) { // ~1 second timeout
                 int ulen = uart_read_bytes(UART_NUM, uart_rx_buffer + total_rx, UART_BUF_SIZE - total_rx, 10 / portTICK_PERIOD_MS);
                 if (ulen > 0) {
                     total_rx += ulen;
                     // Check for newline
                     char *newline = memchr(uart_rx_buffer, '\n', total_rx);
                     if (newline) {
                         // Parse DATA:
                         if (strncmp((char*)uart_rx_buffer, "DATA:", 5) == 0) {
                             int tx_len = 0;
                             char *p = (char*)uart_rx_buffer + 5;
                             while (p < newline) {
                                  unsigned int val;
                                  if (sscanf(p, "%2x", &val) == 1) {
                                      i2c_tx_buffer[tx_len++] = (uint8_t)val;
                                      p += 2;
                                  } else {
                                      break; 
                                  }
                             }
                             
                             if (tx_len > 0) {
                                 i2c_slave_write_buffer(I2C_NUM_0, i2c_tx_buffer, tx_len, 100 / portTICK_PERIOD_MS);
                                 ESP_LOGI(TAG, "Loaded %d bytes to TX buffer", tx_len);
                             } else {
                                 // Even if empty, we might need to clear or write 0 if Master expects read?
                                 // If Python sends empty DATA:, it means just ack/nothing to send.
                             }
                             data_received = true;
                         }
                         break;
                     }
                 }
            }

            if (!data_received) {
                ESP_LOGW(TAG, "Timeout waiting for Python response");
            }
        }
        
        // Clear RX buffer not needed as read_buffer consumes it, but good practice if using ring buffer?
        // i2c_reset_rx_fifo(I2C_NUM_0); // This might be too aggressive if data came in during processing?
        // Legacy driver handles fifo logic.
    }
}