File content as of revision 1:0a1a89c55c7c:

/* mbed RGB Matrix Library
 * Written for Nucleo-F446RE, v1.0, 03-10-2015
 *
 * Copyright (c) 2013-2015 JackB, cstyles
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "RGB_Matrix.h"

RGB_Matrix::RGB_Matrix(PinName Pin_R1, PinName Pin_R2,
                       PinName Pin_G1, PinName Pin_G2,
                       PinName Pin_B1, PinName Pin_B2,
                       PinName Pin_CLK, PinName Pin_LAT, PinName Pin_OE,
                       PinName Pin_A, PinName Pin_B, PinName Pin_C, PinName Pin_D) :
    _Pin_R1(Pin_R1), _Pin_R2(Pin_R2),
    _Pin_G1(Pin_G1), _Pin_G2(Pin_G2),
    _Pin_B1(Pin_B1), _Pin_B2(Pin_B2),
    _Pin_CLK(Pin_CLK), _Pin_LAT(Pin_LAT), _Pin_OE(Pin_OE),
    _Pin_A(Pin_A), _Pin_B(Pin_B), _Pin_C(Pin_C), _Pin_D(Pin_D) {
}

void RGB_Matrix::Init(void) {
//    uint8_t *ptr;

    _Pin_CLK = 0; // Clock low
    _Pin_OE  = 1; // Disable LED output during row/plane switchover
    _Pin_LAT = 1; // Latch data loaded during *prior* interrupt

    plane     = PLANES - 1;
    row       = ROWS   - 1;

    // Allocate and initialize matrix buffer:

    int buffsize  = WIDTH * ROWS * PLANES,
        allocsize = (DBUF == true) ? (buffsize * 2) : buffsize;

    if(NULL == (matrixbuff[0] = (uint8_t *)malloc(allocsize)))
        return;
    memset(matrixbuff[0], 0, allocsize);

    if(NULL == (matrixbuff[1] = (uint8_t *)malloc(allocsize)))
        return;
    memset(matrixbuff[1], 0, allocsize);

    // If not double-buffered, both buffers then point to the same address:
//    matrixbuff[1] = (DBUF == true) ? &matrixbuff[0][buffsize] : matrixbuff[0];

    bufferDisplay = 0;
    bufferWrite   = 0;
//    buffptr       = matrixbuff[bufferDisplay]; // -> front buffer

    /*
        // Some data!
        buffptr[0] = 0x11;
        buffptr[1] = 0x22;
        buffptr[2] = 0x44;
        buffptr[0+PIXELS] = 0x11;
        buffptr[1+PIXELS] = 0x22;
        buffptr[2+PIXELS] = 0x44;
        buffptr[0+PIXELS*2] = 0x11;
        buffptr[1+PIXELS*2] = 0x22;
        buffptr[2+PIXELS*2] = 0x44;
        buffptr[0+PIXELS*3] = 0x11;
        buffptr[1+PIXELS*3] = 0x22;
        buffptr[2+PIXELS*3] = 0x44;
        buffptr[0+PIXELS*4] = 0x11;
        buffptr[1+PIXELS*4] = 0x22;
        buffptr[2+PIXELS*4] = 0x44;
        buffptr[0+PIXELS*5] = 0x11;
        buffptr[1+PIXELS*5] = 0x22;
        buffptr[2+PIXELS*5] = 0x44;
        buffptr[0+PIXELS*6] = 0x11;
        buffptr[1+PIXELS*6] = 0x22;
        buffptr[2+PIXELS*6] = 0x44;

        buffptr[0+WIDTH] = 0x00;
        buffptr[1+WIDTH] = 0x00;
        buffptr[2+WIDTH] = 0x00;
        buffptr[0+WIDTH+PIXELS] = 0x11;
        buffptr[1+WIDTH+PIXELS] = 0x22;
        buffptr[2+WIDTH+PIXELS] = 0x44;
        buffptr[0+WIDTH+PIXELS*2] = 0x11;
        buffptr[1+WIDTH+PIXELS*2] = 0x22;
        buffptr[2+WIDTH+PIXELS*2] = 0x44;
        buffptr[0+WIDTH+PIXELS*3] = 0x11;
        buffptr[1+WIDTH+PIXELS*3] = 0x22;
        buffptr[2+WIDTH+PIXELS*3] = 0x44;
        buffptr[0+WIDTH+PIXELS*4] = 0x11;
        buffptr[1+WIDTH+PIXELS*4] = 0x22;
        buffptr[2+WIDTH+PIXELS*4] = 0x44;
        buffptr[0+WIDTH+PIXELS*5] = 0x11;
        buffptr[1+WIDTH+PIXELS*5] = 0x22;
        buffptr[2+WIDTH+PIXELS*5] = 0x44;
        buffptr[0+WIDTH+PIXELS*6] = 0x11;
        buffptr[1+WIDTH+PIXELS*6] = 0x22;
        buffptr[2+WIDTH+PIXELS*6] = 0x44;

        buffptr[0+WIDTH*2] = 0x00;
        buffptr[1+WIDTH*2] = 0x00;
        buffptr[2+WIDTH*2] = 0x00;
        buffptr[0+WIDTH*2+PIXELS] = 0x00;
        buffptr[1+WIDTH*2+PIXELS] = 0x00;
        buffptr[2+WIDTH*2+PIXELS] = 0x00;
        buffptr[0+WIDTH*2+PIXELS*2] = 0x11;
        buffptr[1+WIDTH*2+PIXELS*2] = 0x22;
        buffptr[2+WIDTH*2+PIXELS*2] = 0x44;
        buffptr[0+WIDTH*2+PIXELS*3] = 0x11;
        buffptr[1+WIDTH*2+PIXELS*3] = 0x22;
        buffptr[2+WIDTH*2+PIXELS*3] = 0x44;
        buffptr[0+WIDTH*2+PIXELS*4] = 0x11;
        buffptr[1+WIDTH*2+PIXELS*4] = 0x22;
        buffptr[2+WIDTH*2+PIXELS*4] = 0x44;
        buffptr[0+WIDTH*2+PIXELS*5] = 0x11;
        buffptr[1+WIDTH*2+PIXELS*5] = 0x22;
        buffptr[2+WIDTH*2+PIXELS*5] = 0x44;
        buffptr[0+WIDTH*2+PIXELS*6] = 0x11;
        buffptr[1+WIDTH*2+PIXELS*6] = 0x22;
        buffptr[2+WIDTH*2+PIXELS*6] = 0x44;

        buffptr[0+WIDTH*3] = 0x00;
        buffptr[1+WIDTH*3] = 0x00;
        buffptr[2+WIDTH*3] = 0x00;
        buffptr[0+WIDTH*3+PIXELS] = 0x00;
        buffptr[1+WIDTH*3+PIXELS] = 0x00;
        buffptr[2+WIDTH*3+PIXELS] = 0x00;
        buffptr[0+WIDTH*3+PIXELS*2] = 0x00;
        buffptr[1+WIDTH*3+PIXELS*2] = 0x00;
        buffptr[2+WIDTH*3+PIXELS*2] = 0x00;
        buffptr[0+WIDTH*3+PIXELS*3] = 0x11;
        buffptr[1+WIDTH*3+PIXELS*3] = 0x22;
        buffptr[2+WIDTH*3+PIXELS*3] = 0x44;
        buffptr[0+WIDTH*3+PIXELS*4] = 0x11;
        buffptr[1+WIDTH*3+PIXELS*4] = 0x22;
        buffptr[2+WIDTH*3+PIXELS*4] = 0x44;
        buffptr[0+WIDTH*3+PIXELS*5] = 0x11;
        buffptr[1+WIDTH*3+PIXELS*5] = 0x22;
        buffptr[2+WIDTH*3+PIXELS*5] = 0x44;
        buffptr[0+WIDTH*3+PIXELS*6] = 0x11;
        buffptr[1+WIDTH*3+PIXELS*6] = 0x22;
        buffptr[2+WIDTH*3+PIXELS*6] = 0x44;

        buffptr[0+WIDTH*4] = 0x00;
        buffptr[1+WIDTH*4] = 0x00;
        buffptr[2+WIDTH*4] = 0x00;
        buffptr[0+WIDTH*4+PIXELS] = 0x00;
        buffptr[1+WIDTH*4+PIXELS] = 0x00;
        buffptr[2+WIDTH*4+PIXELS] = 0x00;
        buffptr[0+WIDTH*4+PIXELS*2] = 0x00;
        buffptr[1+WIDTH*4+PIXELS*2] = 0x00;
        buffptr[2+WIDTH*4+PIXELS*2] = 0x00;
        buffptr[0+WIDTH*4+PIXELS*3] = 0x00;
        buffptr[1+WIDTH*4+PIXELS*3] = 0x00;
        buffptr[2+WIDTH*4+PIXELS*3] = 0x00;
        buffptr[0+WIDTH*4+PIXELS*4] = 0x11;
        buffptr[1+WIDTH*4+PIXELS*4] = 0x22;
        buffptr[2+WIDTH*4+PIXELS*4] = 0x44;
        buffptr[0+WIDTH*4+PIXELS*5] = 0x11;
        buffptr[1+WIDTH*4+PIXELS*5] = 0x22;
        buffptr[2+WIDTH*4+PIXELS*5] = 0x44;
        buffptr[0+WIDTH*4+PIXELS*6] = 0x11;
        buffptr[1+WIDTH*4+PIXELS*6] = 0x22;
        buffptr[2+WIDTH*4+PIXELS*6] = 0x44;

        buffptr[0+WIDTH*5] = 0x00;
        buffptr[1+WIDTH*5] = 0x00;
        buffptr[2+WIDTH*5] = 0x00;
        buffptr[0+WIDTH*5+PIXELS] = 0x00;
        buffptr[1+WIDTH*5+PIXELS] = 0x00;
        buffptr[2+WIDTH*5+PIXELS] = 0x00;
        buffptr[0+WIDTH*5+PIXELS*2] = 0x00;
        buffptr[1+WIDTH*5+PIXELS*2] = 0x00;
        buffptr[2+WIDTH*5+PIXELS*2] = 0x00;
        buffptr[0+WIDTH*5+PIXELS*3] = 0x00;
        buffptr[1+WIDTH*5+PIXELS*3] = 0x00;
        buffptr[2+WIDTH*5+PIXELS*3] = 0x00;
        buffptr[0+WIDTH*5+PIXELS*4] = 0x00;
        buffptr[1+WIDTH*5+PIXELS*4] = 0x00;
        buffptr[2+WIDTH*5+PIXELS*4] = 0x00;
        buffptr[0+WIDTH*5+PIXELS*5] = 0x11;
        buffptr[1+WIDTH*5+PIXELS*5] = 0x22;
        buffptr[2+WIDTH*5+PIXELS*5] = 0x44;
        buffptr[0+WIDTH*5+PIXELS*6] = 0x11;
        buffptr[1+WIDTH*5+PIXELS*6] = 0x22;
        buffptr[2+WIDTH*5+PIXELS*6] = 0x44;

        buffptr[0+WIDTH*6] = 0x00;
        buffptr[1+WIDTH*6] = 0x00;
        buffptr[2+WIDTH*6] = 0x00;
        buffptr[0+WIDTH*6+PIXELS] = 0x00;
        buffptr[1+WIDTH*6+PIXELS] = 0x00;
        buffptr[2+WIDTH*6+PIXELS] = 0x00;
        buffptr[0+WIDTH*6+PIXELS*2] = 0x00;
        buffptr[1+WIDTH*6+PIXELS*2] = 0x00;
        buffptr[2+WIDTH*6+PIXELS*2] = 0x00;
        buffptr[0+WIDTH*6+PIXELS*3] = 0x00;
        buffptr[1+WIDTH*6+PIXELS*3] = 0x00;
        buffptr[2+WIDTH*6+PIXELS*3] = 0x00;
        buffptr[0+WIDTH*6+PIXELS*4] = 0x00;
        buffptr[1+WIDTH*6+PIXELS*4] = 0x00;
        buffptr[2+WIDTH*6+PIXELS*4] = 0x00;
        buffptr[0+WIDTH*6+PIXELS*5] = 0x00;
        buffptr[1+WIDTH*6+PIXELS*5] = 0x00;
        buffptr[2+WIDTH*6+PIXELS*5] = 0x00;
        buffptr[0+WIDTH*6+PIXELS*6] = 0x11;
        buffptr[1+WIDTH*6+PIXELS*6] = 0x22;
        buffptr[2+WIDTH*6+PIXELS*6] = 0x44;
    */
    /*
        buffptr[1] = 0x02;
        buffptr[2] = 0x03;
        buffptr[3] = 0x04;
        buffptr[4] = 0x08;
        buffptr[5] = 0x0c;
        buffptr[6] = 0x10;
        buffptr[7] = 0x20;
        buffptr[8] = 0x30;
        buffptr[WIDTH*ROWS+3] = 0x01;
        buffptr[WIDTH*ROWS+4] = 0x04;
        buffptr[WIDTH*ROWS+5] = 0x10;

        drawPixel(2, 4, RGB(255, 128, 0)); // RRrrrGGg gggBBbbb
    */

    Update.attach(this, &RGB_Matrix::updateDisplay, INTTIME); // the address of the function to be attached (flip) and the interval (0.5 seconds)
    char_x = 0;
    char_y = 0;
    foreground(RGB(0, 64, 0));
    background(RGB(0, 0, 0));
    SetOrientation(LANDSCAPE_B);
}

