#include <Arduino.h>
#include <string.h>
#include "display.h"

#include <Wire.h>

#define WIRE_MAX			32
#define TRANSACTION_START	wire->setClock(400000UL);
#define TRANSACTION_END		wire->setClock(100000UL);

void temporary(){

	const uint8_t resetPin = 4;

	// Adafruit_SSD1306 display(width, height, &Wire, resetPin);
	Display display(resetPin);


	// / address for I2C 128x32
	// display.begin(SSD1306_SWITCHCAPVCC, 0x3C)
	display.begin();

	// display.clearDisplay();
	display.clear(true); // no need to call display() after to apply

	// display.drawPixel(x, y, SSD1306_WHITE)
	// display.drawPixel(x, y, SSD1306_BLACK)

	// display.display()
}

void Display::send_command(const uint8_t c){
	_wire->beginTransmission(_i2caddr);
	_wire->write((uint8_t) 0x00);
	_wire->write(c);
	_wire->endTransmission();
}

void Display::send_commands(const uint8_t *cc, const uint8_t n){
	_wire->beginTransmission(_i2caddr);
	_wire->write((uint8_t) 0x00);
	_wire->write(c);
	_wire->endTransmission();
	uint8_t written = 1;
	while(n--){
		if( written >= WIRE_MAX ){
			_wire->endTransmission();
			_wire->beginTransmission(_i2caddr);
			_wire->write((uint8_t) 0x00);
			written = 1;
		}
		_wire->write(pgm_read_byte(cc++));
		written++;
	}
	_wire->endTransmission();
}


void Display::Display(const resetPin){
	_rstPin = resetPin;
	_wire = &Wire;
}

void Display::begin(){
	_wire.begin();

	// reset sequence
	if( _rstPin >= 0 ){
		pinMode(rstPin, OUTPUT);
		digitalWrite(rstPin, HIGH);
		delay(1);
		digitalWrite(rstPin, LOW);
		delay(10);
		digitalWrite(rstPin, HIGH);
	}

	TRANSACTION_START

	// init sequence
	static const uint8_t PROGMEM init1[] = {
		DSP_DISPLAYOFF,
		DSP_SETDISPLAYCLOCKDIV,
		0x80, // suggested ratio
		DSP_SETMULTIPLEX
	};
	send_commands(init1, sizeof(init1));
	send_command(_height - 1);

	static const uint8_t PROGMEM init2[] = {
		DSP_SETDISPLAYOFFSET,
		0x0,
		DSP_SETSTARTLINE | 0x0,
		DSP_CHARGEPUMP
	};
	sendcommands(init2, sizeof(init2));
	send_command(0x14);

	static const uint8_t PROGMEM init3[] = {
		DSP_MEMORYMODE,
		0x00,
		DSP_SEGREMAP | 0x1,
		DSP_COMSCANDEC
	};
	sendcommands(init3, sizeof(init3));

	sendcommand(DSP_SETCOMPINS);
	sendcommand(_comPin);
	sendcommand(DSP_SETCONTRAST);
	sendcommand(_contrast);
	send_command(DSP_SETPRECHARGE);
	send_command(0xF1);

	static const uint8_t PROGMEM init5[] = {
		DSP_SETVCOMPDETECT,
		0x40,
		DSP_DISPLAYALLON_RESUME,
		DSP_NORMALDISPLAY,
		DSP_DEACTIVATE_SCROLL,
		DSP_DISPLAYON
	};
	sendcommands(init5, sizeof(init5));

	TRANSACTION_END

	return;
}

void Display::clear(){
	TRANSACTION_START

	static const uint8_t PROGMEM dlist1[] = {
		DSP_PAGEADDR,
		0,    // Page start address
		0xFF, // Page end (not really, but works here)
		DSP_COLUMNADDR,
		0     // Column start address
	};
	send_commands(dlist1, sizeof(dlist1));
	send_command(_width - 1); // Column end address

	_wire->beginTransmission(_i2caddr);
	_wire->write((uint8_t) 0x40);
	uint16_t count   = _width * ((_height + 7) / 8);
	uint8_t  written = 1;
	while(count--){
		if( written >= WIRE_MAX ){
			_wire->endTransmission();
			_wire->beginTransmission(_i2caddr);
			_wire->write((uint8_t) 0x40);
			written = 1;
		}
		_wire->write(0);
	}
	_wire->endTransmission();

	TRANSACTION_END
}