/*********************************************************************
SSD1306 I2C Library for Raspberry Pi.
Based on Adafruit SSD1306 Arduino library. Some functions came from Adafruit GFX lib

Modified by Ilia Penev
Tested on Raspberry Pi 2 with 0.96 Yellow/Blue OLED
*********************************************************************/

/*********************************************************************
This is a library for our Monochrome OLEDs based on SSD1306 drivers

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/category/63_98

These displays use SPI to communicate, 4 or 5 pins are required to
interface

Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!

Written by Limor Fried/Ladyada  for Adafruit Industries.
BSD license, check license.txt for more information
All text above, and the splash screen below must be included in any redistribution
*********************************************************************/

#include <stdint.h>
#include <string.h>

#include "ssd1306_i2c.h"
#include "oled_fonts.h"
#include "lcd_oled.h"

#define true 1
#define false 0

#define rotation 0

int8_t cursor_y = 0;
int8_t cursor_x = 0;

// the memory buffer for the LCD. Displays Adafruit logo
uint8_t buffer[SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8] = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x80,
	0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x80, 0x80, 0xC0, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xF8, 0xE0,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x80, 0x80, 0x80,
	0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80,
	0x80, 0x80, 0x00, 0xFF,
	0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00,
	0x80, 0x80, 0x00, 0x00,
	0x80, 0xFF, 0xFF, 0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x80, 0x80, 0x80,
	0x80, 0x00, 0x80, 0x80,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x00, 0x00, 0x8C, 0x8E, 0x84,
	0x00, 0x00, 0x80, 0xF8,
	0xF8, 0xF8, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
	0xE0, 0xE0, 0xC0, 0x80,
	0x00, 0xE0, 0xFC, 0xFE, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE,
	0xFF, 0xC7, 0x01, 0x01,
	0x01, 0x01, 0x83, 0xFF, 0xFF, 0x00, 0x00, 0x7C, 0xFE, 0xC7, 0x01, 0x01,
	0x01, 0x01, 0x83, 0xFF,
	0xFF, 0xFF, 0x00, 0x38, 0xFE, 0xC7, 0x83, 0x01, 0x01, 0x01, 0x83, 0xC7,
	0xFF, 0xFF, 0x00, 0x00,
	0x01, 0xFF, 0xFF, 0x01, 0x01, 0x00, 0xFF, 0xFF, 0x07, 0x01, 0x01, 0x01,
	0x00, 0x00, 0x7F, 0xFF,
	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0xFF, 0xFF, 0xFF,
	0x00, 0x00, 0x01, 0xFF,
	0xFF, 0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x03, 0x0F, 0x3F, 0x7F, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xE7, 0xC7, 0xC7, 0x8F,
	0x8F, 0x9F, 0xBF, 0xFF, 0xFF, 0xC3, 0xC0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFC, 0xFC, 0xFC,
	0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xF8, 0xF8, 0xF0, 0xF0, 0xE0, 0xC0, 0x00,
	0x01, 0x03, 0x03, 0x03,
	0x03, 0x03, 0x01, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03,
	0x03, 0x03, 0x01, 0x01,
	0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x01, 0x01,
	0x03, 0x03, 0x00, 0x00,
	0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01,
	0x03, 0x03, 0x03, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x01,
	0x00, 0x00, 0x00, 0x03,
	0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF9, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0x3F, 0x1F, 0x0F,
	0x87, 0xC7, 0xF7, 0xFF, 0xFF, 0x1F, 0x1F, 0x3D, 0xFC, 0xF8, 0xF8, 0xF8,
	0xF8, 0x7C, 0x7D, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0x0F, 0x07, 0x00,
	0x30, 0x30, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0xE0, 0xC0, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0xC0, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0x7F, 0x7F, 0x3F, 0x1F,
	0x0F, 0x07, 0x1F, 0x7F, 0xFF, 0xFF, 0xF8, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFE, 0xF8, 0xE0,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0xFE, 0xFE, 0x00, 0x00,
	0x00, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x0E, 0xFC, 0xF8, 0x00, 0x00,
	0xF0, 0xF8, 0x1C, 0x0E,
	0x06, 0x06, 0x06, 0x0C, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0xFE, 0xFE, 0x00,
	0x00, 0x00, 0x00, 0xFC,
	0xFE, 0xFC, 0x00, 0x18, 0x3C, 0x7E, 0x66, 0xE6, 0xCE, 0x84, 0x00, 0x00,
	0x06, 0xFF, 0xFF, 0x06,
	0x06, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x06, 0x00, 0x00, 0xFE, 0xFE,
	0x00, 0x00, 0xC0, 0xF8,
	0xFC, 0x4E, 0x46, 0x46, 0x46, 0x4E, 0x7C, 0x78, 0x40, 0x18, 0x3C, 0x76,
	0xE6, 0xCE, 0xCC, 0x80,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x01, 0x07, 0x0F, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F,
	0x3F, 0x1F, 0x0F, 0x03,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x0F, 0x0F, 0x00, 0x00,
	0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00,
	0x03, 0x07, 0x0E, 0x0C,
	0x18, 0x18, 0x0C, 0x06, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x01, 0x0F, 0x0E,
	0x0C, 0x18, 0x0C, 0x0F,
	0x07, 0x01, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07, 0x00, 0x00,
	0x00, 0x0F, 0x0F, 0x00,
	0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F,
	0x00, 0x00, 0x00, 0x07,
	0x07, 0x0C, 0x0C, 0x18, 0x1C, 0x0C, 0x06, 0x06, 0x00, 0x04, 0x0E, 0x0C,
	0x18, 0x0C, 0x0F, 0x07,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00
};

