Subversion Repositories Code-Repo

Rev

Rev 157 | Blame | Last modification | View Log | RSS feed 

#include <xc.h>
#include <delays.h>
#include <string.h>
#include <stdio.h>
#include "defines.h"
#include "base_SPI.h"
#include "display_oled_ssd1306.h"
#include "glcdfont.c"
#include "base_UART.h"

static SSD1306_DATA *ssd1306_data_p;

// 512 (128x32) or 1024 (128x64) bytes allocated for LCD buffer
// See linker file for details
static char LCD_BUFFER[SSD1306_LCDHEIGHT * SSD1306_LCDWIDTH / 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,
#if (SSD1306_LCDHEIGHT == 64)
    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
#endif
};

int SSD1306_Abs(int i) {
    if (i < 0)
        return -i;
    else
        return i;
}

void SSD1306_Swap(int *a, int *b) {
    int tmp = *a;
    *a = *b;
    *b = tmp;
}

void SSD1306_Init(SSD1306_DATA *data) {
    ssd1306_data_p = data;
    ssd1306_data_p->_width = ssd1306_data_p->WIDTH = SSD1306_LCDWIDTH;
    ssd1306_data_p->_height = ssd1306_data_p->HEIGHT = SSD1306_LCDHEIGHT;
    ssd1306_data_p->rotation = 0;
    ssd1306_data_p->cursor_x = ssd1306_data_p->cursor_y = 0;
    ssd1306_data_p->textsize = 1;
    ssd1306_data_p->textcolor = SSD1306_WHITE;
    ssd1306_data_p->textbgcolor = SSD1306_BLACK;
    ssd1306_data_p->wrap = 1;
}

void SSD1306_Begin(char vccstate) {
    // Toggle reset pin
    SPI_RESET_LAT = 0;
    Delay10KTCYx(1);
    SPI_RESET_LAT = 1;

#if defined SSD1306_128_32
    // Init sequence for 128x32 OLED module
    SSD1306_Command(SSD1306_DISPLAYOFF); // 0xAE
    SSD1306_Command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
    SSD1306_Command(0x80); // The suggested ratio 0x80
    SSD1306_Command(SSD1306_SETMULTIPLEX); // 0xA8
    SSD1306_Command(0x1F);
    SSD1306_Command(SSD1306_SETDISPLAYOFFSET); // 0xD3
    SSD1306_Command(0x0); // No offset
    SSD1306_Command(SSD1306_SETSTARTLINE | 0x0); // Line #0
    SSD1306_Command(SSD1306_CHARGEPUMP); // 0x8D
    if (vccstate == SSD1306_EXTERNALVCC) {
        SSD1306_Command(0x10);
    } else {
        SSD1306_Command(0x14);
    }
    SSD1306_Command(SSD1306_MEMORYMODE); // 0x20
    SSD1306_Command(0x00); // 0x0 act like ks0108
    SSD1306_Command(SSD1306_SEGREMAP | 0x1);
    SSD1306_Command(SSD1306_COMSCANDEC);
    SSD1306_Command(SSD1306_SETCOMPINS); // 0xDA
    SSD1306_Command(0x02);
    SSD1306_Command(SSD1306_SETCONTRAST); // 0x81
    SSD1306_Command(0x8F);
    SSD1306_Command(SSD1306_SETPRECHARGE); // 0xd9
    if (vccstate == SSD1306_EXTERNALVCC) {
        SSD1306_Command(0x22);
    } else {
        SSD1306_Command(0xF1);
    }
    SSD1306_Command(SSD1306_SETVCOMDETECT); // 0xDB
    SSD1306_Command(0x40);
    SSD1306_Command(SSD1306_DISPLAYALLON_RESUME); // 0xA4
    SSD1306_Command(SSD1306_NORMALDISPLAY); // 0xA6
#endif

#if defined SSD1306_128_64
    // Init sequence for 128x64 OLED module
    SSD1306_Command(SSD1306_DISPLAYOFF); // 0xAE
    SSD1306_Command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
    SSD1306_Command(0x80); // The suggested ratio 0x80
    SSD1306_Command(SSD1306_SETMULTIPLEX); // 0xA8
    SSD1306_Command(0x3F);
    SSD1306_Command(SSD1306_SETDISPLAYOFFSET); // 0xD3
    SSD1306_Command(0x0); // No offset
    SSD1306_Command(SSD1306_SETSTARTLINE | 0x0); // Line #0
    SSD1306_Command(SSD1306_CHARGEPUMP); // 0x8D
    if (vccstate == SSD1306_EXTERNALVCC) {
        SSD1306_Command(0x10);
    } else {
        SSD1306_Command(0x14);
    }
    SSD1306_Command(SSD1306_MEMORYMODE); // 0x20
    SSD1306_Command(0x00); // 0x0 act like ks0108
    SSD1306_Command(SSD1306_SEGREMAP | 0x1);
    SSD1306_Command(SSD1306_COMSCANDEC);
    SSD1306_Command(SSD1306_SETCOMPINS); // 0xDA
    SSD1306_Command(0x12);
    SSD1306_Command(SSD1306_SETCONTRAST); // 0x81
    if (vccstate == SSD1306_EXTERNALVCC) {
        SSD1306_Command(0x9F);
    } else {
        SSD1306_Command(0xCF);
    }
    SSD1306_Command(SSD1306_SETPRECHARGE); // 0xd9
    if (vccstate == SSD1306_EXTERNALVCC) {
        SSD1306_Command(0x22);
    } else {
        SSD1306_Command(0xF1);
    }
    SSD1306_Command(SSD1306_SETVCOMDETECT); // 0xDB
    SSD1306_Command(0x40);
    SSD1306_Command(SSD1306_DISPLAYALLON_RESUME); // 0xA4
    SSD1306_Command(SSD1306_NORMALDISPLAY); // 0xA6
#endif

    SSD1306_Command(SSD1306_DISPLAYON); // Turn on OLED panel
}

