#include "stm32c0xx_hal.h"
#include <string.h>

/* I2C Handle */
I2C_HandleTypeDef hi2c1;

/* SSD1306 I2C Address */
#define SSD1306_ADDR  (0x3C << 1)

/* Function Prototypes */
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);

/* OLED Functions */
void SSD1306_Command(uint8_t cmd);
void SSD1306_Data(uint8_t *data, uint16_t size);
void SSD1306_Init(void);
void SSD1306_Clear(void);
void SSD1306_SetCursor(uint8_t x, uint8_t page);
void SSD1306_Print(char *str);

/* Simple Font (5x7 for A only demo) */
const uint8_t font_A[5] = {0x7E,0x11,0x11,0x11,0x7E};

int main(void)
{
  HAL_Init();
  SystemClock_Config();

  MX_GPIO_Init();
  MX_I2C1_Init();

  SSD1306_Init();
  SSD1306_Clear();

  SSD1306_SetCursor(0,0);
  SSD1306_Print("VECTOR OLED TEST");

  SSD1306_SetCursor(0,2);
  SSD1306_Print("HELLO WOKWI");

  while (1)
  {
  }
}

void SSD1306_Command(uint8_t cmd)
{
  uint8_t data[2];
  data[0] = 0x00;   // Command mode
  data[1] = cmd;
  HAL_I2C_Master_Transmit(&hi2c1, SSD1306_ADDR, data, 2, HAL_MAX_DELAY);
}

void SSD1306_Data(uint8_t *data, uint16_t size)
{
  uint8_t buffer[129];
  buffer[0] = 0x40; // Data mode

  for(int i=0;i<size;i++)
    buffer[i+1] = data[i];

  HAL_I2C_Master_Transmit(&hi2c1, SSD1306_ADDR, buffer, size+1, HAL_MAX_DELAY);
}

void SSD1306_Init(void)
{
  HAL_Delay(100);

  SSD1306_Command(0xAE); // Display OFF
  SSD1306_Command(0x20); // Address mode
  SSD1306_Command(0x00); // Horizontal mode

  SSD1306_Command(0xB0);
  SSD1306_Command(0xC8);
  SSD1306_Command(0x00);
  SSD1306_Command(0x10);

  SSD1306_Command(0x40);
  SSD1306_Command(0x81);
  SSD1306_Command(0xFF);

  SSD1306_Command(0xA1);
  SSD1306_Command(0xA6);

  SSD1306_Command(0xA8);
  SSD1306_Command(0x3F);

  SSD1306_Command(0xD3);
  SSD1306_Command(0x00);

  SSD1306_Command(0xD5);
  SSD1306_Command(0xF0);

  SSD1306_Command(0xD9);
  SSD1306_Command(0x22);

  SSD1306_Command(0xDA);
  SSD1306_Command(0x12);

  SSD1306_Command(0xDB);
  SSD1306_Command(0x20);

  SSD1306_Command(0x8D);
  SSD1306_Command(0x14);

  SSD1306_Command(0xAF); // Display ON
}

void SSD1306_Clear(void)
{
  uint8_t clear[128];

  for(int i=0;i<128;i++)
    clear[i] = 0x00;

  for(uint8_t page=0; page<8; page++)
  {
    SSD1306_Command(0xB0 + page);
    SSD1306_Command(0x00);
    SSD1306_Command(0x10);

    SSD1306_Data(clear,128);
  }
}

void SSD1306_SetCursor(uint8_t x, uint8_t page)
{
  SSD1306_Command(0xB0 + page);
  SSD1306_Command((x & 0x0F));
  SSD1306_Command(0x10 | (x >> 4));
}

void SSD1306_Print(char *str)
{
  while(*str)
  {
    uint8_t blank[6] = {0x00,0x00,0x00,0x00,0x00,0x00};

    // Simple placeholder (no font yet)
    SSD1306_Data(blank,6);

    str++;
  }
}

static void MX_I2C1_Init(void)
{
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x00303D5B; // 100kHz @ 48MHz
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;

  HAL_I2C_Init(&hi2c1);
}

static void MX_GPIO_Init(void)
{
  __HAL_RCC_GPIOB_CLK_ENABLE();

  GPIO_InitTypeDef GPIO_InitStruct = {0};

  GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; // PB6=SCL, PB7=SDA
  GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.Alternate = GPIO_AF6_I2C1;

  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}