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

// I2C config
#define I2C_PORT i2c0
#define OLED_ADDR 0x3C
#define SDA_PIN 4
#define SCL_PIN 5

#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_PAGES (OLED_HEIGHT / 8)

// Send command to OLED
void ssd1306_cmd(uint8_t cmd) {
  uint8_t buf[2] = {0x00, cmd};
  i2c_write_blocking(I2C_PORT, OLED_ADDR, buf, 2, false);
}

// Send data to OLED
void ssd1306_data(uint8_t *data, size_t len) {
  uint8_t buf[129];
  buf[0] = 0x40;
  memcpy(&buf[1], data, len);
  i2c_write_blocking(I2C_PORT, OLED_ADDR, buf, len + 1, false);
}

// Initialize the OLED
void ssd1306_init() {
  sleep_ms(100); // Power-up delay

  const uint8_t init_seq[] = {
    0xAE,  // Display off
    0xD5, 0x80, 0xA8, 0x3F, 0xD3, 0x00, 0x40, 0x8D, 0x14, 0x20, 0x00, 0xA1, 0xC8, 0xDA, 0x12, 0x81, 0xCF, 0xD9, 0xF1, 0xDB, 0x40, 0xA4, 0xA6,
    0xAF // Display on
  };
  for (uint i = 0; i < sizeof init_seq; ++i) ssd1306_cmd(init_seq[i]);
}

// Clear the display
void ssd1306_clear() {
  for (uint8_t page = 0; page < OLED_PAGES; page++) {
    ssd1306_cmd(0xB0 + page);  // Set page
    ssd1306_cmd(0x00);         // Set lower column
    ssd1306_cmd(0x10);         // Set higher column

    uint8_t zeros[128] = {0};
    ssd1306_data(zeros, 128);
  }
}

// Set cursor
void ssd1306_set_cursor(uint8_t col, uint8_t page) {
  ssd1306_cmd(0xB0 + page);
  ssd1306_cmd(0x00 + (col & 0x0F));
  ssd1306_cmd(0x10 + ((col >> 4) & 0x0F));
}

// Draw single character
void ssd1306_draw_char(char c) {
  if (c < 32 || c > 127) c = '?';
  ssd1306_data((uint8_t *)font5x7[c - 32], 5);
  uint8_t space = 0x00;
  ssd1306_data(&space, 1); // spacing
}

// Draw string
void ssd1306_draw_string(const char *str) {
  while (*str) {
    ssd1306_draw_char(*str++);
  }
}

int main() {
  stdio_init_all();

  i2c_init(I2C_PORT, 400 * 1000);
  gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
  gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
  gpio_pull_up(SDA_PIN);
  gpio_pull_up(SCL_PIN);

  ssd1306_init();
  ssd1306_clear();

  ssd1306_set_cursor(0, 0); // Top-left corner
  ssd1306_draw_string("Hello, World!");

  while (1) {
    tight_loop_contents();
  }
}