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

// I2C configuration for LCD via PCF8574
#define I2C_PORT     i2c1
#define I2C_SDA_PIN  2
#define I2C_SCL_PIN  3
#define LCD_ADDR     0x27  // or 0x3F for some displays
#define I2C_BAUDRATE 400000

// LCD control bits
#define LCD_RS       0x01  // Register select
#define LCD_EN       0x04  // Enable
#define LCD_BL       0x08  // Backlight

// LCD commands
#define LCD_CLEAR    0x01
#define LCD_HOME     0x02
#define LCD_ENTRY    0x06
#define LCD_DISPLAY  0x0C
#define LCD_FUNCTION 0x28  // 4-bit, 2 lines, 5x8 font

// Write data to PCF8574
static void pcf8574_write(uint8_t data) {
    i2c_write_blocking(I2C_PORT, LCD_ADDR, &data, 1, false);
}

// Pulse enable pin
static void lcd_pulse(uint8_t data) {
    pcf8574_write(data | LCD_EN);
    tight_loop_contents();
    pcf8574_write(data & ~LCD_EN);
}

// Write 4-bit nibble to LCD
static void lcd_write_nibble(uint8_t nibble, bool rs) {
    uint8_t data = (nibble & 0xF0) | LCD_BL | (rs ? LCD_RS : 0);
    lcd_pulse(data);
}

// Write 8-bit data to LCD
static void lcd_write_byte(uint8_t byte, bool rs) {
    lcd_write_nibble(byte, rs);
    lcd_write_nibble(byte << 4, rs);
}

// Send command to LCD
static void lcd_command(uint8_t cmd) {
    lcd_write_byte(cmd, false);
}

// Send data to LCD
static void lcd_data(uint8_t data) {
    lcd_write_byte(data, true);
}

// Initialize LCD
static void lcd_init() {
    // Initialize I2C
    i2c_init(I2C_PORT, I2C_BAUDRATE);
    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SDA_PIN);
    gpio_pull_up(I2C_SCL_PIN);
    sleep_ms(50);

    // LCD initialization sequence
    lcd_write_nibble(0x30, false); sleep_ms(5);
    lcd_write_nibble(0x30, false); sleep_ms(1);
    lcd_write_nibble(0x30, false); sleep_ms(1);
    lcd_write_nibble(0x20, false); // Set 4-bit mode

    lcd_command(LCD_FUNCTION); // 4-bit, 2 lines, 5x8 font
    lcd_command(LCD_DISPLAY);  // Display on, cursor off
    lcd_command(LCD_CLEAR);    // Clear display
    sleep_ms(2);
    lcd_command(LCD_ENTRY);    // Entry mode, cursor moves right
}

// Set cursor position
static void lcd_set_cursor(uint8_t row, uint8_t col) {
    static const uint8_t row_offsets[] = {0x00, 0x40, 0x14, 0x54};
    lcd_command(0x80 | (col + row_offsets[row]));
}

// Print string to LCD
static void lcd_print(const char *str) {
    while (*str) lcd_data(*str++);
}

int main() {
    stdio_init_all();
    lcd_init();

    lcd_command(LCD_CLEAR);
    sleep_ms(2);
    lcd_set_cursor(0, 0);
    lcd_print("Hello, World!");
    lcd_set_cursor(1, 0);

    while (true) {
        sleep_ms(100); // Keep display active
    }
    return 0;
}