void SSD1306_Command(char cmd) {
    char c = cmd;
    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(&c, 1);
}

void SSD1306_Data(char data) {
    char c = data;
    SPI_DC_SELECT_LAT = 1; // D/C high (data)
    SPI2_Write(&c, 1);
}

void SSD1306_Clear_Display() {
    memset(LCD_BUFFER, 0, (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8));
}

void SSD1306_Invert_Display(char c) {
    if (c) {
        SSD1306_Command(SSD1306_INVERTDISPLAY);
    } else {
        SSD1306_Command((SSD1306_NORMALDISPLAY));
    }
}

void SSD1306_Display() {
    SSD1306_Command(SSD1306_SETLOWCOLUMN | 0x0); // low col = 0
    SSD1306_Command(SSD1306_SETHIGHCOLUMN | 0x0); // hi col = 0
    SSD1306_Command(SSD1306_SETSTARTLINE | 0x0); // line #0

    SPI_DC_SELECT_LAT = 1; // D/C high (data)
    SPI2_Write(LCD_BUFFER, SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8);

//    if (SSD1306_LCDHEIGHT == 32) {
//        SPI2_Write_Repeat(0, SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8);
//    }
}

void SSD1306_Draw_Pixel(int x, int y, unsigned int color) {
    if ((x < 0) || (x >= ssd1306_data_p->_width) || (y < 0) || (y >= ssd1306_data_p->_height))
        return;

    // check rotation, move pixel around if necessary
    switch (ssd1306_data_p->rotation) {
        case 1:
            SSD1306_Swap(&x, &y);
            x = SSD1306_LCDWIDTH - x - 1;
            break;
        case 2:
            x = SSD1306_LCDWIDTH - x - 1;
            y = SSD1306_LCDHEIGHT - y - 1;
            break;
        case 3:
            SSD1306_Swap(&x, &y);
            y = SSD1306_LCDHEIGHT - y - 1;
            break;
        default:
            break;
    }

    // Need to do this for some reason since x + (y / 8) * SSD1306_LCDWIDTH returns -128?!
    // TODO: Change this back when they fix the compiler
    int loc = (y / 8) * SSD1306_LCDWIDTH;
    loc += x;
    // x is which column
    if (color == SSD1306_WHITE) {
        LCD_BUFFER[loc] |= 1<<(y % 8);
    } else {
        LCD_BUFFER[loc] &= ~(1<<(y % 8));
    }
}

