Subversion Repositories Code-Repo

#include "maindefs.h"
#include "oled_ssd1331.h"
#include "spi.h"
#include "string.h"
#include "glcdfont.c"
#include <delays.h>
#include <string.h>
#include <stdio.h>

static SSD1331_DATA ssd1331_data;
static SSD1331_DATA *ssd1331_data_p = &ssd1331_data;

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

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

void SSD1331_Init() {
    ssd1331_data_p->_width = ssd1331_data_p->WIDTH = SSD1331_LCDWIDTH;
    ssd1331_data_p->_height = ssd1331_data_p->HEIGHT = SSD1331_LCDHEIGHT;
    ssd1331_data_p->rotation = 0;
    ssd1331_data_p->cursor_x = ssd1331_data_p->cursor_y = 0;
    ssd1331_data_p->textsize = 1;
    ssd1331_data_p->textcolor = ssd1331_data_p->textbgcolor = 0xFFFF;
    ssd1331_data_p->wrap = 1;
}

void SSD1331_Begin() {
    unsigned char buffer[37];

    // Toggle reset pin
    SPI_RESET_LAT = 0;
    Delay10KTCYx(1);
    SPI_RESET_LAT = 1;

    // Initialization Sequence
    buffer[0] = SSD1331_CMD_DISPLAYOFF; // 0xAE
    buffer[1] = SSD1331_CMD_SETREMAP; // 0xA0
#if defined SSD1331_COLORORDER_RGB
    buffer[2] = 0x72; // RGB Color
#else
    buffer[2] = 0x76; // BGR Color
#endif
    buffer[3] = SSD1331_CMD_STARTLINE; // 0xA1
    buffer[4] = 0x0;
    buffer[5] = SSD1331_CMD_DISPLAYOFFSET; // 0xA2
    buffer[6] = 0x0;
    buffer[7] = SSD1331_CMD_NORMALDISPLAY; // 0xA4
    buffer[8] = SSD1331_CMD_SETMULTIPLEX; // 0xA8
    buffer[9] = 0x3F; // 0x3F 1/64 duty
    buffer[10] = SSD1331_CMD_SETMASTER; // 0xAD
    buffer[11] = 0x8E;
    buffer[12] = SSD1331_CMD_POWERMODE; // 0xB0
    buffer[13] = 0x0B;
    buffer[14] = SSD1331_CMD_PRECHARGE; // 0xB1
    buffer[15] = 0x31;
    buffer[16] = SSD1331_CMD_CLOCKDIV; // 0xB3
    buffer[17] = 0xF0; // 7:4 = Oscillator Frequency, 3:0 = CLK Div Ratio (A[3:0]+1 = 1..16)
    buffer[18] = SSD1331_CMD_PRECHARGEA; // 0x8A
    buffer[19] = 0x64;
    buffer[20] = SSD1331_CMD_PRECHARGEB; // 0x8B
    buffer[21] = 0x78;
    buffer[22] = SSD1331_CMD_PRECHARGEA; // 0x8C
    buffer[23] = 0x64;
    buffer[24] = SSD1331_CMD_PRECHARGELEVEL; // 0xBB
    buffer[25] = 0x3A;
    buffer[26] = SSD1331_CMD_VCOMH; // 0xBE
    buffer[27] = 0x3E;
    buffer[28] = SSD1331_CMD_MASTERCURRENT; // 0x87
    buffer[29] = 0x06;
    buffer[30] = SSD1331_CMD_CONTRASTA; // 0x81
    buffer[31] = 0x91;
    buffer[32] = SSD1331_CMD_CONTRASTB; // 0x82
    buffer[33] = 0x50;
    buffer[34] = SSD1331_CMD_CONTRASTC; // 0x83
    buffer[35] = 0x7D;
    buffer[36] = SSD1331_CMD_DISPLAYON; //--turn on oled panel

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 37);
}

void SSD1331_GoTo(int x, int y) {
    unsigned char buffer[6];
    if ((x >= SSD1331_LCDWIDTH) || (y >= SSD1331_LCDHEIGHT)) return;

    // set x and y coordinate
    buffer[0] = (SSD1331_CMD_SETCOLUMN);
    buffer[1] = (x); // Start x address
    buffer[2] = (SSD1331_LCDWIDTH - 1); // End x address

    buffer[3] = (SSD1331_CMD_SETROW);
    buffer[4] = (y); // Start y address
    buffer[5] = (SSD1331_LCDHEIGHT - 1); // End y address

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 6);
}

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

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

