#include <Wire.h>

#define SSD1306_MEMORYMODE                           0x20
#define SSD1306_COLUMNADDR                           0x21
#define SSD1306_PAGEADDR                             0x22
#define SSD1306_SETCONTRAST                          0x81
#define SSD1306_CHARGEPUMP                           0x8D
#define SSD1306_SEGREMAP                             0xA0
#define SSD1306_DISPLAYALLON_RESUME                  0xA4
#define SSD1306_DISPLAYALLON                         0xA5
#define SSD1306_NORMALDISPLAY                        0xA6
#define SSD1306_INVERTDISPLAY                        0xA7
#define SSD1306_SETMULTIPLEX                         0xA8
#define SSD1306_DISPLAYOFF                           0xAE
#define SSD1306_DISPLAYON                            0xAF
#define SSD1306_COMSCANINC                           0xC0
#define SSD1306_COMSCANDEC                           0xC8
#define SSD1306_SETDISPLAYOFFSET                     0xD3
#define SSD1306_SETDISPLAYCLOCKDIV                   0xD5
#define SSD1306_SETPRECHARGE                         0xD9
#define SSD1306_SETCOMPINS                           0xDA
#define SSD1306_SETVCOMDETECT                        0xDB

#define SSD1306_SETLOWCOLUMN                         0x00
#define SSD1306_SETHIGHCOLUMN                        0x10
#define SSD1306_SETSTARTLINE                         0x40

// TODO: remove
#define SSD1306_EXTERNALVCC                          0x01 // External display voltage source
#define SSD1306_SWITCHCAPVCC                         0x02 // Gen. display voltage from 3.3V

#define SSD1306_RIGHT_HORIZONTAL_SCROLL              0x26 // Init rt scroll
#define SSD1306_LEFT_HORIZONTAL_SCROLL               0x27 // Init left scroll
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29 // Init diag scroll
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL  0x2A // Init diag scroll
#define SSD1306_DEACTIVATE_SCROLL                    0x2E // Stop scroll
#define SSD1306_ACTIVATE_SCROLL                      0x2F // Start scroll
#define SSD1306_SET_VERTICAL_SCROLL_AREA             0xA3 // Set scroll range

#define WIDTH      85
#define HEIGHT     64
#define BUFFERSIZE (WIDTH * HEIGHT / 8)
#define I2C_ADDR   0x3c
#define OLED_RESET 4

uint8_t framebuffer[BUFFERSIZE] = {};

void pixel_set(int x, int y, bool value) {
	uint8_t  bitmask   = (1 << (y & 0x7));
	uint8_t* offset = &buffer[x + (y / 8) * WIDTH];
	if (value) *offset |= bitmask;
	else       *offset &= ~bitmask;
}

void pixel_flip(int x, int y) {
	uint8_t  bitmask   = (1 << (y & 0x7));
	uint8_t* offset = &buffer[x + (y / 8) * WIDTH];
	*offset ^= bitmask;
}

uint8_t* decode_frame(uint8_t* data) {
	int pixel_x = 0;
	int pixel_y = 0;
	while (pixel_x != WIDHT-1 && pixel_y != HEIGHT-1) {
		uint8_t byte = *data++;

		int do_flip    = byte & 0x80;
		int n_affected = byte & 0x3f;
		if (byte & 0x40) {
			n_affected = (n_affected << 8) | *(data++);
		}

		// idea: traverse along y instead, to optimize this logic
		while (n_affected--) {
			if (do_flip) pixel_flip(pixel_x, pixel_y);
			if (pixel_x == WIDTH-1) {
				pixel_x = 0;
				pixel_y++;
			} else {
				pixel_x++;
			}
		}
	}
	return data; // returns the next frame offset
}

void ssd1306_command(uint8_t c) {
	Wire->beginTransmission(I2C_ADDR);
	Wire.write(0x00); // Co = 0, D/C = 0
	Wire.write(c);
	Wire->endTransmission();
}

void init_display(int reset_pin, bool vcc_is_5v_external) {
	Wire.begin();
	pinMode(OLED_RESET, OUTPUT);

	// reset
    digitalWrite(OLED_RESET, HIGH);
    delay(1);
    digitalWrite(OLED_RESET, LOW);
    delay(10);
    digitalWrite(OLED_RESET, HIGH);

	// init sequence
	ssd1306_command(SSD1306_DISPLAYOFF);
	ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV);
	ssd1306_command(0x80);                       // the suggested ratio
	ssd1306_command(SSD1306_SETMULTIPLEX);
	ssd1306_command(HEIGHT - 1);

	ssd1306_command(SSD1306_SETDISPLAYOFFSET);
	ssd1306_command(0x00);                       // no offset
	ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // (line #0)
	ssd1306_command(SSD1306_CHARGEPUMP);

	ssd1306_command((vcc_is_5v_external) ? 0x10 : 0x14);

	ssd1306_command(SSD1306_MEMORYMODE);         // 0x20
	ssd1306_command(0x00);                       // 0x0 act like ks0108
	ssd1306_command(SSD1306_SEGREMAP | 0x1);
	ssd1306_command(SSD1306_COMSCANDEC);

	uint8_t comPins, contrast;
	if      ((WIDTH == 128) && (HEIGHT == 64)) { comPins = 0x12; contrast = (vcc_is_5v_external) ? 0x9F : 0xCF; }
	//else if ((WIDTH == 128) && (HEIGHT == 32)) { comPins = 0x02; contrast = 0x8F; }
	//else if ((WIDTH ==  96) && (HEIGHT == 16)) { comPins = 0x02; contrast = (vcc_is_5v_external) ? 0x10 : 0xAF; }
	//else                                       { /* Other screen varieties -- TBD */ }

	ssd1306_command(SSD1306_SETCOMPINS);
    ssd1306_command(comPins);
    ssd1306_command(SSD1306_SETCONTRAST);
    ssd1306_command(contrast);

	ssd1306_command(SSD1306_SETPRECHARGE);       // 0xd9
    ssd1306_command((vcc_is_5v_external) ? 0x22 : 0xF1);

	ssd1306_command(SSD1306_SETVCOMDETECT);
	ssd1306_command(0x40);
	ssd1306_command(SSD1306_DISPLAYALLON_RESUME);
	ssd1306_command(SSD1306_NORMALDISPLAY);
	ssd1306_command(SSD1306_DEACTIVATE_SCROLL);
	ssd1306_command(SSD1306_DISPLAYON);          // Main screen turn on

	ssd1306_command();
}

void send_framebuffer() {
	int buffersize = 32;
	Wire.beginTransmission(I2C_ADDR);
	Wire.write(0x40);
	for (int i = 0; i < BUFFERSIZE; i++) {
		if (!--buffersize) {
			Wire.endTransmission();
			Wire.beginTransmission(I2C_ADDR);
			buffersize = 32;
		}
		Wire.write(framebuffer[i]);
	}
	Wire.endTransmission();
}


void Setup() {
	init_display();
	send_framebuffer();
}

Void Loop() {

}