void SSD1306_Draw_Line(int x0, int y0, int x1, int y1, unsigned int color) {
    int dx, dy, err, ystep;
    int steep = SSD1306_Abs(y1 - y0) > SSD1306_Abs(x1 - x0);
    if (steep) {
        SSD1306_Swap(&x0, &y0);
        SSD1306_Swap(&x1, &y1);
    }

    if (x0 > x1) {
        SSD1306_Swap(&x0, &x1);
        SSD1306_Swap(&y0, &y1);
    }

    dx = x1 - x0;
    dy = SSD1306_Abs(y1 - y0);

    err = dx / 2;

    if (y0 < y1) {
        ystep = 1;
    } else {
        ystep = -1;
    }

    for (; x0 <= x1; x0++) {

        if (steep) {
            SSD1306_Draw_Pixel(y0, x0, color);
        } else {
            SSD1306_Draw_Pixel(x0, y0, color);
        }
        err -= dy;
        if (err < 0) {
            y0 += ystep;
            err += dx;
        }
    }
}

void SSD1306_Draw_Fast_VLine(int x, int y, int h, unsigned int color) {
    SSD1306_Draw_Line(x, y, x, y + h - 1, color);
}

void SSD1306_Draw_Fast_HLine(int x, int y, int w, unsigned int color) {
    SSD1306_Draw_Line(x, y, x + w - 1, y, color);
}

void SSD1306_Draw_Rect(int x, int y, int w, int h, unsigned int color) {
    SSD1306_Draw_Fast_HLine(x, y, w, color);
    SSD1306_Draw_Fast_HLine(x, y + h, w, color);
    SSD1306_Draw_Fast_VLine(x, y, h, color);
    SSD1306_Draw_Fast_VLine(x + w, y, h, color);
}

void SSD1306_Fill_Rect(int x, int y, int w, int h, unsigned int color) {
    int i;
    for (i = x; i < x + w; i++) {
        SSD1306_Draw_Fast_VLine(i, y, h, color);
    }
}

void SSD1306_Draw_Circle(int x0, int y0, int r, unsigned int color) {
    int f = 1 - r;
    int ddF_x = 1;
    int ddF_y = -2 * r;
    int x = 0;
    int y = r;

    SSD1306_Draw_Pixel(x0, y0 + r, color);
    SSD1306_Draw_Pixel(x0, y0 - r, color);
    SSD1306_Draw_Pixel(x0 + r, y0, color);
    SSD1306_Draw_Pixel(x0 - r, y0, color);

    while (x < y) {
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;

        SSD1306_Draw_Pixel(x0 + x, y0 + y, color);
        SSD1306_Draw_Pixel(x0 - x, y0 + y, color);
        SSD1306_Draw_Pixel(x0 + x, y0 - y, color);
        SSD1306_Draw_Pixel(x0 - x, y0 - y, color);
        SSD1306_Draw_Pixel(x0 + y, y0 + x, color);
        SSD1306_Draw_Pixel(x0 - y, y0 + x, color);
        SSD1306_Draw_Pixel(x0 + y, y0 - x, color);
        SSD1306_Draw_Pixel(x0 - y, y0 - x, color);
    }
}

void SSD1306_Draw_Circle_Helper(int x0, int y0, int r, char cornername, unsigned int color) {
    int f = 1 - r;
    int ddF_x = 1;
    int ddF_y = -2 * r;
    int x = 0;
    int y = r;

    while (x < y) {
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;
        if (cornername & 0x4) {
            SSD1306_Draw_Pixel(x0 + x, y0 + y, color);
            SSD1306_Draw_Pixel(x0 + y, y0 + x, color);
        }
        if (cornername & 0x2) {
            SSD1306_Draw_Pixel(x0 + x, y0 - y, color);
            SSD1306_Draw_Pixel(x0 + y, y0 - x, color);
        }
        if (cornername & 0x8) {
            SSD1306_Draw_Pixel(x0 - y, y0 + x, color);
            SSD1306_Draw_Pixel(x0 - x, y0 + y, color);
        }
        if (cornername & 0x1) {
            SSD1306_Draw_Pixel(x0 - y, y0 - x, color);
            SSD1306_Draw_Pixel(x0 - x, y0 - y, color);
        }
    }
}

void SSD1306_Fill_Circle(int x0, int y0, int r, unsigned int color) {
    SSD1306_Draw_Fast_VLine(x0, y0 - r, 2 * r + 1, color);
    SSD1306_Fill_Circle_Helper(x0, y0, r, 3, 0, color);
}