int8_t _vccstate;
int16_t i2cd;

#define ssd1306_swap(a, b) { int16_t t = a; a = b; b = t; }


//Section : 10.1.12 Set Display ON/OFF (AEh/AFh) | Page 37
void ssd1306_i2c_set_display_off(i2c_t *i2c_dev)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_DISPLAYOFF);
}

//Section : 10.1.12 Set Display ON/OFF (AEh/AFh) | Page 37
void ssd1306_i2c_set_display_on(i2c_t *i2c_dev)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_DISPLAYON);
}

// TODO: Find a ferq Calculator
// Section : 10.1.16 Set Display Clock Divide Ratio/ Oscillator Frequency (D5h) | Page : 40
void ssd1306_i2c_set_display_clkDiv_oscFreq(i2c_t *i2c_dev, uint8_t clockDivider, uint8_t oscillatorFreq)
{
	
	uint8_t value = 0;

	if(oscillatorFreq <= 15)
	{
		if(clockDivider <= 16)
		{
			value = (oscillatorFreq << 4) | clockDivider;
		}
	}

	ssd1306_i2c_command(i2c_dev,SSD1306_SETDISPLAYCLOCKDIV);	// 0xD5
	ssd1306_i2c_command(i2c_dev,value);	// the suggested ratio 0x80
}

// Section : 10.1.11 Set Multiplex Ratio (A8h) | Page : 37 
void ssd1306_i2c_set_multiplex_ratio(i2c_t *i2c_dev, uint8_t ratio)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETMULTIPLEX);	// 0xA8
	ssd1306_i2c_command(i2c_dev,ratio);
}

// Section : 10.1.15 Set Display Offset (D3h) | Page : 37 
void ssd1306_i2c_set_display_offset(i2c_t *i2c_dev, uint8_t offset)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETDISPLAYOFFSET);	// 0xD3
	ssd1306_i2c_command(i2c_dev,offset);	
}
// Section : 10.1.13 Set Page Start Address for Page Addressing Mode (B0h~B7h) | Page : 37
void ssd1306_i2c_set_display_start_line(i2c_t *i2c_dev, uint8_t start)
{
	if(start <= 7)
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_SETSTARTLINE | start);
	}
}