void RGB_Matrix::swap(int16_t &x, int16_t &y) {
    int16_t temp = x;
    x = y;
    y = temp;
}

uint16_t RGB_Matrix::rgbToColor(uint8_t R, uint8_t G, uint8_t  B) {
    return ((R >> 6) << 14) + ((G >> 6) << 9) + ((B >> 6) << 3); // RRrrrGGg gggBBbbb
}

void RGB_Matrix::drawPixel2(int16_t x, int16_t y, uint16_t c) {
    uint8_t r, g, b, bit, limit, *ptr;

    if((x < 0) || (x >= WIDTH) || (y < 0) || (y >= HEIGHT))
        return;

    switch(_orientation) {
        case 1:
            swap(x, y);
            x = WIDTH  - 1 - x;
            break;
        case 2:
            x = WIDTH  - 1 - x;
            y = HEIGHT - 1 - y;
            break;
        case 3:
            swap(x, y);
            y = HEIGHT - 1 - y;
            break;
    }

    // Adafruit_GFX uses 16-bit color in 5/6/5 format, while matrix needs
    // 2/2/2.  Pluck out relevant bits while separating into R,G,B:
    r =  c >> 14;        // RRrrrggggggbbbbb
    g = (c >>  9) & 0x03; // rrrrrGGggggbbbbb
    b = (c >>  3) & 0x03; // rrrrrggggggBBbbb

    uint8_t color = r + (g << 2) + (b << 4);

    buffptr = matrixbuff[bufferWrite]; // Set first buffer
    buffptr[y * WIDTH + x] = color;
}