void SSD1331_Clear_Display() {
    unsigned char buffer[5];

    buffer[0] = SSD1331_CMD_CLEARWINDOW;
    buffer[1] = 0;
    buffer[2] = 0;
    buffer[3] = SSD1331_LCDWIDTH-1;
    buffer[4] = SSD1331_LCDHEIGHT-1;

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 5);

    Delay1KTCYx(4);
}

void SSD1331_Draw_Pixel(int x, int y, unsigned int color) {
    unsigned char buffer[2];
    buffer[0] = color >> 8;
    buffer[1] = color;
    if ((x < 0) || (x >= ssd1331_data_p->_width) || (y < 0) || (y >= ssd1331_data_p->_height)) return;

    // check rotation, move pixel around if necessary
    switch (ssd1331_data_p->rotation) {
        case 1:
            SSD1331_Swap(&x, &y);
            x = SSD1331_LCDWIDTH - x - 1;
            break;
        case 2:
            x = SSD1331_LCDWIDTH - x - 1;
            y = SSD1331_LCDHEIGHT - y - 1;
            break;
        case 3:
            SSD1331_Swap(&x, &y);
            y = SSD1331_LCDHEIGHT - y - 1;
            break;
    }

    SSD1331_GoTo(x, y);

    // setup for data
    SPI_DC_SELECT_LAT = 1; // D/C high (data)

    SPI2_Write(buffer, 2);
}

void SSD1331_Draw_Line(int x0, int y0, int x1, int y1, unsigned int color) {
    unsigned char buffer[8];

    // check rotation, move pixel around if necessary
    switch (ssd1331_data_p->rotation) {
        case 1:
            SSD1331_Swap(&x0, &y0);
            SSD1331_Swap(&x1, &y1);
            x0 = SSD1331_LCDWIDTH - x0 - 1;
            x1 = SSD1331_LCDWIDTH - x1 - 1;
            break;
        case 2:
            x0 = SSD1331_LCDWIDTH - x0 - 1;
            y0 = SSD1331_LCDHEIGHT - y0 - 1;
            x1 = SSD1331_LCDWIDTH - x1 - 1;
            y1 = SSD1331_LCDHEIGHT - y1 - 1;
            break;
        case 3:
            SSD1331_Swap(&x0, &y0);
            SSD1331_Swap(&x1, &y1);
            y0 = SSD1331_LCDHEIGHT - y0 - 1;
            y1 = SSD1331_LCDHEIGHT - y1 - 1;
            break;
    }

    // Boundary check
    if ((y0 >= SSD1331_LCDHEIGHT) && (y1 >= SSD1331_LCDHEIGHT))
        return;
    if ((x0 >= SSD1331_LCDWIDTH) && (x1 >= SSD1331_LCDWIDTH))
        return;
    if (x0 >= SSD1331_LCDWIDTH)
        x0 = SSD1331_LCDWIDTH - 1;
    if (y0 >= SSD1331_LCDHEIGHT)
        y0 = SSD1331_LCDHEIGHT - 1;
    if (x1 >= SSD1331_LCDWIDTH)
        x1 = SSD1331_LCDWIDTH - 1;
    if (y1 >= SSD1331_LCDHEIGHT)
        y1 = SSD1331_LCDHEIGHT - 1;
    if (x0 < 0)
        x0 = 0;
    if (y0 < 0)
        y0 = 0;
    if (x1 < 0)
        x1 = 0;
    if (y1 < 0)
        y1 = 0;

    buffer[0] = SSD1331_CMD_DRAWLINE;
    buffer[1] = x0;
    buffer[2] = y0;
    buffer[3] = x1;
    buffer[4] = y1;
    buffer[5] = (color >> 11) << 1;
    buffer[6] = (color >> 5) & 0x3F;
    buffer[7] = (color << 1) & 0x3F;

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 8);
}

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

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