// TODO : Some things are not clear What does 0x10 do ? 
// Section : 2.1 Command Table for Charge Bump Setting | Page : 60
void ssd1306_i2c_set_chage_pump(i2c_t *i2c_dev, uint8_t voltageSource)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_CHARGEPUMP);	// 0x8D
	if (voltageSource == SSD1306_EXTERNALVCC) {
		ssd1306_i2c_command(i2c_dev,0x10);
	} else {
		ssd1306_i2c_command(i2c_dev,SSD1306_ENABLE_CHAGE_PUMP);
	}
}

// Section :10.1.3 Set Memory Addressing Mode (20h) | Page : 34
void ssd1306_i2c_set_memory_addressing_mode(i2c_t *i2c_dev, uint8_t mode)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_MEMORYMODE);	
	ssd1306_i2c_command(i2c_dev,mode);
}

// TODO : Not very clear what this functionality does :S
// Section : 10.1.8 Set Segment Re-map (A0h/A1h)
void ssd1306_i2c_set_segment_remap(i2c_t *i2c_dev, uint8_t enable)
{
	if(enable)
	{	
		ssd1306_i2c_command(i2c_dev,SSD1306_SEGREMAP | 0x1);
	}
	else
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_SEGREMAP | 0x0);
	}
}
// Section : 10.1.14 Set COM Output Scan Direction (C0h/C8h) | Page 37 
void ssd1306_i2c_set_com_scan_direction(i2c_t *i2c_dev, uint8_t incremental)
{
	if(incremental)
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_COMSCANINC);
	}
	else
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_COMSCANDEC);
	}
}

// Section : 10.1.18 Set COM Pins Hardware Configuration (DAh) | Page : 40 
void ssd1306_i2c_set_com_pins(i2c_t *i2c_dev, uint8_t alignment)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETCOMPINS);
	ssd1306_i2c_command(i2c_dev,alignment);
}

void ssd1306_i2c_set_contrast(i2c_t *i2c_dev, uint8_t contrast)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETCONTRAST);	// 0x81
	ssd1306_i2c_command(i2c_dev,contrast);	
}

void ssd1306_i2c_set_prechage_period(i2c_t *i2c_dev, uint8_t period)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETPRECHARGE);	// 0xd9
	ssd1306_i2c_command(i2c_dev,period);
}

void ssd1306_i2c_set_com_deselect_level(i2c_t *i2c_dev, uint8_t voltageLevel)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SETVCOMDETECT);	// 0xDB
	ssd1306_i2c_command(i2c_dev,voltageLevel);
}

void ssd1306_i2c_set_display_entire_on(i2c_t *i2c_dev, uint8_t resumeOrForce)
{
	if(resumeOrForce = SSD1306_DISPLAYALLON_RESUME)
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_DISPLAYALLON_RESUME);	// 0xA4
	}
	else 
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_DISPLAYALLON);	// 0xA4
	}

}

void ssd1306_i2c_set_display_invert_pixel(i2c_t *i2c_dev, uint8_t inverted)
{
	if(inverted)
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_INVERTDISPLAY);	// 0xA6
	}
	else 
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_NORMALDISPLAY);	// 0xA6
	}
}

void ssd1306_i2c_set_scroll(i2c_t *i2c_dev, uint8_t onOff)
{
	if(onOff)
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_ACTIVATE_SCROLL);
	}
	else
	{
		ssd1306_i2c_command(i2c_dev,SSD1306_DEACTIVATE_SCROLL);
	}
}


void ssd1306_invertDisplay(i2c_t *i2c_dev, uint16_t i)
{
	if (i) {
		ssd1306_i2c_command(i2c_dev,SSD1306_INVERTDISPLAY);
	} else {
		ssd1306_i2c_command(i2c_dev,SSD1306_NORMALDISPLAY);
	}
}