void RGB_Matrix::drawPixel(int16_t x, int16_t y, uint16_t c) {
    uint8_t r, g, b, bit, limit, *ptr;

    if((x < 0) || (x >= WIDTH) || (y < 0) || (y >= HEIGHT))
        return;

    switch(_orientation) {
        case 1:
            swap(x, y);
            x = WIDTH  - 1 - x;
            break;
        case 2:
            x = WIDTH  - 1 - x;
            y = HEIGHT - 1 - y;
            break;
        case 3:
            swap(x, y);
            y = HEIGHT - 1 - y;
            break;
    }

    // Adafruit_GFX uses 16-bit color in 5/6/5 format, while matrix needs
    // 2/2/2.  Pluck out relevant bits while separating into R,G,B:
    r =  c >> 13;         // RRRrrggggggbbbbb
    g = (c >>  8) & 0x07; // rrrrrGGGgggbbbbb
    b = (c >>  2) & 0x07; // rrrrrggggggBBBbb

    uint8_t color = r + (g << 2) + (b << 4);


    uint8_t *buffptr;
    buffptr = matrixbuff[bufferWrite]; // Set first buffer

    const uint8_t PixelR1[] = {
        0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe,
        0xfe, 0xfe, 0xfe, 0x01, 0xfe, 0xfe, 0xfe,
        0xfe, 0x01, 0xfe, 0xfe, 0xfe, 0x01, 0xfe,
        0xfe, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xfe,
        0x01, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xfe,
        0x01, 0x01, 0x01, 0xfe, 0x01, 0x01, 0xfe,
        0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0xfe,
        0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01
    };

    const uint8_t PixelR2[] = {
        0xef, 0xef, 0xef, 0xef, 0xef, 0xef, 0xef,
        0xef, 0xef, 0xef, 0x10, 0xef, 0xef, 0xef,
        0xef, 0x10, 0xef, 0xef, 0xef, 0x10, 0xef,
        0xef, 0x10, 0xef, 0x10, 0xef, 0x10, 0xef,
        0x10, 0x10, 0xef, 0x10, 0xef, 0x10, 0xef,
        0x10, 0x10, 0x10, 0xef, 0x10, 0x10, 0xef,
        0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0xef,
        0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10
    };

    const uint8_t PixelG1[] = {
        0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd,
        0xfd, 0xfd, 0xfd, 0x02, 0xfd, 0xfd, 0xfd,
        0xfd, 0x02, 0xfd, 0xfd, 0xfd, 0x02, 0xfd,
        0xfd, 0x02, 0xfd, 0x02, 0xfd, 0x02, 0xfd,
        0x02, 0x02, 0xfd, 0x02, 0xfd, 0x02, 0xfd,
        0x02, 0x02, 0x02, 0xfd, 0x02, 0x02, 0xfd,
        0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0xfd,
        0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02
    };

    const uint8_t PixelG2[] = {
        0xdf, 0xdf, 0xdf, 0xdf, 0xdf, 0xdf, 0xdf,
        0xdf, 0xdf, 0xdf, 0x20, 0xdf, 0xdf, 0xdf,
        0xdf, 0x20, 0xdf, 0xdf, 0xdf, 0x20, 0xdf,
        0xdf, 0x20, 0xdf, 0x20, 0xdf, 0x20, 0xdf,
        0x20, 0x20, 0xdf, 0x20, 0xdf, 0x20, 0xdf,
        0x20, 0x20, 0x20, 0xdf, 0x20, 0x20, 0xdf,
        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0xdf,
        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
    };

    const uint8_t PixelB1[] = {
        0xfb, 0xfb, 0xfb, 0xfb, 0xfb, 0xfb, 0xfb,
        0xfb, 0xfb, 0xfb, 0x04, 0xfb, 0xfb, 0xfb,
        0xfb, 0x04, 0xfb, 0xfb, 0xfb, 0x04, 0xfb,
        0xfb, 0x04, 0xfb, 0x04, 0xfb, 0x04, 0xfb,
        0x04, 0x04, 0xfb, 0x04, 0xfb, 0x04, 0xfb,
        0x04, 0x04, 0x04, 0xfb, 0x04, 0x04, 0xfb,
        0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0xfb,
        0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04
    };

    const uint8_t PixelB2[] = {
        0xbf, 0xbf, 0xbf, 0xbf, 0xbf, 0xbf, 0xbf,
        0xbf, 0xbf, 0xbf, 0x40, 0xbf, 0xbf, 0xbf,
        0xbf, 0x40, 0xbf, 0xbf, 0xbf, 0x40, 0xbf,
        0xbf, 0x40, 0xbf, 0x40, 0xbf, 0x40, 0xbf,
        0x40, 0x40, 0xbf, 0x40, 0xbf, 0x40, 0xbf,
        0x40, 0x40, 0x40, 0xbf, 0x40, 0x40, 0xbf,
        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0xbf,
        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40
    };

    // PWM planes
    // xBGRxBGR

    for (uint8_t p = 0; p < PLANES; p++) {
        if (y < ROWS) {
            if ((PixelR1[p+r*7] & 0x80) == 0)
                buffptr[x+WIDTH*y+PIXELS*p] |= PixelR1[p+r*7];
            else
                buffptr[x+WIDTH*y+PIXELS*p] &= PixelR1[p+r*7];
            if ((PixelG1[p+g*7] & 0x80) == 0)
                buffptr[x+WIDTH*y+PIXELS*p] |= PixelG1[p+g*7];
            else
                buffptr[x+WIDTH*y+PIXELS*p] &= PixelG1[p+g*7];
            if ((PixelB1[p+b*7] & 0x80) == 0)
                buffptr[x+WIDTH*y+PIXELS*p] |= PixelB1[p+b*7];
            else
                buffptr[x+WIDTH*y+PIXELS*p] &= PixelB1[p+b*7];
        } else {
            if ((PixelR2[p+r*7] & 0x80) == 0)
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] |= PixelR2[p+r*7];
            else
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] &= PixelR2[p+r*7];
            if ((PixelG2[p+g*7] & 0x80) == 0)
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] |= PixelG2[p+g*7];
            else
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] &= PixelG2[p+g*7];
            if ((PixelB2[p+b*7] & 0x80) == 0)
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] |= PixelB2[p+b*7];
            else
                buffptr[x+WIDTH*(y-ROWS)+PIXELS*p] &= PixelB2[p+b*7];
        }
    }
}