void SSD1331_Draw_Rect(int tx0, int ty0, int tx1, int ty1, unsigned int color) {
    unsigned char buffer[13];
    int x0,y0,x1,y1;
    
    // check rotation, move pixel around if necessary
    switch (ssd1331_data_p->rotation) {
        case 0:
            x0 = tx0;
            y0 = ty0;
            x1 = tx1;
            y1 = ty1;
            break;
        case 1:
            x0 = SSD1331_LCDWIDTH - ty1 - 1;
            y0 = tx0;
            x1 = SSD1331_LCDWIDTH - ty0 - 1;
            y1 = tx1;
            break;
        case 2:
            x0 = SSD1331_LCDWIDTH - tx1 - 1;
            y0 = SSD1331_LCDHEIGHT - ty1 - 1;
            x1 = SSD1331_LCDWIDTH - tx0 - 1;
            y1 = SSD1331_LCDHEIGHT - ty0 - 1;
            break;
        case 3:
            x0 = ty0;
            y0 = SSD1331_LCDHEIGHT - tx1 - 1;
            x1 = ty1;
            y1 = SSD1331_LCDHEIGHT - tx0 - 1;
            break;
    }

    // Boundary check
    if ((y0 >= SSD1331_LCDHEIGHT) && (y1 >= SSD1331_LCDHEIGHT))
        return;
    if ((x0 >= SSD1331_LCDWIDTH) && (x1 >= SSD1331_LCDWIDTH))
        return;
    if (x0 >= SSD1331_LCDWIDTH)
        x0 = SSD1331_LCDWIDTH - 1;
    if (y0 >= SSD1331_LCDHEIGHT)
        y0 = SSD1331_LCDHEIGHT - 1;
    if (x1 >= SSD1331_LCDWIDTH)
        x1 = SSD1331_LCDWIDTH - 1;
    if (y1 >= SSD1331_LCDHEIGHT)
        y1 = SSD1331_LCDHEIGHT - 1;
    if (x0 < 0)
        x0 = 0;
    if (y0 < 0)
        y0 = 0;
    if (x1 < 0)
        x1 = 0;
    if (y1 < 0)
        y1 = 0;

    buffer[0] = SSD1331_CMD_FILL;
    buffer[1] = 0;
    buffer[2] = SSD1331_CMD_DRAWRECT;
    buffer[3] = x0;
    buffer[4] = y0;
    buffer[5] = x1;
    buffer[6] = y1;
    buffer[7] = (color >> 11) << 1;
    buffer[8] = (color >> 5) & 0x3F;
    buffer[9] = (color << 1) & 0x3F;
    buffer[10] = 0;
    buffer[11] = 0;
    buffer[12] = 0;

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 13);
}

void SSD1331_Fill_Rect(int tx0, int ty0, int tx1, int ty1, unsigned int color) {
    unsigned char buffer[13];
    int x0,y0,x1,y1;
    
    // check rotation, move pixel around if necessary
    switch (ssd1331_data_p->rotation) {
        case 0:
            x0 = tx0;
            y0 = ty0;
            x1 = tx1;
            y1 = ty1;
            break;
        case 1:
            x0 = SSD1331_LCDWIDTH - ty1 - 1;
            y0 = tx0;
            x1 = SSD1331_LCDWIDTH - ty0 - 1;
            y1 = tx1;
            break;
        case 2:
            x0 = SSD1331_LCDWIDTH - tx1 - 1;
            y0 = SSD1331_LCDHEIGHT - ty1 - 1;
            x1 = SSD1331_LCDWIDTH - tx0 - 1;
            y1 = SSD1331_LCDHEIGHT - ty0 - 1;
            break;
        case 3:
            x0 = ty0;
            y0 = SSD1331_LCDHEIGHT - tx1 - 1;
            x1 = ty1;
            y1 = SSD1331_LCDHEIGHT - tx0 - 1;
            break;
    }

    // Boundary check
    if ((y0 >= SSD1331_LCDHEIGHT) && (y1 >= SSD1331_LCDHEIGHT))
        return;
    if ((x0 >= SSD1331_LCDWIDTH) && (x1 >= SSD1331_LCDWIDTH))
        return;
    if (x0 >= SSD1331_LCDWIDTH)
        x0 = SSD1331_LCDWIDTH - 1;
    if (y0 >= SSD1331_LCDHEIGHT)
        y0 = SSD1331_LCDHEIGHT - 1;
    if (x1 >= SSD1331_LCDWIDTH)
        x1 = SSD1331_LCDWIDTH - 1;
    if (y1 >= SSD1331_LCDHEIGHT)
        y1 = SSD1331_LCDHEIGHT - 1;
    if (x0 < 0)
        x0 = 0;
    if (y0 < 0)
        y0 = 0;
    if (x1 < 0)
        x1 = 0;
    if (y1 < 0)
        y1 = 0;

    buffer[0] = SSD1331_CMD_FILL;
    buffer[1] = 1;
    buffer[2] = SSD1331_CMD_DRAWRECT;
    buffer[3] = x0;
    buffer[4] = y0;
    buffer[5] = x1;
    buffer[6] = y1;
    buffer[7] = (color >> 11) << 1;
    buffer[8] = (color >> 5) & 0x3F;
    buffer[9] = (color << 1) & 0x3F;
    buffer[10] = (color >> 11) << 1;
    buffer[11] = (color >> 5) & 0x3F;
    buffer[12] = (color << 1) & 0x3F;

    SPI_DC_SELECT_LAT = 0; // D/C low (cmd)
    SPI2_Write(buffer, 13);

    Delay1KTCYx(4);
}