void ssd1306_i2c_command(i2c_t *i2c_dev,uint16_t c)
{
	// I2C
	uint16_t control = 0x00;	// Co = 0, D/C = 0
	uint8_t i2cDataLenght = 1;	// Co = 0, D/C = 0
	uint16_t address = SSD1306_I2C_ADDRESS;   	// Co = 0, D/C = 0
	i2c_write(i2c_dev, &address, &control, &c, &i2cDataLenght);
}


// startscrollright
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// ssd1306_scrollright(0x00, 0x0F)
void ssd1306_startscrollright(i2c_t *i2c_dev, uint16_t start, uint16_t stop)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_RIGHT_HORIZONTAL_SCROLL);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,start);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,stop);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,0XFF);
	ssd1306_i2c_command(i2c_dev,SSD1306_ACTIVATE_SCROLL);
}

// startscrollleft
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// ssd1306_scrollright(0x00, 0x0F)
void ssd1306_startscrollleft(i2c_t *i2c_dev,uint16_t start, uint16_t stop)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_LEFT_HORIZONTAL_SCROLL);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,start);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,stop);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,0XFF);
	ssd1306_i2c_command(i2c_dev,SSD1306_ACTIVATE_SCROLL);
}

// startscrolldiagright
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// ssd1306_scrollright(0x00, 0x0F)
void ssd1306_startscrolldiagright(i2c_t *i2c_dev, uint16_t start, uint16_t stop)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SET_VERTICAL_SCROLL_AREA);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,SSD1306_LCDHEIGHT);
	ssd1306_i2c_command(i2c_dev,SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,start);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,stop);
	ssd1306_i2c_command(i2c_dev,0X01);
	ssd1306_i2c_command(i2c_dev,SSD1306_ACTIVATE_SCROLL);
}

// startscrolldiagleft
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// ssd1306_scrollright(0x00, 0x0F)
void ssd1306_startscrolldiagleft(i2c_t *i2c_dev, uint16_t start, uint16_t stop)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_SET_VERTICAL_SCROLL_AREA);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,SSD1306_LCDHEIGHT);
	ssd1306_i2c_command(i2c_dev,SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,start);
	ssd1306_i2c_command(i2c_dev,0X00);
	ssd1306_i2c_command(i2c_dev,stop);
	ssd1306_i2c_command(i2c_dev,0X01);
	ssd1306_i2c_command(i2c_dev,SSD1306_ACTIVATE_SCROLL);
}

void ssd1306_stopscroll(i2c_t *i2c_dev)
{
	ssd1306_i2c_command(i2c_dev,SSD1306_DEACTIVATE_SCROLL);
}


void ssd1306_drawFastHLineInternal(int16_t x, int16_t y, int16_t w, uint16_t color)
{
	// Do bounds/limit checks
	if (y < 0 || y >= HEIGHT) {
		return;
	}
	// make sure we don't try to draw below 0
	if (x < 0) {
		w += x;
		x = 0;
	}
	// make sure we don't go off the edge of the display
	if ((x + w) > WIDTH) {
		w = (WIDTH - x);
	}
	// if our width is now negative, punt
	if (w <= 0) {
		return;
	}
	// set up the pointer for movement through the buffer
	uint16_t *pBuf = buffer;
	// adjust the buffer pointer for the current row
	pBuf += ((y / 8) * SSD1306_LCDWIDTH);
	// and offset x columns in
	pBuf += x;

	uint16_t mask = 1 << (y & 7);

	switch (color) {
	case WHITE:
		while (w--) {
			*pBuf++ |= mask;
		};
		break;
	case BLACK:
		mask = ~mask;
		while (w--) {
			*pBuf++ &= mask;
		};
		break;
	case INVERSE:
		while (w--) {
			*pBuf++ ^= mask;
		};
		break;
	}
}