void RGB_Matrix::set_font(unsigned char* f) {
    font = f;
}

void RGB_Matrix::character(int x, int y, int c) {
    unsigned int hor,vert,offset,bpl,j,i,b;
    unsigned char* char_offset;
    unsigned char z,w;

    if ((c < 31) || (c > 127)) return;   // test char range

    // read font parameter from start of array
    offset = font[BYTESPERCHAR];                    // bytes / char
    hor    = font[FONTWIDTH];                       // get hor size of font
    vert   = font[FONTHEIGHT];                      // get vert size of font
    bpl    = font[BYTESPERLINE];                    // bytes per vertical line

    if (char_x + hor > width()) {
        // the next character doesn't fit on the same line
        char_x = 0;
        char_y = char_y + vert;
        if (char_y > height() - font[FONTHEIGHT]) {
            // the next line doesn't fit at the bottom of the screen
            char_y = 0;
        }
    }

//    window(char_x, char_y,hor,vert); // char box

    char_offset = &font[((c -32) * offset) + 4]; // start of char bitmap
    w = char_offset[0];                          // width of actual char
    for (j=0; j<vert; j++) {  //  vert line
        for (i=0; i<hor; i++) {   //  horz line
            z =  char_offset[bpl * i + ((j & 0xF8) >> 3)+1];
            b = 1 << (j & 0x07);
            if (( z & b ) == 0x00) {
                drawPixel(char_x + i, char_y + j, _background); // background
            } else {
                drawPixel(char_x + i, char_y + j, _foreground); // foreground
            }
        }
    }
//    WindowMax();
    if ((w + 2) < hor) {                   // x offset to next char
        char_x += w + 2;
    } else char_x += hor;
}

