#include <SoftI2C.h>

// Address of the I2C OLED 
#define OLED_I2C_ADDRESS 0x3C

// Commands for SSD1306
#define SSD1306_DISPLAYOFF          0xAE
#define SSD1306_SETDISPLAYCLOCKDIV  0xD5
#define SSD1306_SETMULTIPLEX        0xA8
#define SSD1306_SETDISPLAYOFFSET    0xD3
#define SSD1306_SETSTARTLINE        0x40
#define SSD1306_CHARGEPUMP          0x8D
#define SSD1306_MEMORYMODE          0x20
#define SSD1306_SEGREMAP            0xA1
#define SSD1306_COMSCANDEC          0xC8
#define SSD1306_SETCOMPINS          0xDA
#define SSD1306_SETCONTRAST         0x81
#define SSD1306_SETPRECHARGE        0xD9
#define SSD1306_SETVCOMDETECT       0xDB
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_NORMALDISPLAY       0xA6
#define SSD1306_DISPLAYON           0xAF

void I2CWriteCommand(uint8_t command) {
  I2CStart();
  I2CSend(OLED_I2C_ADDRESS << 1); // I2C address + Write bit
  I2CSend(0x00); // Co = 0, D/C# = 0
  I2CSend(command);
  I2CStop();
}

void I2CWriteData(uint8_t data) {
  I2CStart();
  I2CSend(OLED_I2C_ADDRESS << 1); // I2C address + Write bit
  I2CSend(0x40); // Co = 0, D/C# = 1
  I2CSend(data);
  I2CStop();
}

void initializeOLED() {
  I2CWriteCommand(SSD1306_DISPLAYOFF);
  I2CWriteCommand(SSD1306_SETDISPLAYCLOCKDIV);
  I2CWriteCommand(0x80); // Suggested value
  I2CWriteCommand(SSD1306_SETMULTIPLEX);
  I2CWriteCommand(0x3F);
  I2CWriteCommand(SSD1306_SETDISPLAYOFFSET);
  I2CWriteCommand(0x00);
  I2CWriteCommand(SSD1306_SETSTARTLINE | 0x0);
  I2CWriteCommand(SSD1306_CHARGEPUMP);
  I2CWriteCommand(0x14);
  I2CWriteCommand(SSD1306_MEMORYMODE);
  I2CWriteCommand(0x00);
  I2CWriteCommand(SSD1306_SEGREMAP | 0x1);
  I2CWriteCommand(SSD1306_COMSCANDEC);
  I2CWriteCommand(SSD1306_SETCOMPINS);
  I2CWriteCommand(0x12);
  I2CWriteCommand(SSD1306_SETCONTRAST);
  I2CWriteCommand(0xCF);
  I2CWriteCommand(SSD1306_SETPRECHARGE);
  I2CWriteCommand(0xF1);
  I2CWriteCommand(SSD1306_SETVCOMDETECT);
  I2CWriteCommand(0x40);
  I2CWriteCommand(SSD1306_DISPLAYALLON_RESUME);
  I2CWriteCommand(SSD1306_NORMALDISPLAY);
  I2CWriteCommand(SSD1306_DISPLAYON);
}

void clearScreen(){
  for (uint8_t i = 0; i < 8; i++) {
    I2CWriteCommand(0xB0 + i); // Set page address
    I2CWriteCommand(0x00); // Set lower column address
    I2CWriteCommand(0x10); // Set higher column address
    for (uint8_t j = 0; j < 128; j++) {
      I2CWriteData(0x00);
    }
  }
}

void setup() {
  // Initialize I2C pins
  scl_pin = 30; // P3.0
  sda_pin = 31; // P3.1
  I2CInit();
  initializeOLED();
}

void loop() {
  // Clear the display
 clearScreen();


  delay(1000);
}