/*
 * EMOTIONAL ROBOT - CONTINUOUS GPIO DEBUG
 * Shows GPIO levels continuously
 */

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

#define BTN_HAPPY 0
#define BTN_SAD 1
#define BTN_LOVE 2
#define BTN_ANGRY 3
#define BTN_MISS 6

#define I2C_MASTER_SCL_IO 5
#define I2C_MASTER_SDA_IO 4
#define I2C_MASTER_NUM I2C_NUM_0
#define I2C_MASTER_FREQ_HZ 100000
#define OLED_ADDR 0x3C

#define EMOTION_HAPPY 0
#define EMOTION_SAD 1
#define EMOTION_LOVE 2
#define EMOTION_ANGRY 3
#define EMOTION_MISS 4

uint8_t display_buffer[1024];
int current_emotion = EMOTION_HAPPY;

void i2c_init(void) {
    printf("[1] Starting I2C init...\n");
    fflush(stdout);
    
    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,
    };
    
    i2c_param_config(I2C_MASTER_NUM, &conf);
    i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
    
    printf("✓ I2C initialized\n");
    fflush(stdout);
}

void oled_cmd(uint8_t cmd) {
    i2c_cmd_handle_t handle = i2c_cmd_link_create();
    i2c_master_start(handle);
    i2c_master_write_byte(handle, (OLED_ADDR << 1) | 0, 1);
    i2c_master_write_byte(handle, 0x00, 1);
    i2c_master_write_byte(handle, cmd, 1);
    i2c_master_stop(handle);
    i2c_master_cmd_begin(I2C_MASTER_NUM, handle, pdMS_TO_TICKS(100));
    i2c_cmd_link_delete(handle);
}

void oled_data(uint8_t *data, uint16_t len) {
    i2c_cmd_handle_t handle = i2c_cmd_link_create();
    i2c_master_start(handle);
    i2c_master_write_byte(handle, (OLED_ADDR << 1) | 0, 1);
    i2c_master_write_byte(handle, 0x40, 1);
    i2c_master_write(handle, data, len, 1);
    i2c_master_stop(handle);
    i2c_master_cmd_begin(I2C_MASTER_NUM, handle, pdMS_TO_TICKS(100));
    i2c_cmd_link_delete(handle);
}

void oled_init(void) {
    printf("[2] Starting OLED init...\n");
    fflush(stdout);
    
    vTaskDelay(pdMS_TO_TICKS(100));
    
    oled_cmd(0xAE);
    oled_cmd(0xD5);
    oled_cmd(0x80);
    oled_cmd(0xA8);
    oled_cmd(0x3F);
    oled_cmd(0xD3);
    oled_cmd(0x00);
    oled_cmd(0x40);
    oled_cmd(0x8D);
    oled_cmd(0x14);
    oled_cmd(0x20);
    oled_cmd(0x00);
    oled_cmd(0xA1);
    oled_cmd(0xC8);
    oled_cmd(0xDA);
    oled_cmd(0x12);
    oled_cmd(0x81);
    oled_cmd(0xCF);
    oled_cmd(0xD9);
    oled_cmd(0xF1);
    oled_cmd(0xDB);
    oled_cmd(0x40);
    oled_cmd(0xA4);
    oled_cmd(0xA6);
    oled_cmd(0xAF);
    
    printf("✓ OLED initialized\n");
    fflush(stdout);
}

void clear_display(void) {
    memset(display_buffer, 0x00, 1024);
}

void draw_pixel(int x, int y) {
    if (x < 0 || x >= 128 || y < 0 || y >= 64) return;
    int byte_idx = (y / 8) * 128 + x;
    int bit = y % 8;
    display_buffer[byte_idx] |= (1 << bit);
}

void display_frame(void) {
    oled_cmd(0x21);
    oled_cmd(0x00);
    oled_cmd(0x7F);
    oled_cmd(0x22);
    oled_cmd(0x00);
    oled_cmd(0x07);
    
    for (int i = 0; i < 8; i++) {
        oled_data(&display_buffer[i * 128], 128);
    }
}

void show_happy(void) {
    printf("→ Happy 😊\n");
    fflush(stdout);
    clear_display();
    
    for (int i = 0; i < 30; i++) {
        draw_pixel(50 + i, 30);
        draw_pixel(50 + i, 35);
    }
    
    display_frame();
}