int RGB_Matrix::putc(int value) {
    if (value == '\n') {    // new line
        char_x = 0;
        char_y = char_y + font[FONTHEIGHT];
        if (char_y >= height() - font[FONTHEIGHT]) {
            char_y = 0;
        }
    } else {
        character(char_x, char_y, value);
    }
    return value;
}

void RGB_Matrix::printString(char *string) {
    for (uint8_t i = 0; i < strlen(string); i++) {
        putc(string[i]);
    }
}

void RGB_Matrix::printStringCenter(char *string) {
    uint8_t pwidth = strlen(string) * font[FONTWIDTH] - 1;
    locatePixelX((width() - pwidth) / 2);
    for (uint8_t i = 0; i < strlen(string); i++) {
        putc(string[i]);
    }
}

void RGB_Matrix::foreground(uint16_t colour) {
    _foreground = colour;
}

void RGB_Matrix::background(uint16_t colour) {
    _background = colour;
}

int RGB_Matrix::width() {
    if (_orientation == 0 || _orientation == 2) return WIDTH;
    else return HEIGHT;
}

int RGB_Matrix::height() {
    if (_orientation == 0 || _orientation == 2) return HEIGHT;
    else return WIDTH;
}

void RGB_Matrix::SetOrientation(uint8_t orientation) {
    _orientation = orientation;
}