void SSD1306_Fill_Circle_Helper(int x0, int y0, int r, char cornername, int delta, unsigned int color) {
    int f = 1 - r;
    int ddF_x = 1;
    int ddF_y = -2 * r;
    int x = 0;
    int y = r;

    while (x < y) {
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;

        if (cornername & 0x1) {
            SSD1306_Draw_Fast_VLine(x0 + x, y0 - y, 2 * y + 1 + delta, color);
            SSD1306_Draw_Fast_VLine(x0 + y, y0 - x, 2 * x + 1 + delta, color);
        }
        if (cornername & 0x2) {
            SSD1306_Draw_Fast_VLine(x0 - x, y0 - y, 2 * y + 1 + delta, color);
            SSD1306_Draw_Fast_VLine(x0 - y, y0 - x, 2 * x + 1 + delta, color);
        }
    }
}

void SSD1306_Draw_Triangle(int x0, int y0, int x1, int y1, int x2, int y2, unsigned int color) {
    SSD1306_Draw_Line(x0, y0, x1, y1, color);
    SSD1306_Draw_Line(x1, y1, x2, y2, color);
    SSD1306_Draw_Line(x2, y2, x0, y0, color);
}

void SSD1306_Fill_Triangle(int x0, int y0, int x1, int y1, int x2, int y2, unsigned int color) {
    int a, b, y, last;
    int dx01 = x1 - x0;
    int dy01 = y1 - y0;
    int dx02 = x2 - x0;
    int dy02 = y2 - y0;
    int dx12 = x2 - x1;
    int dy12 = y2 - y1;
    int sa = 0;
    int sb = 0;

    // Sort coordinates by Y order (y2 >= y1 >= y0)
    if (y0 > y1) {
        SSD1306_Swap(&y0, &y1);
        SSD1306_Swap(&x0, &x1);
    }
    if (y1 > y2) {
        SSD1306_Swap(&y2, &y1);
        SSD1306_Swap(&x2, &x1);
    }
    if (y0 > y1) {
        SSD1306_Swap(&y0, &y1);
        SSD1306_Swap(&x0, &x1);
    }

    if (y0 == y2) { // Handle awkward all-on-same-line case as its own thing
        a = b = x0;
        if (x1 < a) a = x1;
        else if (x1 > b) b = x1;
        if (x2 < a) a = x2;
        else if (x2 > b) b = x2;
        SSD1306_Draw_Fast_HLine(a, y0, b - a + 1, color);
        return;
    }

    // For upper part of triangle, find scanline crossings for segments
    // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
    // is included here (and second loop will be skipped, avoiding a /0
    // error there), otherwise scanline y1 is skipped here and handled
    // in the second loop...which also avoids a /0 error here if y0=y1
    // (flat-topped triangle).
    if (y1 == y2) last = y1; // Include y1 scanline
    else last = y1 - 1; // Skip it

    for (y = y0; y <= last; y++) {
        a = x0 + sa / dy01;
        b = x0 + sb / dy02;
        sa += dx01;
        sb += dx02;
        /* longhand:
        a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
        b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
         */
        if (a > b) SSD1306_Swap(&a, &b);
        SSD1306_Draw_Fast_HLine(a, y, b - a + 1, color);
    }

    // For lower part of triangle, find scanline crossings for segments
    // 0-2 and 1-2.  This loop is skipped if y1=y2.
    sa = dx12 * (y - y1);
    sb = dx02 * (y - y0);
    for (; y <= y2; y++) {
        a = x1 + sa / dy12;
        b = x0 + sb / dy02;
        sa += dx12;
        sb += dx02;
        /* longhand:
        a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
        b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
         */
        if (a > b) SSD1306_Swap(&a, &b);
        SSD1306_Draw_Fast_HLine(a, y, b - a + 1, color);
    }
}

void SSD1306_Draw_Round_Rect(int x, int y, int w, int h, int r, unsigned int color) {
    // smarter version
    SSD1306_Draw_Fast_HLine(x + r, y, w - 2 * r, color); // Top
    SSD1306_Draw_Fast_HLine(x + r, y + h - 1, w - 2 * r, color); // Bottom
    SSD1306_Draw_Fast_VLine(x, y + r, h - 2 * r, color); // Left
    SSD1306_Draw_Fast_VLine(x + w - 1, y + r, h - 2 * r, color); // Right

    // draw four corners
    SSD1306_Draw_Circle_Helper(x + r, y + r, r, 1, color);
    SSD1306_Draw_Circle_Helper(x + w - r - 1, y + r, r, 2, color);
    SSD1306_Draw_Circle_Helper(x + w - r - 1, y + h - r - 1, r, 4, color);
    SSD1306_Draw_Circle_Helper(x + r, y + h - r - 1, r, 8, color);
}