void show_sad(void) {
    printf("→ Sad 😢\n");
    fflush(stdout);
    clear_display();
    
    for (int i = 0; i < 30; i++) {
        draw_pixel(50 + i, 30);
        draw_pixel(50 + i, 40);
    }
    
    display_frame();
}

void show_love(void) {
    printf("→ Love ❤️\n");
    fflush(stdout);
    clear_display();
    
    for (int i = 0; i < 30; i++) {
        for (int j = 0; j < 10; j++) {
            draw_pixel(50 + i, 25 + j);
        }
    }
    
    display_frame();
}

void show_angry(void) {
    printf("→ Angry 😡\n");
    fflush(stdout);
    clear_display();
    
    for (int i = 0; i < 30; i++) {
        draw_pixel(50 + i, 20 + i/3);
        draw_pixel(50 + i, 45 - i/3);
    }
    
    display_frame();
}

void show_miss(void) {
    printf("→ Miss You 😔\n");
    fflush(stdout);
    clear_display();
    
    for (int i = 0; i < 30; i++) {
        draw_pixel(50 + i, 32 + i/10);
    }
    
    display_frame();
}

void gpio_init(void) {
    printf("[3] Starting GPIO init...\n");
    fflush(stdout);
    
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << BTN_HAPPY) | (1ULL << BTN_SAD) | 
                       (1ULL << BTN_LOVE) | (1ULL << BTN_ANGRY) | (1ULL << BTN_MISS),
        .pull_down_en = GPIO_PULLDOWN_ENABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE,
    };
    
    gpio_config(&io_conf);
    
    printf("✓ GPIO initialized\n");
    fflush(stdout);
}

void print_gpio_status(void) {
    int btn0 = gpio_get_level(BTN_HAPPY);
    int btn1 = gpio_get_level(BTN_SAD);
    int btn2 = gpio_get_level(BTN_LOVE);
    int btn3 = gpio_get_level(BTN_ANGRY);
    int btn6 = gpio_get_level(BTN_MISS);
    
    printf("[GPIO STATUS] 0:%d  1:%d  2:%d  3:%d  6:%d\n", btn0, btn1, btn2, btn3, btn6);
    fflush(stdout);
    
    // Check for any HIGH value
    if (btn0 == 1) {
        printf(">>> GPIO 0 HIGH! Changing to HAPPY\n");
        fflush(stdout);
        current_emotion = EMOTION_HAPPY;
        show_happy();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    
    if (btn1 == 1) {
        printf(">>> GPIO 1 HIGH! Changing to SAD\n");
        fflush(stdout);
        current_emotion = EMOTION_SAD;
        show_sad();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    
    if (btn2 == 1) {
        printf(">>> GPIO 2 HIGH! Changing to LOVE\n");
        fflush(stdout);
        current_emotion = EMOTION_LOVE;
        show_love();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    
    if (btn3 == 1) {
        printf(">>> GPIO 3 HIGH! Changing to ANGRY\n");
        fflush(stdout);
        current_emotion = EMOTION_ANGRY;
        show_angry();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    
    if (btn6 == 1) {
        printf(">>> GPIO 6 HIGH! Changing to MISS YOU\n");
        fflush(stdout);
        current_emotion = EMOTION_MISS;
        show_miss();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void app_main(void) {
    printf("\n\n");
    printf("████████████████████████████████████\n");
    printf("  CONTINUOUS GPIO MONITORING\n");
    printf("████████████████████████████████████\n\n");
    fflush(stdout);
    
    i2c_init();
    vTaskDelay(pdMS_TO_TICKS(100));
    
    oled_init();
    vTaskDelay(pdMS_TO_TICKS(100));
    
    gpio_init();
    vTaskDelay(pdMS_TO_TICKS(100));
    
    printf("\n✓ All systems ready!\n");
    printf("✓ Showing default emotion...\n\n");
    fflush(stdout);
    
    show_happy();
    
    printf("\n████████████████████████████████████\n");
    printf("GPIO MONITORING ACTIVE\n");
    printf("Watch GPIO levels below:\n");
    printf("Format: [GPIO STATUS] 0:X  1:X  2:X  3:X  6:X\n");
    printf("  0 = LOW (button not pressed)\n");
    printf("  1 = HIGH (button pressed)\n");
    printf("████████████████████████████████████\n\n");
    fflush(stdout);
    
    int loop_count = 0;
    while (1) {
        print_gpio_status();
        vTaskDelay(pdMS_TO_TICKS(1000));  // Print every 1 second
        loop_count++;
    }
}