---

#include <Wire.h>

#define SSD1306_ADDR 0x3C
#define WIDTH 128
#define HEIGHT 64

uint8_t buffer[1024];

void ssd1306_cmd(uint8_t cmd) {
  Wire.beginTransmission(SSD1306_ADDR);
  Wire.write(0x00);
  Wire.write(cmd);
  Wire.endTransmission();
}

void ssd1306_data(uint8_t data) {
  Wire.beginTransmission(SSD1306_ADDR);
  Wire.write(0x40);
  Wire.write(data);
  Wire.endTransmission();
}

void init_display() {
  ssd1306_cmd(0xAE);
  ssd1306_cmd(0xD5); ssd1306_cmd(0x80);
  ssd1306_cmd(0xA8); ssd1306_cmd(0x3F);
  ssd1306_cmd(0xD3); ssd1306_cmd(0x00);
  ssd1306_cmd(0x40);
  ssd1306_cmd(0x8D); ssd1306_cmd(0x14);
  ssd1306_cmd(0x20); ssd1306_cmd(0x00);
  ssd1306_cmd(0xA1);
  ssd1306_cmd(0xC8);
  ssd1306_cmd(0xDA); ssd1306_cmd(0x12);
  ssd1306_cmd(0x81); ssd1306_cmd(0xCF);
  ssd1306_cmd(0xD9); ssd1306_cmd(0xF1);
  ssd1306_cmd(0xDB); ssd1306_cmd(0x40);
  ssd1306_cmd(0xA4);
  ssd1306_cmd(0xA6);
  ssd1306_cmd(0xAF);
}

void clear_buffer() {
  for(int i = 0; i < 1024; i++) buffer[i] = 0;
}

void draw_pixel(int x, int y) {
  if(x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT) return;
  buffer[x + (y / 8) * WIDTH] |= (1 << (y & 7));
}

void draw_line(int x0, int y0, int x1, int y1) {
  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
  int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
  int err = dx + dy, e2;
  
  while(true) {
    draw_pixel(x0, y0);
    if(x0 == x1 && y0 == y1) break;
    e2 = 2 * err;
    if(e2 >= dy) { err += dy; x0 += sx; }
    if(e2 <= dx) { err += dx; y0 += sy; }
  }
}

void update_display() {
  ssd1306_cmd(0x21); ssd1306_cmd(0); ssd1306_cmd(127);
  ssd1306_cmd(0x22); ssd1306_cmd(0); ssd1306_cmd(7);
  
  Wire.beginTransmission(SSD1306_ADDR);
  Wire.write(0x40);
  for(int i = 0; i < 1024; i++) Wire.write(buffer[i]);
  Wire.endTransmission();
}

void setup() {
  Wire.begin();
  init_display();
  clear_buffer();
  
  for(int i = 1; i < 3; i++) {
    draw_line(i * 42, 0, i * 42, 63);
    draw_line(0, i * 21, 127, i * 21);
  }
  
  update_display();
}

void loop() {}