void SSD1306_Fill_Round_Rect(int x, int y, int w, int h, int r, unsigned int color) {
    // smarter version
    SSD1306_Fill_Rect(x + r, y, w - 2 * r, h, color);

    // draw four corners
    SSD1306_Fill_Circle_Helper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);
    SSD1306_Fill_Circle_Helper(x + r, y + r, r, 2, h - 2 * r - 1, color);
}

void SSD1306_Draw_Bitmap(int x, int y, const char* bitmap, int w, int h, unsigned int color) {
    int i, j;
    for (j = 0; j < h; j++) {
        for (i = 0; i < w; i++) {
            if (bitmap[i + (j / 8) * w] & (j % 8)) {
                SSD1306_Draw_Pixel(x + i, y + j, color);
            }
        }
    }
}

void SSD1306_Draw_Char(int x, int y, char c, unsigned int color, unsigned int bg, char size) {
    int i, j;
    unsigned int line;

    if ((x >= ssd1306_data_p->_width) || // Clip right
            (y >= ssd1306_data_p->_height) || // Clip bottom
            ((x + 5 * size - 1) < 0) || // Clip left
            ((y + 8 * size - 1) < 0)) // Clip top
        return;

    for (i = 0; i < 6; i++) {
        if (i == 5)
            line = 0x0;
        else
            line = font[(c * 5) + i];
        for (j = 0; j < 8; j++) {
            if (line & 0x1) {
                if (size == 1) {// default size
                    SSD1306_Draw_Pixel(x + i, y + j, color);
                } else { // big size
                    SSD1306_Fill_Rect(x + (i * size), y + (j * size), size, size, color);
                }
            } else if (bg != color) {
                if (size == 1) { // default size
                    SSD1306_Draw_Pixel(x + i, y + j, bg);
                } else { // big size
                    SSD1306_Fill_Rect(x + i*size, y + j*size, size, size, bg);
                }
            }
            line >>= 1;
        }
    }
}

void SSD1306_Write(char c) {
    if (c == '\n' || c == '\r') {
        ssd1306_data_p->cursor_y += ssd1306_data_p->textsize * 8;
        ssd1306_data_p->cursor_x = 0;
        //    } else if (c == '\r') {
        //        // skip em
    } else {
        SSD1306_Draw_Char(ssd1306_data_p->cursor_x, ssd1306_data_p->cursor_y, c, ssd1306_data_p->textcolor, ssd1306_data_p->textbgcolor, ssd1306_data_p->textsize);
        ssd1306_data_p->cursor_x += ssd1306_data_p->textsize * 6;
        if (ssd1306_data_p->wrap && (ssd1306_data_p->cursor_x > (ssd1306_data_p->_width - ssd1306_data_p->textsize * 6))) {
            ssd1306_data_p->cursor_y += ssd1306_data_p->textsize * 8;
            ssd1306_data_p->cursor_x = 0;
        }
    }
}

void SSD1306_Write_String(char* msg, char length) {
    for (char i = 0; i < length; i++) {
        SSD1306_Write(msg[i]);
    }
}

//void SSD1306_Write_String(const rom char *fmt, ...) {
//    char i, len;
//    char buffer[SSD1306_STRING_BUFFER_SIZE];
//
//    // Parse and create string
//    va_list args;
//    va_start(args, fmt);
//    vsprintf((char *) buffer, fmt, args);
//    va_end(args);
//    len = strlen((char *) buffer);
//
//    // Make sure string to insert fits in buffer, truncate if necessary
//    if (len > SSD1306_STRING_BUFFER_SIZE)
//        len = SSD1306_STRING_BUFFER_SIZE;
//
//    // Print buffer to string
//    for (i = 0; i < len; i++) {
//        SSD1306_Write(buffer[i]);
//    }
//}