void SSD1331_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;

    SSD1331_Draw_Pixel(x0, y0 + r, color);
    SSD1331_Draw_Pixel(x0, y0 - r, color);
    SSD1331_Draw_Pixel(x0 + r, y0, color);
    SSD1331_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;

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

void SSD1331_Draw_Circle_Helper(int x0, int y0, int r, unsigned 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) {
            SSD1331_Draw_Pixel(x0 + x, y0 + y, color);
            SSD1331_Draw_Pixel(x0 + y, y0 + x, color);
        }
        if (cornername & 0x2) {
            SSD1331_Draw_Pixel(x0 + x, y0 - y, color);
            SSD1331_Draw_Pixel(x0 + y, y0 - x, color);
        }
        if (cornername & 0x8) {
            SSD1331_Draw_Pixel(x0 - y, y0 + x, color);
            SSD1331_Draw_Pixel(x0 - x, y0 + y, color);
        }
        if (cornername & 0x1) {
            SSD1331_Draw_Pixel(x0 - y, y0 - x, color);
            SSD1331_Draw_Pixel(x0 - x, y0 - y, color);
        }
    }
}

void SSD1331_Fill_Circle(int x0, int y0, int r, unsigned int color) {
    SSD1331_Draw_Fast_VLine(x0, y0 - r, 2 * r + 1, color);
    SSD1331_Fill_Circle_Helper(x0, y0, r, 3, 0, color);
}

void SSD1331_Fill_Circle_Helper(int x0, int y0, int r, unsigned 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) {
            SSD1331_Draw_Fast_VLine(x0 + x, y0 - y, 2 * y + 1 + delta, color);
            SSD1331_Draw_Fast_VLine(x0 + y, y0 - x, 2 * x + 1 + delta, color);
        }
        if (cornername & 0x2) {
            SSD1331_Draw_Fast_VLine(x0 - x, y0 - y, 2 * y + 1 + delta, color);
            SSD1331_Draw_Fast_VLine(x0 - y, y0 - x, 2 * x + 1 + delta, color);
        }
    }
}
void SSD1331_Draw_Triangle(int x0, int y0, int x1, int y1, int x2, int y2, unsigned int color) {
    SSD1331_Draw_Line(x0, y0, x1, y1, color);
    SSD1331_Draw_Line(x1, y1, x2, y2, color);
    SSD1331_Draw_Line(x2, y2, x0, y0, color);
}

void SSD1331_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) {
        SSD1331_Swap(&y0, &y1);
        SSD1331_Swap(&x0, &x1);
    }
    if (y1 > y2) {
        SSD1331_Swap(&y2, &y1);
        SSD1331_Swap(&x2, &x1);
    }
    if (y0 > y1) {
        SSD1331_Swap(&y0, &y1);
        SSD1331_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;
        SSD1331_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) SSD1331_Swap(&a, &b);
        SSD1331_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) SSD1331_Swap(&a, &b);
        SSD1331_Draw_Fast_HLine(a, y, b - a + 1, color);
    }
}

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

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

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

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

void SSD1331_Draw_Bitmap(int x, int y, const unsigned 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)) {
                SSD1331_Draw_Pixel(x + i, y + j, color);
            }
        }
    }
}

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

    if ((x >= ssd1331_data_p->_width) || // Clip right
            (y >= ssd1331_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
                    SSD1331_Draw_Pixel(x + i, y + j, color);
                } else { // big size
                    SSD1331_Fill_Rect(x + (i * size), y + (j * size), size, size, color);
                }
            } else if (bg != color) {
                if (size == 1) { // default size
                    SSD1331_Draw_Pixel(x + i, y + j, bg);
                } else { // big size
                    SSD1331_Fill_Rect(x + i*size, y + j*size, size, size, bg);
                }
            }
            line >>= 1;
        }
    }
}

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

