#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"

#define OLED_ADDR 0x3C

#define UART_ID uart0
#define BAUD_RATE 115200
#define UART_TX_PIN 0
#define UART_RX_PIN 1

void send_command(uint8_t cmd) {
    uint8_t buf[2] = {0x00, cmd};
    i2c_write_blocking(i2c0, OLED_ADDR, buf, 2, false);
    printf("Sent command: 0x%02X\n", cmd);  // Debug output
}

void send_data(uint8_t data) {
    uint8_t buf[2] = {0x40, data};
    i2c_write_blocking(i2c0, OLED_ADDR, buf, 2, false);
}

void init_display() {
    printf("Initializing display...\n");
    
    sleep_ms(100);
    
    // SSD1306 initialization sequence
    send_command(0xAE);        // Display off
    send_command(0xD5);        // Set display clock divide ratio/oscillator frequency
    send_command(0x80);        // Suggested ratio
    send_command(0xA8);        // Set multiplex ratio
    send_command(0x3F);        // 128x64
    send_command(0xD3);        // Set display offset
    send_command(0x00);        // No offset
    send_command(0x40);        // Set start line address
    send_command(0x8D);        // Set charge pump
    send_command(0x14);        // Enable charge pump
    send_command(0x20);        // Set memory addressing mode
    send_command(0x00);        // Horizontal addressing mode
    send_command(0xA1);        // Set segment re-map
    send_command(0xC8);        // Set COM output scan direction
    send_command(0xDA);        // Set COM pins hardware configuration
    send_command(0x12);
    send_command(0x81);        // Set contrast control
    send_command(0xCF);
    send_command(0xD9);        // Set pre-charge period
    send_command(0xF1);
    send_command(0xDB);        // Set VCOMH deselect level
    send_command(0x40);
    send_command(0xA4);        // Resume to RAM content display
    send_command(0xA6);        // Normal display
    send_command(0xAF);        // Display on
    
    printf("Display initialization complete\n");
}

void draw_test_pattern() {
    printf("Drawing test pattern...\n");
    
    // Set page address
    send_command(0x22);  // Page address command
    send_command(0x00);  // Start page
    send_command(0x07);  // End page
    
    // Set column address
    send_command(0x21);  // Column address command
    send_command(0x00);  // Start column
    send_command(0x7F);  // End column
    
    // Draw pattern
    for (int page = 0; page < 8; page++) {
        for (int col = 0; col < 128; col++) {
//            send_data((col + page) % 2 ? 0xFF : 0x00);  // Checkerboard pattern
              if ((col + page) % 2 == 1) {
                  send_data(0xFF);  // Define um padrão de tabuleiro de xadrez
              } else {
                  send_data(0x00);
              }
        }
    }
    
    printf("Test pattern complete\n");
}

int main() {

    uart_init(UART_ID, BAUD_RATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    stdio_init_all();
    
    // Wait for serial to be available
    sleep_ms(1000);
    
    printf("Starting OLED test program...\n");
    
    // Initialize I2C
    i2c_init(i2c0, 400000);
    gpio_set_function(16, GPIO_FUNC_I2C);
    gpio_set_function(17, GPIO_FUNC_I2C);
    gpio_pull_up(16);
    gpio_pull_up(17);
    
    printf("I2C initialized\n");
    
    // Scan for I2C devices
    printf("Scanning I2C bus...\n");
    for (int addr = 0; addr < 128; addr++) {
        uint8_t rxdata;
        int ret = i2c_read_blocking(i2c0, addr, &rxdata, 1, false);
        if (ret >= 0) {
            printf("Found I2C device at address: 0x%02X\n", addr);
            printf("Found I2C device at address: %d\n", addr);
        }
    }
    
    init_display();
    
    while (1) {
        printf("Drawing new pattern...\n");
        draw_test_pattern();
        sleep_ms(2000);
    }
    
    return 0;
}