//void SSD1306_Append_String(const rom char *fmt, ...) {
//    char i, len;
//    char buffer[SSD1306_STRING_BUFFER_SIZE];
//
//    // Parse and create string
//    va_list args;
//    va_start(args, fmt);
//    vsprintf((char *) buffer, fmt, args);
//    va_end(args);
//
//    // Make sure string to insert fits in buffer, truncate if necessary
//    len = strlen((char *) buffer);
//
//    if (len == 1) {  // This will only occur on "\n"
//        // Do nothing?
//        return;
//    }
//
//    if (len > SSD1306_STRING_BUFFER_SIZE)
//        len = SSD1306_STRING_BUFFER_SIZE;
//
//    // Omit the newline if string fill entire line
//    if (((len - 1)%(ssd1306_data_p->_width / 6)) == 0) { // 16 or 10
//        len -= 1;
//    }
//
//    // Shift everything right and insert string at beginning
//    for (i = 127; i > len - 1; i--) {
//        ssd1306_data_p->lcd_buffer[i] = ssd1306_data_p->lcd_buffer[i - len];
//    }
//    memcpy((char *)ssd1306_data_p->lcd_buffer, (const char *) buffer, len);
//
//    // Print full buffer to screen
//    SSD1306_Clear_Display();
//    SSD1306_Display();
//
//    SSD1306_Set_Cursor(0,0);
//    for (i = 0; i < SSD1306_LCD_BUFFER_SIZE-1; i++) {
//        SSD1306_Write(ssd1306_data_p->lcd_buffer[i]);
//    }
//}

void SSD1306_Set_Cursor(int x, int y) {
    ssd1306_data_p->cursor_x = x;
    ssd1306_data_p->cursor_y = y;
}

void SSD1306_Set_Text_Color(unsigned int c) {
    // for 'transparent' background, we'll set the bg
    // to the same as fg instead of using a flag
    ssd1306_data_p->textcolor = c;
    ssd1306_data_p->textbgcolor = c;
}

void SSD1306_Set_Text_Color_BG(unsigned int c, unsigned int bg) {
    ssd1306_data_p->textcolor = c;
    ssd1306_data_p->textbgcolor = bg;
}

void SSD1306_Set_Text_Size(char s) {
    ssd1306_data_p->textsize = (s > 0) ? s : 1;
}

void SSD1306_Set_Text_Wrap(char w) {
    ssd1306_data_p->wrap = w;
}

void SSD1306_Set_Rotation(char x) {
    x %= 4; // cant be higher than 3
    ssd1306_data_p->rotation = x;
    switch (x) {
        case 0:
        case 2:
            ssd1306_data_p->_width = ssd1306_data_p->WIDTH;
            ssd1306_data_p->_height = ssd1306_data_p->HEIGHT;
            break;
        case 1:
        case 3:
            ssd1306_data_p->_width = ssd1306_data_p->HEIGHT;
            ssd1306_data_p->_height = ssd1306_data_p->WIDTH;
            break;
    }
}



void SSD1306_Test_DrawChar() {
    char i;
    SSD1306_Set_Text_Size(1);
    SSD1306_Set_Text_Color(SSD1306_WHITE);
    SSD1306_Set_Cursor(0, 0);

    for (i = 0; i < 168; i++) {
        if (i == '\n') continue;
        SSD1306_Write(i);
//        if ((i > 0) && (i % 21 == 0))
//            SSD1306_write('\n');
    }
    SSD1306_Display();
}

void SSD1306_Test_DrawCircle() {
    int i;
    for (i = 0; i < ssd1306_data_p->_height; i += 2) {
        SSD1306_Draw_Circle(ssd1306_data_p->_width / 2, ssd1306_data_p->_height / 2, i, SSD1306_WHITE);
        SSD1306_Display();
    }
}

void SSD1306_Test_DrawRect(void) {
    int i;
    for (i = 0; i < ssd1306_data_p->_height / 2; i += 2) {
        SSD1306_Draw_Rect(i, i, ssd1306_data_p->_width - 2 * i, ssd1306_data_p->_height - 2 * i, SSD1306_WHITE);
        SSD1306_Display();
    }
}

void SSD1306_Test_FillRect(void) {
    char color = 1;
    int i;
    for (i = 0; i < ssd1306_data_p->_height / 2; i += 3) {
        // alternate colors
        SSD1306_Fill_Rect(i, i, ssd1306_data_p->_width - i * 2, ssd1306_data_p->_height - i * 2, color % 2);
        SSD1306_Display();
        color++;
    }
}

void SSD1306_Test_DrawTriangle(void) {
    int i;
    int min = ssd1306_data_p->_width < ssd1306_data_p->_height ? ssd1306_data_p->_width : ssd1306_data_p->_height;
    for (i = 0; i < min / 2; i += 5) {
        SSD1306_Draw_Triangle(ssd1306_data_p->_width / 2, ssd1306_data_p->_height / 2 - i,
                ssd1306_data_p->_width / 2 - i, ssd1306_data_p->_height / 2 + i,
                ssd1306_data_p->_width / 2 + i, ssd1306_data_p->_height / 2 + i, SSD1306_WHITE);
        SSD1306_Display();
    }
}