void SSD1331_Write_String(const rom char *fmt, ...) {
    unsigned char i, len;
    unsigned char buffer[SSD1331_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 > SSD1331_STRING_BUFFER_SIZE)
        len = SSD1331_STRING_BUFFER_SIZE;

    // Print buffer to string
    for (i = 0; i < len; i++) {
        SSD1331_Write(buffer[i]);
    }
}

void SSD1331_Set_Cursor(int x, int y) {
    ssd1331_data_p->cursor_x = x;
    ssd1331_data_p->cursor_y = y;
}

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

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

void SSD1331_Set_Text_Size(unsigned char s) {
    ssd1331_data_p->textsize = (s > 0) ? s : 1;
}

void SSD1331_Set_Text_Wrap(unsigned char w) {
    ssd1331_data_p->wrap = w;
}

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

unsigned int SSD1331_Color565(unsigned char r, unsigned char g, unsigned char b) {
    unsigned int c;
    c = r >> 3;
    c <<= 6;
    c |= g >> 2;
    c <<= 5;
    c |= b >> 3;

    return c;
}

void SSD1331_Test_DrawLines(unsigned int color) {
    int x, y;
    SSD1331_Clear_Display();
    for (x = 0; x < ssd1331_data_p->_width - 1; x += 6) {
        SSD1331_Draw_Line(0, 0, x, ssd1331_data_p->_height - 1, color);
    }
    for (y = 0; y < ssd1331_data_p->_height - 1; y += 6) {
        SSD1331_Draw_Line(0, 0, ssd1331_data_p->_width - 1, y, color);
    }

    SSD1331_Clear_Display();
    for (x = 0; x < ssd1331_data_p->_width - 1; x += 6) {
        SSD1331_Draw_Line(ssd1331_data_p->_width - 1, 0, x, ssd1331_data_p->_height - 1, color);
    }
    for (y = 0; y < ssd1331_data_p->_height - 1; y += 6) {
        SSD1331_Draw_Line(ssd1331_data_p->_width - 1, 0, 0, y, color);
    }

    SSD1331_Clear_Display();
    for (x = 0; x < ssd1331_data_p->_width - 1; x += 6) {
        SSD1331_Draw_Line(0, ssd1331_data_p->_height - 1, x, 0, color);
    }
    for (y = 0; y < ssd1331_data_p->_height - 1; y += 6) {
        SSD1331_Draw_Line(0, ssd1331_data_p->_height - 1, ssd1331_data_p->_width - 1, y, color);
    }

    SSD1331_Clear_Display();
    for (x = 0; x < ssd1331_data_p->_width - 1; x += 6) {
        SSD1331_Draw_Line(ssd1331_data_p->_width - 1, ssd1331_data_p->_height - 1, x, 0, color);
    }
    for (y = 0; y < ssd1331_data_p->_height - 1; y += 6) {
        SSD1331_Draw_Line(ssd1331_data_p->_width - 1, ssd1331_data_p->_height - 1, 0, y, color);
    }
}

void SSD1331_Test_DrawRect(unsigned int color) {
    int x;
    SSD1331_Clear_Display();
    if (ssd1331_data_p->_height < ssd1331_data_p->_width) {
        for (x = 0; x < ssd1331_data_p->_height - 1; x += 6) {
            SSD1331_Draw_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color);
        }
    } else {
        for (x = 0; x < ssd1331_data_p->_width - 1; x += 6) {
            SSD1331_Draw_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color);
        }
    }
}

void SSD1331_Test_FillRect(unsigned int color1, unsigned int color2) {
    int x;
    SSD1331_Clear_Display();
    if (ssd1331_data_p->_height < ssd1331_data_p->_width) {
        for (x = ssd1331_data_p->_height - 1; x > 6; x -= 6) {
            SSD1331_Fill_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color1);
            SSD1331_Draw_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color2);
        }
    } else {
        for (x = ssd1331_data_p->_width - 1; x > 6; x -= 6) {
            SSD1331_Fill_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color1);
            SSD1331_Draw_Rect((ssd1331_data_p->_width - 1) / 2 - x / 2, (ssd1331_data_p->_height - 1) / 2 - x / 2, x, x, color2);
        }
    }
}

void SSD1331_Test_DrawCircle(unsigned int radius, unsigned int color) {
    int x, y;
    for (x = 0; x < ssd1331_data_p->_width - 1 + radius; x += radius * 2) {
        for (y = 0; y < ssd1331_data_p->_height - 1 + radius; y += radius * 2) {
            SSD1331_Draw_Circle(x, y, radius, color);
        }
    }
}

