#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_err.h"

#include "driver/i2c.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "esp_vfs_fat.h"
#include "sdmmc_cmd.h"
#include "driver/spi_common.h"
#include "sdkconfig.h"

static const char *TAG = "diag_test";

/* --- Pin mapping (do diagram.json) --- */
#define I2C_MASTER_SCL_IO           9    // SCL
#define I2C_MASTER_SDA_IO           8    // SDA
#define I2C_MASTER_NUM              I2C_NUM_0
#define I2C_MASTER_FREQ_HZ          400000

/* Buttons */
#define BTN1_GPIO                   40
#define BTN2_GPIO                   38

/* Buzzer */
#define BUZZER_GPIO                 18
#define BUZZER_LEDC_TIMER           LEDC_TIMER_0
#define BUZZER_LEDC_CHANNEL         LEDC_CHANNEL_0
#define BUZZER_FREQ_HZ              2000

/* SD (SDSPI) pins */
#define SD_SPI_CS_GPIO              10
#define SD_SPI_MOSI_GPIO            11
#define SD_SPI_MISO_GPIO            13
#define SD_SPI_SCK_GPIO             12

/* I2C device addresses */
#define SSD1306_I2C_ADDR            0x3C
#define MPU6050_I2C_ADDR            0x68
#define MPU6050_WHO_AM_I_REG        0x75
#define MPU6050_WHO_AM_I_EXPECT     0x68

/* Simple I2C read/write wrappers */
static esp_err_t i2c_master_read_reg(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg, uint8_t *data, size_t len)
{
    if (len == 0) return ESP_ERR_INVALID_ARG;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_READ, true);
    if (len > 1) {
        i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
    }
    i2c_master_read_byte(cmd, data + (len - 1), I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    return ret;
}

static esp_err_t i2c_scan_and_check(i2c_port_t i2c_num, uint8_t addr)
{
    // Try to write zero bytes to detect device presence
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(500));
    i2c_cmd_link_delete(cmd);
    return ret;
}

/* Initialize I2C */
static esp_err_t i2c_init(void)
{
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    esp_err_t ret = i2c_param_config(I2C_MASTER_NUM, &conf);
    if (ret != ESP_OK) return ret;
    return i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
}

/* Buzzer init and beep */
static void buzzer_init(void)
{
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = BUZZER_LEDC_TIMER,
        .duty_resolution = LEDC_TIMER_10_BIT,
        .freq_hz = BUZZER_FREQ_HZ,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel = {
        .gpio_num = BUZZER_GPIO,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = BUZZER_LEDC_CHANNEL,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = BUZZER_LEDC_TIMER,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&ledc_channel);
}

static void buzzer_beep(int ms)
{
    // 50% duty cycle -> on
    uint32_t duty = (1 << 10) / 2; // 10-bit resolution
    ledc_set_duty(LEDC_LOW_SPEED_MODE, BUZZER_LEDC_CHANNEL, duty);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, BUZZER_LEDC_CHANNEL);
    vTaskDelay(pdMS_TO_TICKS(ms));
    // off
    ledc_set_duty(LEDC_LOW_SPEED_MODE, BUZZER_LEDC_CHANNEL, 0);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, BUZZER_LEDC_CHANNEL);
}

/* Buttons init */
static void buttons_init(void)
{
    gpio_config_t io_conf = {
        .pin_bit_mask = ((1ULL << BTN1_GPIO) | (1ULL << BTN2_GPIO)),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_ENABLE, // default pull-down; wiring might differ
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_conf);
}

/* Test I2C devices: SSD1306 presence and MPU6050 WHO_AM_I */
static void test_i2c_devices(void)
{
    ESP_LOGI(TAG, "Scanning I2C bus on SDA=%d SCL=%d...", I2C_MASTER_SDA_IO, I2C_MASTER_SCL_IO);
    for (int addr = 1; addr < 127; addr++) {
        if (i2c_scan_and_check(I2C_MASTER_NUM, addr) == ESP_OK) {
            ESP_LOGI(TAG, "  Found device at 0x%02X", addr);
        }
    }

    // Check SSD1306
    if (i2c_scan_and_check(I2C_MASTER_NUM, SSD1306_I2C_ADDR) == ESP_OK) {
        ESP_LOGI(TAG, "SSD1306 (0x3C) detected.");
    } else {
        ESP_LOGW(TAG, "SSD1306 (0x3C) NOT detected.");
    }

    // Check MPU6050 WHO_AM_I
    uint8_t who = 0;
    esp_err_t r = i2c_master_read_reg(I2C_MASTER_NUM, MPU6050_I2C_ADDR, MPU6050_WHO_AM_I_REG, &who, 1);
    if (r == ESP_OK) {
        ESP_LOGI(TAG, "MPU6050 WHO_AM_I read: 0x%02X", who);
        if (who == MPU6050_WHO_AM_I_EXPECT) {
            ESP_LOGI(TAG, "MPU6050 detected (matches 0x68).");
        } else {
            ESP_LOGW(TAG, "MPU6050 WHO_AM_I unexpected value.");
        }
    } else {
        ESP_LOGW(TAG, "Failed to read MPU6050 WHO_AM_I (addr 0x68). err=%d", r);
    }
}