void SSD1306_Test_FillTriangle(void) {
    char color = SSD1306_WHITE;
    int i;
    int min = ssd1306_data_p->_width < ssd1306_data_p->_height ? ssd1306_data_p->_width : ssd1306_data_p->_height;
    for (i = min / 2; i > 0; i -= 5) {
        SSD1306_Fill_Triangle(ssd1306_data_p->_width / 2, ssd1306_data_p->_height / 2 - i,
                ssd1306_data_p->_width / 2 - i, ssd1306_data_p->_height / 2 + i,
                ssd1306_data_p->_width / 2 + i, ssd1306_data_p->_height / 2 + i, SSD1306_WHITE);
        if (color == SSD1306_WHITE) color = SSD1306_BLACK;
        else color = SSD1306_WHITE;
        SSD1306_Display();
    }
}

void SSD1306_Test_DrawRoundRect(void) {
    int i;
    for (i = 0; i < ssd1306_data_p->_height / 2 - 2; i += 2) {
        SSD1306_Draw_Round_Rect(i, i, ssd1306_data_p->_width - 2 * i, ssd1306_data_p->_height - 2 * i, ssd1306_data_p->_height / 4, SSD1306_WHITE);
        SSD1306_Display();
    }
}

void SSD1306_Test_FillRoundRect(void) {
    char color = SSD1306_WHITE;
    int i;
    for (i = 0; i < ssd1306_data_p->_height / 2 - 2; i += 2) {
        SSD1306_Fill_Round_Rect(i, i, ssd1306_data_p->_width - 2 * i, ssd1306_data_p->_height - 2 * i, ssd1306_data_p->_height / 4, color);
        if (color == SSD1306_WHITE) color = SSD1306_BLACK;
        else color = SSD1306_WHITE;
        SSD1306_Display();
    }
}

void SSD1306_Test_DrawLine(void) {
    int i;
    for (i = 0; i < ssd1306_data_p->_width; i += 4) {
        SSD1306_Draw_Line(0, 0, i, ssd1306_data_p->_height - 1, SSD1306_WHITE);
        SSD1306_Display();
    }
    for (i = 0; i < ssd1306_data_p->_height; i += 4) {
        SSD1306_Draw_Line(0, 0, ssd1306_data_p->_width - 1, i, SSD1306_WHITE);
        SSD1306_Display();
    }
    Delay10KTCYx(255);

    SSD1306_Clear_Display();
    for (i = 0; i < ssd1306_data_p->_width; i += 4) {
        SSD1306_Draw_Line(0, ssd1306_data_p->_height - 1, i, 0, SSD1306_WHITE);
        SSD1306_Display();
    }
    for (i = ssd1306_data_p->_height - 1; i >= 0; i -= 4) {
        SSD1306_Draw_Line(0, ssd1306_data_p->_height - 1, ssd1306_data_p->_width - 1, i, SSD1306_WHITE);
        SSD1306_Display();
    }
    Delay10KTCYx(255);

    SSD1306_Clear_Display();
    for (i = ssd1306_data_p->_width - 1; i >= 0; i -= 4) {
        SSD1306_Draw_Line(ssd1306_data_p->_width - 1, ssd1306_data_p->_height - 1, i, 0, SSD1306_WHITE);
        SSD1306_Display();
    }
    for (i = ssd1306_data_p->_height - 1; i >= 0; i -= 4) {
        SSD1306_Draw_Line(ssd1306_data_p->_width - 1, ssd1306_data_p->_height - 1, 0, i, SSD1306_WHITE);
        SSD1306_Display();
    }
    Delay10KTCYx(255);

    SSD1306_Clear_Display();
    for (i = 0; i < ssd1306_data_p->_height; i += 4) {
        SSD1306_Draw_Line(ssd1306_data_p->_width - 1, 0, 0, i, SSD1306_WHITE);
        SSD1306_Display();
    }
    for (i = 0; i < ssd1306_data_p->_width; i += 4) {
        SSD1306_Draw_Line(ssd1306_data_p->_width - 1, 0, i, ssd1306_data_p->_height - 1, SSD1306_WHITE);
        SSD1306_Display();
    }
    Delay10KTCYx(255);
}