void SSD1331_Test_FillCircle(unsigned int radius, unsigned int color) {
    unsigned char x, y;
    for (x = radius; x < ssd1331_data_p->_width - 1; x += radius * 2) {
        for (y = radius; y < ssd1331_data_p->_height - 1; y += radius * 2) {
            SSD1331_Fill_Circle(x, y, radius, color);
        }
    }
}

void SSD1331_Test_DrawTria(void) {
    int color = 0xF800;
    int t;
    int w = ssd1331_data_p->_width / 2;
    int x = ssd1331_data_p->_height;
    int y = 0;
    int z = ssd1331_data_p->_width;
    SSD1331_Clear_Display();
    for (t = 0; t <= 15; t += 1) {
        SSD1331_Draw_Triangle(w, y, y, x, z, x, color);
        x -= 4;
        y += 4;
        z -= 4;
        color += 100;
    }
}

void SSD1331_Test_DrawRoundRect(void) {
    int color = 100;
    int i, t, x, y, w, h;
    SSD1331_Clear_Display();
    for (t = 0; t <= 4; t += 1) {
        x = 0;
        y = 0;
        w = ssd1331_data_p->_width;
        h = ssd1331_data_p->_height;
        for (i = 0; i <= 24; i += 1) {
            SSD1331_Draw_Round_Rect(x, y, w, h, 5, color);
            x += 2;
            y += 3;
            w -= 4;
            h -= 6;
            color += 1100;
        }
        color += 100;
    }
}

void SSD1331_Test_MediaButtons(void) {
    // play
    SSD1331_Clear_Display();
    SSD1331_Fill_Round_Rect(25, 10, 78, 60, 8, SSD1331_WHITE);
    SSD1331_Fill_Triangle(42, 20, 42, 60, 90, 40, SSD1331_RED);
    Delay10KTCYx(100);
    // pause
    SSD1331_Fill_Round_Rect(25, 90, 78, 60, 8, SSD1331_WHITE);
    SSD1331_Fill_Round_Rect(39, 98, 20, 45, 5, SSD1331_GREEN);
    SSD1331_Fill_Round_Rect(69, 98, 20, 45, 5, SSD1331_GREEN);
    Delay10KTCYx(100);
    // play color
    SSD1331_Fill_Triangle(42, 20, 42, 60, 90, 40, SSD1331_BLUE);
    Delay10KTCYx(100);
    // pause color
    SSD1331_Fill_Round_Rect(39, 98, 20, 45, 5, SSD1331_RED);
    SSD1331_Fill_Round_Rect(69, 98, 20, 45, 5, SSD1331_RED);
    // play color
    SSD1331_Fill_Triangle(42, 20, 42, 60, 90, 40, SSD1331_GREEN);
}

void SSD1331_Test_Pattern(void) {
    unsigned char buffer[2];
    unsigned int i, j;
    SSD1331_GoTo(0, 0);

    for (i = 0; i < 64; i++) {
        for (j = 0; j < 96; j++) {
            if (i > 55) {
                buffer[0] = (SSD1331_WHITE >> 8);
                buffer[1] = (SSD1331_WHITE);
            } else if (i > 47) {
                buffer[0] = (SSD1331_BLUE >> 8);
                buffer[1] = (SSD1331_BLUE);
            } else if (i > 39) {
                buffer[0] = (SSD1331_GREEN >> 8);
                buffer[1] = (SSD1331_GREEN);
            } else if (i > 31) {
                buffer[0] = (SSD1331_CYAN >> 8);
                buffer[1] = (SSD1331_CYAN);
            } else if (i > 23) {
                buffer[0] = (SSD1331_RED >> 8);
                buffer[1] = (SSD1331_RED);
            } else if (i > 15) {
                buffer[0] = (SSD1331_MAGENTA >> 8);
                buffer[1] = (SSD1331_MAGENTA);
            } else if (i > 7) {
                buffer[0] = (SSD1331_YELLOW >> 8);
                buffer[1] = (SSD1331_YELLOW);
            } else {
                buffer[0] = (SSD1331_BLACK >> 8);
                buffer[1] = (SSD1331_BLACK);
            }
            SPI_DC_SELECT_LAT = 1; // D/C high (data)
            SPI2_Write(buffer, 2);
        }
    }
}