/* Mount SD card using SDSPI (sdmmc library) */
static void test_sd_card(void)
{
    ESP_LOGI(TAG, "Initializing SD card (SDSPI)...");
    esp_vfs_fat_sdmmc_mount_config_t mount_config = {
        .format_if_mount_failed = false,
        .max_files = 5,
        .allocation_unit_size = 16 * 1024
    };

    sdmmc_host_t host = SDSPI_HOST_DEFAULT();
    sdspi_device_config_t slot_config = SDSPI_DEVICE_CONFIG_DEFAULT();
    slot_config.gpio_cs = SD_SPI_CS_GPIO;
    slot_config.gpio_miso = SD_SPI_MISO_GPIO;
    slot_config.gpio_mosi = SD_SPI_MOSI_GPIO;
    slot_config.gpio_sclk = SD_SPI_SCK_GPIO;

    sdmmc_card_t *card;
    esp_err_t ret = esp_vfs_fat_sdmmc_mount("/sdcard", &host, &slot_config, &mount_config, &card);
    if (ret != ESP_OK) {
        ESP_LOGW(TAG, "Failed to mount SD card. err=%d", ret);
        ESP_LOGW(TAG, "Check wiring: CS=%d SCK=%d MOSI=%d MISO=%d", SD_SPI_CS_GPIO, SD_SPI_SCK_GPIO, SD_SPI_MOSI_GPIO, SD_SPI_MISO_GPIO);
        return;
    }
    ESP_LOGI(TAG, "SD card mounted.");

    // Print card info
    sdmmc_card_print_info(stdout, card);

    // List root directory (brief)
    ESP_LOGI(TAG, "Listing /sdcard root (up to 20 entries):");
    int count = 0;
    DIR* dir = opendir("/sdcard");
    if (!dir) {
        ESP_LOGW(TAG, "Could not open /sdcard directory.");
    } else {
        struct dirent* ent;
        while ((ent = readdir(dir)) != NULL && count < 20) {
            ESP_LOGI(TAG, "  %s", ent->d_name);
            count++;
        }
        closedir(dir);
    }

    // Unmount
    esp_vfs_fat_sdmmc_unmount();
    ESP_LOGI(TAG, "SD card unmounted.");
}

/* Buttons test: read current state and prompt user to press */
static void test_buttons(void)
{
    ESP_LOGI(TAG, "Reading buttons (BTN1 GPIO %d, BTN2 GPIO %d).", BTN1_GPIO, BTN2_GPIO);
    int s1 = gpio_get_level(BTN1_GPIO);
    int s2 = gpio_get_level(BTN2_GPIO);
    ESP_LOGI(TAG, "  Initial state BTN1=%d, BTN2=%d", s1, s2);
    ESP_LOGI(TAG, "Please press each button now... (10s window)");

    for (int i = 0; i < 10; i++) {
        int a = gpio_get_level(BTN1_GPIO);
        int b = gpio_get_level(BTN2_GPIO);
        if (a != s1) {
            ESP_LOGI(TAG, "  BTN1 changed to %d", a);
            s1 = a;
        }
        if (b != s2) {
            ESP_LOGI(TAG, "  BTN2 changed to %d", b);
            s2 = b;
        }
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
    ESP_LOGI(TAG, "Button test complete. Final states BTN1=%d BTN2=%d", s1, s2);
}

/* Application entrypoint */
void app_main(void)
{
    esp_log_level_set("*", ESP_LOG_INFO);
    ESP_LOGI(TAG, "Starting device diag test...");

    /* I2C init */
    if (i2c_init() != ESP_OK) {
        ESP_LOGE(TAG, "I2C init failed. Check SDA/SCL wiring.");
    } else {
        ESP_LOGI(TAG, "I2C initialized.");
    }

    /* Initialize buzzer & buttons */
    buzzer_init();
    buttons_init();

    /* Small buzzer confirmation */
    ESP_LOGI(TAG, "Beeping buzzer briefly...");
    buzzer_beep(200);
    vTaskDelay(pdMS_TO_TICKS(200));

    /* Test I2C devices */
    test_i2c_devices();

    /* Test SD card */
    test_sd_card();

    /* Test buttons (interactive) */
    test_buttons();

    ESP_LOGI(TAG, "All tests executed. Looping status checks every 30s. Press Ctrl+C to stop logs.");
    while (1) {
        // Periodic heartbeat beep and I2C quick re-check
        ESP_LOGI(TAG, "Heartbeat: quick I2C scan for 0x3C and 0x68...");
        if (i2c_scan_and_check(I2C_MASTER_NUM, SSD1306_I2C_ADDR) == ESP_OK) {
            ESP_LOGI(TAG, "  SSD1306 present.");
        } else {
            ESP_LOGW(TAG, "  SSD1306 not present.");
        }
        if (i2c_scan_and_check(I2C_MASTER_NUM, MPU6050_I2C_ADDR) == ESP_OK) {
            ESP_LOGI(TAG, "  MPU6050 present.");
        } else {
            ESP_LOGW(TAG, "  MPU6050 not present.");
        }
        buzzer_beep(60);
        vTaskDelay(pdMS_TO_TICKS(30000));
    }
}