void RGB_Matrix::locate(uint8_t x, uint8_t y) {
    char_x = x * font[FONTHEIGHT];
    char_y = y * font[FONTHEIGHT];
}

void RGB_Matrix::locatePixelX(uint8_t x) {
    char_x = x;
}

void RGB_Matrix::locatePixelY(uint8_t y) {
    char_y = y;
}

int RGB_Matrix::columns() {
    return width() / font[FONTHEIGHT];
}

int RGB_Matrix::rows() {
    return height() / font[FONTHEIGHT];
}

void RGB_Matrix::setDisplayBuffer(uint8_t Buffer) {
    bufferDisplay = Buffer;
}

void RGB_Matrix::setWriteBuffer(uint8_t Buffer) {
    bufferWrite = Buffer;
}

void RGB_Matrix::updateDisplay2(void) {
    _Pin_OE  = 1;  // Disable LED output during row/plane switchover
    _Pin_LAT = 1; // Latch data loaded during *prior* interrupt
    _Pin_A   = (row & 0x01);
    _Pin_B   = (row & 0x02) >> 1;
    _Pin_C   = (row & 0x04) >> 2;
    _Pin_D   = (row & 0x08) >> 3;
    _Pin_OE  = 0;   // Re-enable output
    _Pin_LAT = 0;  // Latch down

    if(++plane >= PLANES) {      // Advance plane counter.  Maxed out?
        plane = 0;                // Yes, reset to plane 0, and
    }
    if(++row >= ROWS) {       // advance row counter.  Maxed out?
        row = 0;              // Yes, reset row counter, then...
    }

    buffptr = matrixbuff[bufferDisplay]; // Set first buffer

    for(int x = 0; x < WIDTH; x++) {
        int16_t BufferIndex1 = x + row * WIDTH;
        int16_t BufferIndex2 = BufferIndex1 + ROWS * WIDTH;

        uint8_t PixelValueR1 =  buffptr[BufferIndex1] & 0x03;
        uint8_t PixelValueG1 = (buffptr[BufferIndex1] >> 2) & 0x03;
        uint8_t PixelValueB1 = (buffptr[BufferIndex1] >> 4) & 0x03;
        uint8_t PixelValueR2 =  buffptr[BufferIndex2] & 0x03;
        uint8_t PixelValueG2 = (buffptr[BufferIndex2] >> 2) & 0x03;
        uint8_t PixelValueB2 = (buffptr[BufferIndex2] >> 4) & 0x03;

        if (PixelValueR1 >= plane+1) {
            _Pin_R1 = 1;
        } else {
            _Pin_R1 = 0;
        }
        if (PixelValueG1 >= plane+1) {
            _Pin_G1 = 1;
        } else {
            _Pin_G1 = 0;
        }
        if (PixelValueB1 >= plane+1) {
            _Pin_B1 = 1;
        } else {
            _Pin_B1 = 0;
        }

        if (PixelValueR2 >= plane+1) {
            _Pin_R2 = 1;
        } else {
            _Pin_R2 = 0;
        }
        if (PixelValueG2 >= plane+1) {
            _Pin_G2 = 1;
        } else {
            _Pin_G2 = 0;
        }
        if (PixelValueB2 >= plane+1) {
            _Pin_B2 = 1;
        } else {
            _Pin_B2 = 0;
        }

        _Pin_CLK = 0; // Clock low
        _Pin_CLK = 1; // Clock high
    }
}