void ssd1306_drawFastVLineInternal(int16_t x, int16_t __y, int16_t __h, uint16_t color)
{

	// do nothing if we're off the left or right side of the screen
	if (x < 0 || x >= WIDTH) {
		return;
	}
	// make sure we don't try to draw below 0
	if (__y < 0) {
		// __y is negative, this will subtract enough from __h to account
		// for __y being 0
		__h += __y;
		__y = 0;

	}
	// make sure we don't go past the height of the display
	if ((__y + __h) > HEIGHT) {
		__h = (HEIGHT - __y);
	}
	// if our height is now negative, punt
	if (__h <= 0) {
		return;
	}
	// this display doesn't need ints for coordinates, use local byte
	// registers for faster juggling
	uint16_t y = __y;
	uint16_t h = __h;

	// set up the pointer for fast movement through the buffer
	uint16_t *pBuf = buffer;
	// adjust the buffer pointer for the current row
	pBuf += ((y / 8) * SSD1306_LCDWIDTH);
	// and offset x columns in
	pBuf += x;

	// do the first partial byte, if necessary - this requires some
	// masking
	uint16_t mod = (y & 7);
	if (mod) {
		// mask off the high n bits we want to set
		mod = 8 - mod;

		// note - lookup table results in a nearly 10% performance
		// improvement in fill* functions
		// register uint16_t mask = ~(0xFF >> (mod));
		static uint16_t premask[8] =
		    { 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xFE };
		uint16_t mask = premask[mod];

		// adjust the mask if we're not going to reach the end of this
		// byte
		if (h < mod) {
			mask &= (0XFF >> (mod - h));
		}

		switch (color) {
		case WHITE:
			*pBuf |= mask;
			break;
		case BLACK:
			*pBuf &= ~mask;
			break;
		case INVERSE:
			*pBuf ^= mask;
			break;
		}

		// fast exit if we're done here!
		if (h < mod) {
			return;
		}

		h -= mod;

		pBuf += SSD1306_LCDWIDTH;
	}
	// write solid bytes while we can - effectively doing 8 rows at a time
	if (h >= 8) {
		if (color == INVERSE) {	// separate copy of the code so we don't
			// impact performance of the black/white
			// write version with an extra comparison
			// per loop
			do {
				*pBuf = ~(*pBuf);

				// adjust the buffer forward 8 rows worth of data
				pBuf += SSD1306_LCDWIDTH;

				// adjust h & y (there's got to be a faster way for me to
				// do this, but this should still help a fair bit for now)
				h -= 8;
			}
			while (h >= 8);
		} else {
			// store a local value to work with
			register uint16_t val = (color == WHITE) ? 255 : 0;

			do {
				// write our value in
				*pBuf = val;

				// adjust the buffer forward 8 rows worth of data
				pBuf += SSD1306_LCDWIDTH;

				// adjust h & y (there's got to be a faster way for me to
				// do this, but this should still help a fair bit for now)
				h -= 8;
			}
			while (h >= 8);
		}
	}
	// now do the final partial byte, if necessary
	if (h) {
		mod = h & 7;
		// this time we want to mask the low bits of the byte, vs the high 
		// bits we did above
		// register uint16_t mask = (1 << mod) - 1;
		// note - lookup table results in a nearly 10% performance
		// improvement in fill* functions
		static uint16_t postmask[8] =
		    { 0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F };
		uint16_t mask = postmask[mod];
		switch (color) {
		case WHITE:
			*pBuf |= mask;
			break;
		case BLACK:
			*pBuf &= ~mask;
			break;
		case INVERSE:
			*pBuf ^= mask;
			break;
		}
	}
}

void ssd1306_drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color)
{
	uint16_t bSwap = false;
	switch (rotation) {
	case 0:
		// 0 degree rotation, do nothing
		break;
	case 1:
		// 90 degree rotation, swap x & y for rotation, then invert x
		bSwap = true;
		ssd1306_swap(x, y);
		x = WIDTH - x - 1;
		break;
	case 2:
		// 180 degree rotation, invert x and y - then shift y around for
		// height.
		x = WIDTH - x - 1;
		y = HEIGHT - y - 1;
		x -= (w - 1);
		break;
	case 3:
		// 270 degree rotation, swap x & y for rotation, then invert y and 
		// adjust y for w (not to become h)
		bSwap = true;
		ssd1306_swap(x, y);
		y = HEIGHT - y - 1;
		y -= (w - 1);
		break;
	}

	if (bSwap) {
		ssd1306_drawFastVLineInternal(x, y, w, color);
	} else {
		ssd1306_drawFastHLineInternal(x, y, w, color);
	}
}

void ssd1306_drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color)
{
	uint16_t bSwap = false;
	switch (rotation) {
	case 0:
		break;
	case 1:
		// 90 degree rotation, swap x & y for rotation, then invert x and
		// adjust x for h (now to become w)
		bSwap = true;
		ssd1306_swap(x, y);
		x = WIDTH - x - 1;
		x -= (h - 1);
		break;
	case 2:
		// 180 degree rotation, invert x and y - then shift y around for
		// height.
		x = WIDTH - x - 1;
		y = HEIGHT - y - 1;
		y -= (h - 1);
		break;
	case 3:
		// 270 degree rotation, swap x & y for rotation, then invert y
		bSwap = true;
		ssd1306_swap(x, y);
		y = HEIGHT - y - 1;
		break;
	}

	if (bSwap) {
		ssd1306_drawFastHLineInternal(x, y, h, color);
	} else {
		ssd1306_drawFastVLineInternal(x, y, h, color);
	}
}

void ssd1306_fillRect(int16_t x, int16_t y, int16_t w, int16_t h, int16_t fillcolor)
{

	// Bounds check
	if ((x >= WIDTH) || (y >= HEIGHT))
		return;

	// Y bounds check
	if (y + h > HEIGHT) {
		h = HEIGHT - y - 1;
	}
	// X bounds check
	if (x + w > WIDTH) {
		w = WIDTH - x - 1;
	}

	switch (rotation) {
	case 1:
		swap_values(x, y);
		x = WIDTH - x - 1;
		break;
	case 2:
		x = WIDTH - x - 1;
		y = HEIGHT - y - 1;
		break;
	case 3:
		swap_values(x, y);
		y = HEIGHT - y - 1;
		break;
	}
	int16_t i;
	for (i = 0; i < h; i++)
		ssd1306_drawFastHLine(x, y + i, w, fillcolor);
}

int16_t textsize = 1;
int16_t wrap = 1;

void ssd1306_setTextSize(int16_t s)
{
	textsize = (s > 0) ? s : 1;
}

void ssd1306_write(int16_t c)
{
	if (c == '\n') {
		cursor_y += textsize * 8;
		cursor_x = 0;
	} else if (c == '\r') {
		// skip em
	} else {
		ssd1306_drawChar(cursor_x, cursor_y, c, WHITE, textsize);
		cursor_x += textsize * 6;
		if (wrap && (cursor_x > (WIDTH - textsize * 6))) {
			cursor_y += textsize * 8;
			cursor_x = 0;
		}
	}
}

void ssd1306_drawString(int8_t *str)
{
	int16_t i, end;
	end = 5;
	for (i = 0; i < end; i++)
		ssd1306_write(str[i]);
}

// Draw a character
void ssd1306_drawChar(int16_t x, int16_t y, uint8_t c, int16_t color, int16_t size)
{
	//Adapted and optimized for  oled_lcd.c
}

// the most basic function, set a single pixel
void ssd1306_drawPixel(int16_t x, int16_t y, uint16_t color)
{
	//Adapted and optimized for  oled_lcd.c
}

// clear everything
void ssd1306_clearDisplay()
{
	//Adapted and optimized for  oled_lcd.c
}

// Init SSD1306
void ssd1306_begin(i2c_t *i2c_dev, uint16_t i2caddr)
{
	//Adapted and optimized for  oled_lcd.c
}

void ssd1306_display(i2c_t *i2c_dev)
{
	//Adapted and optimized for  oled_lcd.c
}