void RGB_Matrix::updateDisplay(void) {
    _Pin_OE  = 1;  // Disable LED output during row/plane switchover
    _Pin_LAT = 1; // Latch data loaded during *prior* interrupt
    _Pin_A   =  row & 0x01;
    _Pin_B   = (row >> 1) & 0x01;
    _Pin_C   = (row >> 2) & 0x01;
    _Pin_D   = (row >> 3) & 0x01;
    _Pin_OE  = 0;   // Re-enable output
    _Pin_LAT = 0;  // Latch down

    if(++plane >= PLANES) {       // Advance plane counter.  Maxed out?
        plane = 0;                // Yes, reset to plane 0, and
        if(++row >= ROWS) {       // advance row counter.  Maxed out?
            row = 0;              // Yes, reset row counter, then...
        }
    }

    buffptr = matrixbuff[bufferDisplay]; // Set display buffer
    for(uint8_t x = 0; x < WIDTH; x++) {
        uint16_t BufferIndex = x + row * WIDTH + plane * PIXELS;
        _Pin_R1  =  buffptr[BufferIndex] & 0x01;
        _Pin_G1  = (buffptr[BufferIndex] >> 1) & 0x01;
        _Pin_B1  = (buffptr[BufferIndex] >> 2) & 0x01;
        _Pin_R2  = (buffptr[BufferIndex] >> 4) & 0x01;
        _Pin_G2  = (buffptr[BufferIndex] >> 5) & 0x01;
        _Pin_B2  = (buffptr[BufferIndex] >> 6) & 0x01;
        _Pin_CLK = 0; // Clock low
        _Pin_CLK = 1; // Clock high
    }
}