KSM edits to RA8875

Dependents:   Liz_Test_Code

RA8875.cpp

Committer:
kerrysmartin
Date:
4 months ago
Revision:
177:8620cdfcdbf2
Parent:
176:4ab96d33a8ec

File content as of revision 177:8620cdfcdbf2:

// RA8875 Display Controller Library.
//
// This is being created for a Raio RA8875-based display from buydisplay.com,
// which is either 480 x 272 or 800 x 480, using a 4-wire SPI interface.
// Support is provided for both a keypad and a resistive touch-screen.
//
// See the RA8875.h file for full details.
//
// 20161106: Updated the initialization to set the various registers based on
//           the BuyDisplay.com example code. This altered several registers
//           for the 800x480 display driver.
//
#include "RA8875.h"

//#include "Utility.h"            // private memory manager
#ifndef UTILITY_H
#define swMalloc malloc         // use the standard
#define swFree free
#endif

//#define DEBUG "RAIO"
// ...
// INFO("Stuff to show %d", var); // new-line is automatically appended
//
#if (defined(DEBUG) && !defined(TARGET_LPC11U24))
#define INFO(x, ...) std::printf("[INF %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#define WARN(x, ...) std::printf("[WRN %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#define ERR(x, ...)  std::printf("[ERR %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
static void HexDump(const char * title, const uint8_t * p, int count)
{
    int i;
    char buf[100] = "0000: ";

    if (*title)
        INFO("%s", title);
    for (i=0; i<count; ) {
        sprintf(buf + strlen(buf), "%02X ", *(p+i));
        if ((++i & 0x0F) == 0x00) {
            INFO("%s", buf);
            if (i < count)
                sprintf(buf, "%04X: ", i);
            else
                buf[0] = '\0';
        }
    }
    if (strlen(buf))
        INFO("%s", buf);
}
#else
#define INFO(x, ...)
#define WARN(x, ...)
#define ERR(x, ...)
#define HexDump(a, b, c)
#endif

// Defaults. Users can override this with the init() method.
#define RA8875_DISPLAY_WIDTH  480
#define RA8875_DISPLAY_HEIGHT 272
#define RA8875_COLORDEPTH_BPP 16    /* Not an API */

#ifdef PERF_METRICS
#define PERFORMANCE_RESET performance.reset()
#define REGISTERPERFORMANCE(a) RegisterPerformance(a)
#define COUNTIDLETIME(a) CountIdleTime(a)
static const char *metricsName[] = {
    "Cls", "Pixel", "Pixel Stream", "Boolean Stream",
    "Read Pixel", "Read Pixel Stream",
    "Line",
    "Rectangle", "Rounded Rectangle",
    "Triangle", "Circle", "Ellipse"
};
uint16_t commandsUsed[256];  // track which commands are used with simple counter of number of hits.
#else
#define PERFORMANCE_RESET
#define REGISTERPERFORMANCE(a)
#define COUNTIDLETIME(a)
#endif

// When it is going to poll a register for completion, how many
// uSec should it wait between each polling activity.
#define POLLWAITuSec 10

// Private RawKeyMap for the Keyboard interface
static const uint8_t DefaultKeyMap[22] = {
    0,
    1,  2,  3,  4,  5,  6,  7,  8,  9, 10,
    11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
    255
};

static const char * ErrMessages[] = {
    "noerror",                ///< no errors, command completed successfully
    "bad parameter",          ///< one or more parameters are invalid
    "file not found",         ///< specified file could not be found
    "not bmp format",         ///< file is not a .bmp file
    "not ico format",         ///< file is not a .ico file
    "not supported format",   ///< file format is not yet supported
    "image too big",          ///< image is too large for the screen
    "not enough ram",         ///< could not allocate ram for scanline
    "touch cal. timeout",     ///< calibration could not complete in time
    "external abort",         ///< during an idle callback, the user code initiated an abort
};

typedef struct {
    uint8_t b;
    uint8_t g;
    uint8_t r;
    uint8_t a;
} rgbTrio_t;

/// This is defined as a "Web-Safe" color palette of 216 colors. 
///
/// It is defined so it can be emitted into a BMP file as the color palette, and it is then used
/// for downscaling from higher resolution color depth to an 8-bit format.
///
static const rgbTrio_t WebColorPalette[] = {
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x33,0xFF}, {0x00,0x00,0x66,0xFF}, {0x00,0x00,0x99,0xFF}, {0x00,0x00,0xCC,0xFF}, {0x00,0x00,0xFF,0xFF},
    {0x00,0x33,0x00,0xFF}, {0x00,0x33,0x33,0xFF}, {0x00,0x33,0x66,0xFF}, {0x00,0x33,0x99,0xFF}, {0x00,0x33,0xCC,0xFF}, {0x00,0x33,0xFF,0xFF},
    {0x00,0x66,0x00,0xFF}, {0x00,0x66,0x33,0xFF}, {0x00,0x66,0x66,0xFF}, {0x00,0x66,0x99,0xFF}, {0x00,0x66,0xCC,0xFF}, {0x00,0x66,0xFF,0xFF},
    {0x00,0x99,0x00,0xFF}, {0x00,0x99,0x33,0xFF}, {0x00,0x99,0x66,0xFF}, {0x00,0x99,0x99,0xFF}, {0x00,0x99,0xCC,0xFF}, {0x00,0x99,0xFF,0xFF},
    {0x00,0xCC,0x00,0xFF}, {0x00,0xCC,0x33,0xFF}, {0x00,0xCC,0x66,0xFF}, {0x00,0xCC,0x99,0xFF}, {0x00,0xCC,0xCC,0xFF}, {0x00,0xCC,0xFF,0xFF},
    {0x00,0xFF,0x00,0xFF}, {0x00,0xFF,0x33,0xFF}, {0x00,0xFF,0x66,0xFF}, {0x00,0xFF,0x99,0xFF}, {0x00,0xFF,0xCC,0xFF}, {0x00,0xFF,0xFF,0xFF},
    {0x33,0x00,0x00,0xFF}, {0x33,0x00,0x33,0xFF}, {0x33,0x00,0x66,0xFF}, {0x33,0x00,0x99,0xFF}, {0x33,0x00,0xCC,0xFF}, {0x33,0x00,0xFF,0xFF},
    {0x33,0x33,0x00,0xFF}, {0x33,0x33,0x33,0xFF}, {0x33,0x33,0x66,0xFF}, {0x33,0x33,0x99,0xFF}, {0x33,0x33,0xCC,0xFF}, {0x33,0x33,0xFF,0xFF},
    {0x33,0x66,0x00,0xFF}, {0x33,0x66,0x33,0xFF}, {0x33,0x66,0x66,0xFF}, {0x33,0x66,0x99,0xFF}, {0x33,0x66,0xCC,0xFF}, {0x33,0x66,0xFF,0xFF},
    {0x33,0x99,0x00,0xFF}, {0x33,0x99,0x33,0xFF}, {0x33,0x99,0x66,0xFF}, {0x33,0x99,0x99,0xFF}, {0x33,0x99,0xCC,0xFF}, {0x33,0x99,0xFF,0xFF},
    {0x33,0xCC,0x00,0xFF}, {0x33,0xCC,0x33,0xFF}, {0x33,0xCC,0x66,0xFF}, {0x33,0xCC,0x99,0xFF}, {0x33,0xCC,0xCC,0xFF}, {0x33,0xCC,0xFF,0xFF},
    {0x33,0xFF,0x00,0xFF}, {0x33,0xFF,0x33,0xFF}, {0x33,0xFF,0x66,0xFF}, {0x33,0xFF,0x99,0xFF}, {0x33,0xFF,0xCC,0xFF}, {0x33,0xFF,0xFF,0xFF},
    {0x66,0x00,0x00,0xFF}, {0x66,0x00,0x33,0xFF}, {0x66,0x00,0x66,0xFF}, {0x66,0x00,0x99,0xFF}, {0x66,0x00,0xCC,0xFF}, {0x66,0x00,0xFF,0xFF},
    {0x66,0x33,0x00,0xFF}, {0x66,0x33,0x33,0xFF}, {0x66,0x33,0x66,0xFF}, {0x66,0x33,0x99,0xFF}, {0x66,0x33,0xCC,0xFF}, {0x66,0x33,0xFF,0xFF},
    {0x66,0x66,0x00,0xFF}, {0x66,0x66,0x33,0xFF}, {0x66,0x66,0x66,0xFF}, {0x66,0x66,0x99,0xFF}, {0x66,0x66,0xCC,0xFF}, {0x66,0x66,0xFF,0xFF},
    {0x66,0x99,0x00,0xFF}, {0x66,0x99,0x33,0xFF}, {0x66,0x99,0x66,0xFF}, {0x66,0x99,0x99,0xFF}, {0x66,0x99,0xCC,0xFF}, {0x66,0x99,0xFF,0xFF},
    {0x66,0xCC,0x00,0xFF}, {0x66,0xCC,0x33,0xFF}, {0x66,0xCC,0x66,0xFF}, {0x66,0xCC,0x99,0xFF}, {0x66,0xCC,0xCC,0xFF}, {0x66,0xCC,0xFF,0xFF},
    {0x66,0xFF,0x00,0xFF}, {0x66,0xFF,0x33,0xFF}, {0x66,0xFF,0x66,0xFF}, {0x66,0xFF,0x99,0xFF}, {0x66,0xFF,0xCC,0xFF}, {0x66,0xFF,0xFF,0xFF},
    {0x99,0x00,0x00,0xFF}, {0x99,0x00,0x33,0xFF}, {0x99,0x00,0x66,0xFF}, {0x99,0x00,0x99,0xFF}, {0x99,0x00,0xCC,0xFF}, {0x99,0x00,0xFF,0xFF},
    {0x99,0x33,0x00,0xFF}, {0x99,0x33,0x33,0xFF}, {0x99,0x33,0x66,0xFF}, {0x99,0x33,0x99,0xFF}, {0x99,0x33,0xCC,0xFF}, {0x99,0x33,0xFF,0xFF},
    {0x99,0x66,0x00,0xFF}, {0x99,0x66,0x33,0xFF}, {0x99,0x66,0x66,0xFF}, {0x99,0x66,0x99,0xFF}, {0x99,0x66,0xCC,0xFF}, {0x99,0x66,0xFF,0xFF},
    {0x99,0x99,0x00,0xFF}, {0x99,0x99,0x33,0xFF}, {0x99,0x99,0x66,0xFF}, {0x99,0x99,0x99,0xFF}, {0x99,0x99,0xCC,0xFF}, {0x99,0x99,0xFF,0xFF},
    {0x99,0xCC,0x00,0xFF}, {0x99,0xCC,0x33,0xFF}, {0x99,0xCC,0x66,0xFF}, {0x99,0xCC,0x99,0xFF}, {0x99,0xCC,0xCC,0xFF}, {0x99,0xCC,0xFF,0xFF},
    {0x99,0xFF,0x00,0xFF}, {0x99,0xFF,0x33,0xFF}, {0x99,0xFF,0x66,0xFF}, {0x99,0xFF,0x99,0xFF}, {0x99,0xFF,0xCC,0xFF}, {0x99,0xFF,0xFF,0xFF},
    {0xCC,0x00,0x00,0xFF}, {0xCC,0x00,0x33,0xFF}, {0xCC,0x00,0x66,0xFF}, {0xCC,0x00,0x99,0xFF}, {0xCC,0x00,0xCC,0xFF}, {0xCC,0x00,0xFF,0xFF},
    {0xCC,0x33,0x00,0xFF}, {0xCC,0x33,0x33,0xFF}, {0xCC,0x33,0x66,0xFF}, {0xCC,0x33,0x99,0xFF}, {0xCC,0x33,0xCC,0xFF}, {0xCC,0x33,0xFF,0xFF},
    {0xCC,0x66,0x00,0xFF}, {0xCC,0x66,0x33,0xFF}, {0xCC,0x66,0x66,0xFF}, {0xCC,0x66,0x99,0xFF}, {0xCC,0x66,0xCC,0xFF}, {0xCC,0x66,0xFF,0xFF},
    {0xCC,0x99,0x00,0xFF}, {0xCC,0x99,0x33,0xFF}, {0xCC,0x99,0x66,0xFF}, {0xCC,0x99,0x99,0xFF}, {0xCC,0x99,0xCC,0xFF}, {0xCC,0x99,0xFF,0xFF},
    {0xCC,0xCC,0x00,0xFF}, {0xCC,0xCC,0x33,0xFF}, {0xCC,0xCC,0x66,0xFF}, {0xCC,0xCC,0x99,0xFF}, {0xCC,0xCC,0xCC,0xFF}, {0xCC,0xCC,0xFF,0xFF},
    {0xCC,0xFF,0x00,0xFF}, {0xCC,0xFF,0x33,0xFF}, {0xCC,0xFF,0x66,0xFF}, {0xCC,0xFF,0x99,0xFF}, {0xCC,0xFF,0xCC,0xFF}, {0xCC,0xFF,0xFF,0xFF},
    {0xFF,0x00,0x00,0xFF}, {0xFF,0x00,0x33,0xFF}, {0xFF,0x00,0x66,0xFF}, {0xFF,0x00,0x99,0xFF}, {0xFF,0x00,0xCC,0xFF}, {0xFF,0x00,0xFF,0xFF},
    {0xFF,0x33,0x00,0xFF}, {0xFF,0x33,0x33,0xFF}, {0xFF,0x33,0x66,0xFF}, {0xFF,0x33,0x99,0xFF}, {0xFF,0x33,0xCC,0xFF}, {0xFF,0x33,0xFF,0xFF},
    {0xFF,0x66,0x00,0xFF}, {0xFF,0x66,0x33,0xFF}, {0xFF,0x66,0x66,0xFF}, {0xFF,0x66,0x99,0xFF}, {0xFF,0x66,0xCC,0xFF}, {0xFF,0x66,0xFF,0xFF},
    {0xFF,0x99,0x00,0xFF}, {0xFF,0x99,0x33,0xFF}, {0xFF,0x99,0x66,0xFF}, {0xFF,0x99,0x99,0xFF}, {0xFF,0x99,0xCC,0xFF}, {0xFF,0x99,0xFF,0xFF},
    {0xFF,0xCC,0x00,0xFF}, {0xFF,0xCC,0x33,0xFF}, {0xFF,0xCC,0x66,0xFF}, {0xFF,0xCC,0x99,0xFF}, {0xFF,0xCC,0xCC,0xFF}, {0xFF,0xCC,0xFF,0xFF},
    {0xFF,0xFF,0x00,0xFF}, {0xFF,0xFF,0x33,0xFF}, {0xFF,0xFF,0x66,0xFF}, {0xFF,0xFF,0x99,0xFF}, {0xFF,0xFF,0xCC,0xFF}, {0xFF,0xFF,0xFF,0xFF},

    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
    {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, {0x00,0x00,0x00,0xFF}, 
};

#define sqr(a) ((a) * (a))

/// Find the nearest color match in the lookup table.
///
/// The typical process is to find the difference between a given color and each entry in
/// the table. The difference is defined as:
///     diff = sqrt(sqr(r - table[i].r) + sqr(g - table[i].g) + sqr(b - table[i].b))
/// The square root function is very CPU intensive, especially w/o a floating point unit,
/// so that step is omitted to speed it up a bit.
///
static int FindNearestWebColor(uint8_t r, uint8_t g, uint8_t b) {
    int bestNdx = 0;
    float bestDiff = (sqr(r - WebColorPalette[0].r) + sqr(g - WebColorPalette[0].g) + sqr(b - WebColorPalette[0].b));
    for (int i=1; i<216; i++) {
        float thisDiff = (sqr(r - WebColorPalette[i].r) + sqr(g - WebColorPalette[i].g) + sqr(b - WebColorPalette[i].b));
        if (thisDiff < bestDiff) {
            bestDiff = thisDiff;
            bestNdx = i;
        }
    }
    return bestNdx;
}

// Non-Touch, or Resistive Touch when later initialized that way
//
RA8875::RA8875(PinName mosi, PinName miso, PinName sclk, PinName csel, PinName reset, 
    const char *name)
    : GraphicsDisplay(name)
    , spi(mosi, miso, sclk)
    , cs(csel)
    , res(reset)
{
    touchInfo = (touchInfo_T *)malloc(RESISTIVE_TOUCH_POINTS * sizeof(touchInfo_T));
    if (touchInfo)
        useTouchPanel = TP_RES;
    else
        useTouchPanel = TP_NONE;    // unfortnately a silent failure, but unlikely
    tpFQFN = NULL;
    tpCalMessage = NULL;
    m_irq = NULL;
    m_i2c = NULL;
    m_wake = NULL;
    c_callback = NULL;
    obj_callback = NULL;
    method_callback = NULL;
    idle_callback = NULL;
    fontScaleX = fontScaleY = 1;
}

// Touch, based on FT5206 Controller Chip
//
RA8875::RA8875(PinName mosi, PinName miso, PinName sclk, PinName csel, PinName reset, 
    PinName sda, PinName scl, PinName irq, const char * name) 
    : GraphicsDisplay(name)
    , spi(mosi, miso, sclk)
    , cs(csel)
    , res(reset)
{
    tpFQFN = NULL;
    tpCalMessage = NULL;
    m_irq = new InterruptIn(irq);
    m_i2c = new I2C(sda, scl);

    // Cap touch panel config
    touchInfo = (touchInfo_T *)malloc(FT5206_TOUCH_POINTS * sizeof(touchInfo_T));
    if (touchInfo)
        useTouchPanel = TP_FT5206;
    else
        useTouchPanel = TP_NONE;    // unfortnately a silent failure, but unlikely
    m_addr = (FT5206_I2C_ADDRESS << 1);
    m_i2c->frequency(FT5206_I2C_FREQUENCY);
    m_wake = NULL;      // not used for FT5206
    
    c_callback = NULL;
    obj_callback = NULL;
    method_callback = NULL;
    idle_callback = NULL;
    fontScaleX = fontScaleY = 1;

    // Interrupt
    m_irq->mode(PullUp);
    m_irq->enable_irq();
    #if MBED_VERSION >= MBED_ENCODE_VERSION(5,8,0)
    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
    m_irq->fall(queue.event(callback(this, &RA8875::TouchPanelISR)));
    #elif (MBED_MAJOR_VERSION >= 5) || (MBED_LIBRARY_VERSION > 128)   // Is this the right version?
    m_irq->fall(callback(this, &RA8875::TouchPanelISR));
    #else
    m_irq->fall(this, &RA8875::TouchPanelISR);
    #endif
    TouchPanelInit();
}


// Touch, based on GSL1680 Controller Chip
//
RA8875::RA8875(PinName mosi, PinName miso, PinName sclk, PinName csel, PinName reset, 
    PinName sda, PinName scl, PinName wake, PinName irq, const char * name) 
    : GraphicsDisplay(name)
    , spi(mosi, miso, sclk)
    , cs(csel)
    , res(reset)
{
    tpFQFN = NULL;
    tpCalMessage = NULL;
    m_irq = new InterruptIn(irq);
    m_i2c = new I2C(sda, scl);

    // Cap touch panel config
    touchInfo = (touchInfo_T *)malloc(FT5206_TOUCH_POINTS * sizeof(touchInfo_T));
    if (touchInfo)
        useTouchPanel = TP_GSL1680;
    else
        useTouchPanel = TP_NONE;    // unfortnately a silent failure, but unlikely
    m_addr = (GSL1680_I2C_ADDRESS << 1);
    m_i2c->frequency(GSL1680_I2C_FREQUENCY);
    m_wake = new DigitalOut(wake);

    c_callback = NULL;
    obj_callback = NULL;
    method_callback = NULL;
    idle_callback = NULL;
    fontScaleX = fontScaleY = 1;

    // Interrupt
    m_irq->mode(PullUp);
    m_irq->enable_irq();
    #if MBED_VERSION >= MBED_ENCODE_VERSION(5,8,0)
    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
    m_irq->fall(queue.event(callback(this, &RA8875::TouchPanelISR)));
    #elif (MBED_MAJOR_VERSION >= 5) || (MBED_LIBRARY_VERSION > 128)   // Is this the right version?
    m_irq->fall(callback(this, &RA8875::TouchPanelISR));
    #else
    m_irq->fall(this, &RA8875::TouchPanelISR);
    #endif
    TouchPanelInit();
}



//RA8875::~RA8875()
//{
//}


RetCode_t RA8875::init(int width, int height, int color_bpp, uint8_t poweron, bool keypadon, bool touchscreenon)
{
    font = NULL;                                // no external font, use internal.
    pKeyMap = DefaultKeyMap;                    // set default key map
    _select(false);                             // deselect the display
    frequency(RA8875_DEFAULT_SPI_FREQ);         // data rate
    Reset();
    // Set PLL based on display size from buy-display.com sample code
    if (width == 800) {
        WriteCommand(RA8875_PLLC1, 0x0C);               // PLLC1 - Phase Lock Loop registers
    } else {
        WriteCommand(RA8875_PLLC1, 0x0B);               // PLLC1 - Phase Lock Loop registers
    }
    wait_ms(1);
    WriteCommand(RA8875_PLLC2, 0x02);
    wait_ms(1);

    // System Config Register (SYSR)
    screenbpp = color_bpp;
    if (color_bpp == 16) {
        WriteCommand(RA8875_SYSR, 0x0C);               // 16-bpp (65K colors) color depth, 8-bit interface
    } else { // color_bpp == 8
        WriteCommand(RA8875_SYSR, 0x00);               // 8-bpp (256 colors)
    }

    // Set Pixel Clock Setting Register (PCSR) based on display size from buy-display.com sample code
    if (width == 800) {
        WriteCommand(RA8875_PCSR, 0x81);               // PDAT on PCLK falling edge, PCLK = 4 x System Clock
        wait_ms(1);

        // Horizontal Settings
        screenwidth = width;
        WriteCommand(RA8875_HDWR, width/8 - 1);            //HDWR//Horizontal Display Width Setting Bit[6:0]
        WriteCommand(RA8875_HNDFTR, 0x00);                   //HNDFCR//Horizontal Non-Display Period fine tune Bit[3:0]
        WriteCommand(RA8875_HNDR, 0x03);                   //HNDR//Horizontal Non-Display Period Bit[4:0]
        WriteCommand(RA8875_HSTR, 0x03);                   //HSTR//HSYNC Start Position[4:0]
        WriteCommand(RA8875_HPWR, 0x0B);                   //HPWR//HSYNC Polarity ,The period width of HSYNC.

        // Vertical Settings
        screenheight = height;
        WriteCommand(RA8875_VDHR0, (height-1)&0xFF);        //VDHR0 //Vertical Display Height Bit [7:0]
        WriteCommand(RA8875_VDHR1, (height-1)>>8);          //VDHR1 //Vertical Display Height Bit [8]
        WriteCommand(RA8875_VNDR0, 0x20);                   //VNDR0 //Vertical Non-Display Period Bit [7:0]
        WriteCommand(RA8875_VNDR1, 0x00);                   //VNDR1 //Vertical Non-Display Period Bit [8]
        WriteCommand(RA8875_VSTR0, 0x16);                   //VSTR0 //VSYNC Start Position[7:0]
        WriteCommand(RA8875_VSTR1, 0x00);                   //VSTR1 //VSYNC Start Position[8]
        WriteCommand(RA8875_VPWR, 0x01);                   //VPWR  //VSYNC Polarity ,VSYNC Pulse Width[6:0]
    } else {
        WriteCommand(RA8875_PCSR, 0x82);               // PDAT on PCLK falling edge, PCLK = 4 x System Clock
        wait_ms(1);

        // Horizontal Settings
        screenwidth = width;
        WriteCommand(RA8875_HDWR, width/8 - 1);            //HDWR//Horizontal Display Width Setting Bit[6:0]
        WriteCommand(RA8875_HNDFTR, 0x02);                   //HNDFCR//Horizontal Non-Display Period fine tune Bit[3:0]
        WriteCommand(RA8875_HNDR, 0x03);                   //HNDR//Horizontal Non-Display Period Bit[4:0]
        WriteCommand(RA8875_HSTR, 0x01);                   //HSTR//HSYNC Start Position[4:0]
        WriteCommand(RA8875_HPWR, 0x03);                   //HPWR//HSYNC Polarity ,The period width of HSYNC.

        // Vertical Settings
        screenheight = height;
        WriteCommand(RA8875_VDHR0, (height-1)&0xFF);        //VDHR0 //Vertical Display Height Bit [7:0]
        WriteCommand(RA8875_VDHR1, (height-1)>>8);          //VDHR1 //Vertical Display Height Bit [8]
        WriteCommand(RA8875_VNDR0, 0x0F);                   //VNDR0 //Vertical Non-Display Period Bit [7:0]
        WriteCommand(RA8875_VNDR1, 0x00);                   //VNDR1 //Vertical Non-Display Period Bit [8]
        WriteCommand(RA8875_VSTR0, 0x0e);                   //VSTR0 //VSYNC Start Position[7:0]
        WriteCommand(RA8875_VSTR1, 0x06);                   //VSTR1 //VSYNC Start Position[8]
        WriteCommand(RA8875_VPWR, 0x01);                   //VPWR  //VSYNC Polarity ,VSYNC Pulse Width[6:0]
    }

    portraitmode = false;

    if (width >= 800 && height >= 480 && color_bpp > 8) {
        WriteCommand(RA8875_DPCR, 0x00);               // DPCR - 1-layer mode when the resolution is too high
    } else {
        WriteCommand(RA8875_DPCR, 0x80);               // DPCR - 2-layer mode
    }

    // Set display image to Blue on Black as default
    window(0,0, width, height);             // Initialize to full screen
    SetTextCursorControl();
    foreground(Blue);
    background(Black);
    cls(3);

    Power(poweron);
    Backlight_u8(poweron);
    if (keypadon)
        KeypadInit();
    if (touchscreenon) {
        if (useTouchPanel == TP_NONE)
            useTouchPanel = TP_RES;
        TouchPanelInit();
    }
#ifdef PERF_METRICS
    performance.start();
    ClearPerformance();
#endif
    return noerror;
}


RetCode_t RA8875::Reset(void)
{
    RetCode_t ret;
    
    #if 0
    if (res != (PinName)NC) {
        res = 0;                            // Active low - assert reset
        wait_ms(2);                         // must be > 1024 clock periods. (@25 MHz, this is 40.96 usec)
        res = 1;                            // de-assert reset
    }
    #endif
    ret = WriteCommand(RA8875_PWRR, 0x01);         // Apply Display Off, Reset
    wait_ms(2);                             // no idea if I need to wait, or how long
    if (ret == noerror) {
        ret = WriteCommand(RA8875_PWRR, 0x00);     // Display off, Remove reset
        wait_ms(2);                         // no idea if I need to wait, or how long
    }
    return ret;
}


const char * RA8875::GetErrorMessage(RetCode_t code)
{
    if (code >= LastErrCode)
        code = bad_parameter;
    return ErrMessages[code];
}


uint16_t RA8875::GetDrawingLayer(void)
{
    return (ReadCommand(0x41) & 0x01);
}


RetCode_t RA8875::SelectDrawingLayer(uint16_t layer, uint16_t * prevLayer)
{
    unsigned char mwcr1 = ReadCommand(0x41); // retain all but the currently selected layer
    
    if (prevLayer)
        *prevLayer = mwcr1 & 1;
    
    mwcr1 &= ~0x01; // remove the current layer
    if (screenwidth >= 800 && screenheight >= 480 && screenbpp > 8) {
        layer = 0;
    } else if (layer > 1) {
        layer = 0;
    }
    return WriteCommand(RA8875_MWCR1, mwcr1 | layer);
}


RA8875::LayerMode_T RA8875::GetLayerMode(void)
{
    return (LayerMode_T)(ReadCommand(0x52) & 0x7);
}


RetCode_t RA8875::SetLayerMode(LayerMode_T mode)
{
    unsigned char ltpr0 = ReadCommand(0x52) & ~0x7; // retain all but the display layer mode
    
    if (mode <= (LayerMode_T)6) {
        WriteCommand(RA8875_LTPR0, ltpr0 | (mode & 0x7));
        return noerror;
    } else {
        return bad_parameter;
    }
}


RetCode_t RA8875::SetLayerTransparency(uint8_t layer1, uint8_t layer2)
{
    if (layer1 > 8)
        layer1 = 8;
    if (layer2 > 8)
        layer2 = 8;
    WriteCommand(RA8875_LTPR1, ((layer2 & 0xF) << 4) | (layer1 & 0xF));
    return noerror;
}


RetCode_t RA8875::SetBackgroundTransparencyColor(color_t color)
{
    return _writeColorTrio(0x67, color);
}


color_t RA8875::GetBackgroundTransparencyColor(void)
{
    RGBQUAD q;
    
    q.rgbRed = ReadCommand(0x67);
    q.rgbGreen = ReadCommand(0x68);
    q.rgbBlue = ReadCommand(0x69);
    return RGBQuadToRGB16(&q, 0);
}


RetCode_t RA8875::KeypadInit(bool scanEnable, bool longDetect, uint8_t sampleTime, uint8_t scanFrequency,
                             uint8_t longTimeAdjustment, bool interruptEnable, bool wakeupEnable)
{
    uint8_t value = 0;

    if (sampleTime > 3 || scanFrequency > 7 || longTimeAdjustment  > 3)
        return bad_parameter;
    value |= (scanEnable) ? 0x80 : 0x00;
    value |= (longDetect) ? 0x40 : 0x00;
    value |= (sampleTime & 0x03) << 4;
    value |= (scanFrequency & 0x07);
    WriteCommand(RA8875_KSCR1, value);   // KSCR1 - Enable Key Scan (and ignore possibility of an error)

    value = 0;
    value |= (wakeupEnable) ? 0x80 : 0x00;
    value |= (longTimeAdjustment & 0x03) << 2;
    WriteCommand(RA8875_KSCR2, value);  // KSCR2 - (and ignore possibility of an error)

    value = ReadCommand(0xF0);          // (and ignore possibility of an error)
    value &= ~0x10;
    value |= (interruptEnable) ? 0x10 : 0x00;
    return WriteCommand(RA8875_INTC1, value);   // INT
}


RetCode_t RA8875::SetKeyMap(const uint8_t * CodeList)
{
    pKeyMap = CodeList;
    return noerror;
}


bool RA8875::readable(void)
{
    return (ReadCommand(0xF1) & 0x10);  // check KS status - true if kbhit
}


uint8_t RA8875::getc(void)
{
    //#define GETC_DEV      // for development
#ifdef GETC_DEV
    uint8_t keyCode1, keyCode2;
#endif
    uint8_t keyCode3;
    static uint8_t count = 0;
    uint8_t col, row;
    uint8_t key;
    
    while (!readable()) {
        wait_us(POLLWAITuSec);
        // COUNTIDLETIME(POLLWAITuSec);     // As it is voluntary to call the getc and pend. Don't tally it.
        if (idle_callback) {
            if (external_abort == (*idle_callback)(getc_wait, 0)) {
                return 0;
            }
        }
    }
    // read the key press number
    uint8_t keyNumReg = ReadCommand(0xC1) & 0x03;
    count++;
    switch (keyNumReg) {
        case 0x01:      // one key
            keyCode3 = ReadCommand(0xC2);
#ifdef GETC_DEV
            keyCode2 = 0;
            keyCode1 = 0;
#endif
            break;
        case 0x02:      // two keys
            keyCode3 = ReadCommand(0xC3);
#ifdef GETC_DEV
            keyCode2 = ReadCommand(0xC2);
            keyCode1 = 0;
#endif
            break;
        case 0x03:      // three keys
            keyCode3 = ReadCommand(0xC4);
#ifdef GETC_DEV
            keyCode2 = ReadCommand(0xC3);
            keyCode1 = ReadCommand(0xC2);
#endif
            break;
        default:         // no keys (key released)
            keyCode3 = 0xFF;
#ifdef GETC_DEV
            keyCode2 = 0;
            keyCode1 = 0;
#endif
            break;
    }
    if (keyCode3 == 0xFF)
        key = pKeyMap[0];                    // Key value 0
    else {
        row = (keyCode3 >> 4) & 0x03;
        col = (keyCode3 &  7);
        key = row * 5 + col + 1;    // Keys value 1 - 20
        if (key > 21) {
            key = 21;
        }
        key = pKeyMap[key];
        key |= (keyCode3 & 0x80);   // combine the key held flag
    }
#if GETC_DEV // for Development only
    SetTextCursor(0, 20);
    printf("   Reg: %02x\r\n", keyNumReg);
    printf("  key1: %02x\r\n", keyCode1);
    printf("  key2: %02x\r\n", keyCode2);
    printf("  key3: %02x\r\n", keyCode3);
    printf(" count: %02X\r\n", count);
    printf("   key: %02X\r\n", key);
#endif
    WriteCommand(RA8875_INTC2, 0x10);       // Clear KS status
    return key;
}


#ifdef PERF_METRICS
void RA8875::ClearPerformance()
{
    int i;
    
    for (i=0; i<METRICCOUNT; i++)
        metrics[i] = 0;
    idletime_usec = 0;
    for (i=0; i<256; i++)
        commandsUsed[i] = 0;
}


void RA8875::RegisterPerformance(method_e method)
{
    unsigned long elapsed = performance.read_us();

    if (method < METRICCOUNT && elapsed > metrics[method])
        metrics[method] = elapsed;
}


void RA8875::CountIdleTime(uint32_t t)
{
    idletime_usec += t;
}


void RA8875::ReportPerformance(Serial & pc)
{
    int i;
    
    pc.printf("\r\nPerformance Metrics\r\n");
    for (i=0; i<METRICCOUNT; i++) {
        pc.printf("%10d uS %s\r\n", metrics[i], metricsName[i]);
    }
    pc.printf("%10d uS Idle time polling display for ready.\r\n", idletime_usec);
    for (i=0; i<256; i++) {
        if (commandsUsed[i])
            pc.printf("Command %02X used %5d times.\r\n", i, commandsUsed[i]);
    }
}
#endif


bool RA8875::Intersect(rect_t rect, point_t p)
{
    if (p.x >= min(rect.p1.x, rect.p2.x) && p.x <= max(rect.p1.x, rect.p2.x)
    && p.y >= min(rect.p1.y, rect.p2.y) && p.y <= max(rect.p1.y, rect.p2.y))
        return true;
    else
        return false;
}


bool RA8875::Intersect(rect_t rect1, rect_t rect2)
{
#if 1
    // If one rectangle is on left side of other
    if (max(rect1.p1.x,rect1.p2.x) < min(rect2.p1.x,rect2.p2.x)
        || min(rect1.p1.x, rect1.p2.x) > max(rect2.p1.x, rect2.p2.x))
        return false;
    // If one rectangle is above other
    if (max(rect1.p1.y, rect1.p2.y) < min(rect2.p1.y, rect2.p2.y)
        || min(rect1.p1.y, rect1.p2.y) > max(rect2.p1.y, rect2.p2.y))
        return false;
    return true;            // all that's left is they overlap
#else
    point_t bl, tr;
    bl.x = rect2.p1.x;
    bl.y = rect2.p2.y;
    tr.x = rect2.p2.x;
    tr.y = rect2.p1.y;
    if (Intersect(rect1, rect2.p1) || Intersect(rect1, rect2.p2)
        || Intersect(rect1, bl) || Intersect(rect1, tr))
        return true;
    else
        return false;
#endif
}


bool RA8875::Intersect(rect_t * pRect1, const rect_t * pRect2)
{
    if (Intersect(*pRect1, *pRect2)) {
        rect_t iSect;
        
        iSect.p1.x = max(min(pRect1->p1.x,pRect1->p2.x),min(pRect2->p1.x,pRect2->p2.x));
        iSect.p1.y = max(min(pRect1->p1.y,pRect1->p2.y),min(pRect2->p1.y,pRect2->p2.y));
        iSect.p2.x = min(max(pRect1->p1.x,pRect1->p2.x),max(pRect2->p1.x,pRect2->p2.x));
        iSect.p2.y = min(max(pRect1->p1.y,pRect1->p2.y),max(pRect2->p1.y,pRect2->p2.y));
        *pRect1 = iSect;
        return true;
    } else {
        return false;
    }
}


RetCode_t RA8875::WriteCommandW(uint8_t command, uint16_t data)
{
    WriteCommand(command, data & 0xFF);
    WriteCommand(command+1, data >> 8);
    return noerror;
}


RetCode_t RA8875::WriteCommand(unsigned char command, unsigned int data)
{
#ifdef PERF_METRICS
    if (commandsUsed[command] < 65535)
        commandsUsed[command]++;
#endif
    _select(true);
    _spiwrite(0x80);            // RS:1 (Cmd/Status), RW:0 (Write)
    _spiwrite(command);
    if (data <= 0xFF) {   // only if in the valid range
        _spiwrite(0x00);
        _spiwrite(data);
    }
    _select(false);
    return noerror;
}


RetCode_t RA8875::WriteDataW(uint16_t data)
{
    _select(true);
    _spiwrite(0x00);            // RS:0 (Data), RW:0 (Write)
    _spiwrite(data & 0xFF);
    _spiwrite(data >> 8);
    _select(false);
    return noerror;
}


RetCode_t RA8875::WriteData(unsigned char data)
{
    _select(true);
    _spiwrite(0x00);            // RS:0 (Data), RW:0 (Write)
    _spiwrite(data);
    _select(false);
    return noerror;
}


unsigned char RA8875::ReadCommand(unsigned char command)
{
    WriteCommand(command);
    return ReadData();
}

uint16_t RA8875::ReadCommandW(unsigned char command)
{
    WriteCommand(command);
    return ReadDataW();
}

unsigned char RA8875::ReadData(void)
{
    unsigned char data;

    _select(true);
    _spiwrite(0x40);            // RS:0 (Data), RW:1 (Read)
    data = _spiread();
    _select(false);
    return data;
}


uint16_t RA8875::ReadDataW(void)
{
    uint16_t data;

    _select(true);
    _spiwrite(0x40);            // RS:0 (Data), RW:1 (Read)
    data  = _spiread();
    data |= (_spiread() << 8);
    _select(false);
    return data;
}


unsigned char RA8875::ReadStatus(void)
{
    unsigned char data;

    _select(true);
    _spiwrite(0xC0);            // RS:1 (Cmd/Status), RW:1 (Read) (Read STSR)
    data = _spiread();
    _select(false);
    return data;
}


/// @todo add a timeout and return false, but how long
/// to wait since some operations can be very long.
bool RA8875::_WaitWhileBusy(uint8_t mask)
{
    int i = 20000/POLLWAITuSec; // 20 msec max

    while (i-- && ReadStatus() & mask) {
        wait_us(POLLWAITuSec);
        COUNTIDLETIME(POLLWAITuSec);
        if (idle_callback) {
            if (external_abort == (*idle_callback)(status_wait, 0)) {
                return false;
            }
        }
    }
    if (i)
        return true;
    else
        return false;
}


/// @todo add a timeout and return false, but how long
/// to wait since some operations can be very long.
bool RA8875::_WaitWhileReg(uint8_t reg, uint8_t mask)
{
    int i = 20000/POLLWAITuSec; // 20 msec max

    while (i-- && ReadCommand(reg) & mask) {
        wait_us(POLLWAITuSec);
        COUNTIDLETIME(POLLWAITuSec);
        if (idle_callback) {
            if (external_abort == (*idle_callback)(command_wait, 0)) {
                return false;
            }
        }
    }
    if (i)
        return true;
    else
        return false;
}

// RRRR RGGG GGGB BBBB
// 4321 0543 2104 3210
//           RRRG GGBB
//           2102 1010
uint8_t RA8875::_cvt16to8(color_t c16)
{
    return ((c16 >> 8) & 0xE0)
        | ((c16 >> 6) & 0x1C)
        | ((c16 >> 3) & 0x03);
}

//           RRRG GGBB
//           2102 1010
// RRRR RGGG GGGB BBBB
// 2101 0543 2104 3210
color_t RA8875::_cvt8to16(uint8_t c8)
{
    color_t c16;
    color_t temp = (color_t)c8;
    
    c16 = ((temp & 0xE0) << 8)
        | ((temp & 0xC0) << 5)
        | ((temp & 0x1C) << 6)
        | ((temp & 0x1C) << 3)
        | ((temp & 0x03) << 3)
        | ((temp & 0x03) << 1)
        | ((temp & 0x03) >> 1);
    c16 = (c16 << 8) | (c16 >> 8);
    return c16;
}

RetCode_t RA8875::_writeColorTrio(uint8_t regAddr, color_t color)
{
    RetCode_t rt = noerror;
    
    if (screenbpp == 16) {
        WriteCommand(regAddr+0, (color>>11));                  // BGCR0
        WriteCommand(regAddr+1, (unsigned char)(color>>5));    // BGCR1
        rt = WriteCommand(regAddr+2, (unsigned char)(color));       // BGCR2
    } else {
        uint8_t r, g, b;
        
        // RRRR RGGG GGGB BBBB      RGB
        // RRR   GGG    B B
        r = (uint8_t)((color) >> 13);
        g = (uint8_t)((color) >> 8);
        b = (uint8_t)((color) >> 3);
        WriteCommand(regAddr+0, r);  // BGCR0
        WriteCommand(regAddr+1, g);  // BGCR1
        rt = WriteCommand(regAddr+2, b);  // BGCR2
    }
    return rt;
}

color_t RA8875::_readColorTrio(uint8_t regAddr)
{
    color_t color;
    uint8_t r, g, b;
    
    r = ReadCommand(regAddr+0);
    g = ReadCommand(regAddr+1);
    b = ReadCommand(regAddr+2);
    if (screenbpp == 16) {
        // 000R RRRR 00GG GGGG 000B BBBB
        // RRRR RGGG GGGB BBBB
        color  = (r & 0x1F) << 11;
        color |= (g & 0x3F) << 5;
        color |= (b & 0x1F);
    } else {
        // RRRG GGBB
        // RRRR RGGG GGGB BBBB
        color  = (r & 0x07) << 13;
        color |= (g & 0x07) << 8;
        color |= (b & 0x03) << 3;
    }
    return color;
}


dim_t RA8875::fontwidth(void)
{
    if (font == NULL)
        return (((ReadCommand(0x22) >> 2) & 0x3) + 1) * 8;
    else
        return extFontWidth;
}


dim_t RA8875::fontheight(void)
{
    if (font == NULL)
        return (((ReadCommand(0x22) >> 0) & 0x3) + 1) * 16;
    else
        return extFontHeight;
}


RetCode_t RA8875::locate(textloc_t column, textloc_t row)
{
    return SetTextCursor(column * fontwidth(), row * fontheight());
}


int RA8875::columns(void)
{
    return screenwidth / fontwidth();
}


int RA8875::rows(void)
{
    return screenheight / fontheight();
}


dim_t RA8875::width(void)
{
    if (portraitmode)
        return screenheight;
    else
        return screenwidth;
}


dim_t RA8875::height(void)
{
    if (portraitmode)
        return screenwidth;
    else
        return screenheight;
}


dim_t RA8875::color_bpp(void)
{
    return screenbpp;
}

RetCode_t RA8875::SetTextCursor(point_t p)
{
    return SetTextCursor(p.x, p.y);
}

RetCode_t RA8875::SetTextCursor(loc_t x, loc_t y)
{
    INFO("SetTextCursor(%d, %d)", x, y);
    cursor_x = x;     // set these values for non-internal fonts
    cursor_y = y;
    WriteCommandW(RA8875_FCURXL, x);
    WriteCommandW(RA8875_FCURYL, y);
    return noerror;
}

point_t RA8875::GetTextCursor(void)
{
    point_t p;
    
    p.x = GetTextCursor_X();
    p.y = GetTextCursor_Y();
    return p;
}

loc_t RA8875::GetTextCursor_Y(void)
{
    loc_t y;
    
    if (font == NULL)
        y = ReadCommand(0x2C) | (ReadCommand(0x2D) << 8);
    else
        y = cursor_y;
    INFO("GetTextCursor_Y = %d", y);
    return y;
}


loc_t RA8875::GetTextCursor_X(void)
{
    loc_t x;
    
    if (font == NULL)
        x = ReadCommand(0x2A) | (ReadCommand(0x2B) << 8);
    else
        x = cursor_x;
    INFO("GetTextCursor_X = %d", x);
    return x;
}


RetCode_t RA8875::SetTextCursorControl(cursor_t cursor, bool blink)
{
    unsigned char mwcr0 = ReadCommand(0x40) & 0x0F; // retain direction, auto-increase
    unsigned char mwcr1 = ReadCommand(0x41) & 0x01; // retain currently selected layer
    unsigned char horz = 0;
    unsigned char vert = 0;

    mwcr0 |= 0x80;                  // text mode
    if (cursor != NOCURSOR)
        mwcr0 |= 0x40;              // visible
    if (blink)
        mwcr0 |= 0x20;              // blink
    WriteCommand(RA8875_MWCR0, mwcr0);      // configure the cursor
    WriteCommand(RA8875_MWCR1, mwcr1);      // close the graphics cursor
    WriteCommand(RA8875_BTCR, 0x1f);       // The cursor flashing cycle
    switch (cursor) {
        case IBEAM:
            horz = 0x01;
            vert = 0x1F;
            break;
        case UNDER:
            horz = 0x07;
            vert = 0x01;
            break;
        case BLOCK:
            horz = 0x07;
            vert = 0x1F;
            break;
        case NOCURSOR:
        default:
            break;
    }
    WriteCommand(RA8875_CURHS, horz);       // The cursor size horz
    WriteCommand(RA8875_CURVS, vert);       // The cursor size vert
    return noerror;
}


RetCode_t RA8875::SetTextFont(RA8875::font_t font)
{
    if (/*font >= RA8875::ISO8859_1 && */ font <= RA8875::ISO8859_4) {
        WriteCommand(RA8875_FNCR0, (unsigned int)(font));
        return noerror;
    } else {
        return bad_parameter;
    }
}


RetCode_t RA8875::SetOrientation(RA8875::orientation_t angle)
{
    uint8_t fncr1Val = ReadCommand(0x22);
    uint8_t dpcrVal = ReadCommand(0x20);
    
    fncr1Val &= ~0x10;      // remove the old direction bit
    dpcrVal &= ~0x0C;       // remove the old scan direction bits
    switch (angle) {
        case RA8875::normal:
            //fncr1Val |= 0x10;
            //dpcrVal |= 0x00;
            portraitmode = false;
            break;
        case RA8875::rotate_90:
            fncr1Val |= 0x10;
            dpcrVal |= 0x08;
            portraitmode = true;
            break;
        case RA8875::rotate_180:
            //fncr1Val |= 0x00;
            dpcrVal |= 0x0C;
            portraitmode = false;
            break;
        case RA8875::rotate_270:
            fncr1Val |= 0x10;
            dpcrVal |= 0x04;
            portraitmode = true;
            break;
        default:
            return bad_parameter;
    }
    INFO("Orientation: %d, %d", angle, portraitmode);
    WriteCommand(RA8875_FNCR1, fncr1Val);
    return WriteCommand(RA8875_DPCR, dpcrVal);
}


RetCode_t RA8875::SetTextFontControl(fill_t fillit,
                                     RA8875::HorizontalScale hScale,
                                     RA8875::VerticalScale vScale,
                                     RA8875::alignment_t alignment)
{
    if (hScale >= 1 && hScale <= 4 &&
            vScale >= 1 && vScale <= 4) {
        uint8_t fncr1Val = ReadCommand(0x22);
        
        fncr1Val &= ~0x10;      // do not disturb the rotate flag
        if (alignment == align_full)
            fncr1Val |= 0x80;
        if (fillit == NOFILL)
            fncr1Val |= 0x40;
        fncr1Val |= ((hScale - 1) << 2);
        fncr1Val |= ((vScale - 1) << 0);
        return WriteCommand(RA8875_FNCR1, fncr1Val);
    } else {
        return bad_parameter;
    }
}


RetCode_t RA8875::SetTextFontSize(RA8875::HorizontalScale hScale, RA8875::VerticalScale vScale)
{
    unsigned char reg = ReadCommand(0x22);

    if (vScale == -1)
        vScale = hScale;
    if (hScale >= 1 && hScale <= 4 && vScale >= 1 && vScale <= 4) {
        fontScaleX = hScale;    // save for use with a Soft Font
        fontScaleY = vScale;
        reg &= 0xF0;    // keep the high nibble as is.
        reg |= ((hScale - 1) << 2);
        reg |= ((vScale - 1) << 0);
        WriteCommand(RA8875_FNCR1, reg);
        return noerror;
    } else {
        return bad_parameter;
    }
}

RetCode_t RA8875::GetTextFontSize(RA8875::HorizontalScale * hScale, RA8875::VerticalScale * vScale)
{
    unsigned char reg = ReadCommand(0x22);

    if (hScale)
        *hScale = 1 + (reg >> 2) & 0x03;
    if (vScale)
        *vScale = 1 + reg & 0x03;
    return noerror;
}

int RA8875::_putc(int c)
{
    if (font == NULL) {
        return _internal_putc(c);
    } else {
        return _external_putc(c);
    }
}



// Questions to ponder -
// - if we choose to wrap to the next line, because the character won't fit on the current line,
//      should it erase the space to the width of the screen (in case there is leftover junk there)?
// - it currently wraps from the bottom of the screen back to the top. I have pondered what
//      it might take to scroll the screen - but haven't thought hard enough about it.
//
int RA8875::_external_putc(int c)
{
    if (c) {
        if (c == '\r') {
            cursor_x = windowrect.p1.x;
        } else if (c == '\n') {
            cursor_y += extFontHeight;
        } else {
            dim_t charWidth, charHeight;
            const uint8_t * charRecord;
            
            charRecord = getCharMetrics(c, &charWidth, &charHeight);
            //int advance = charwidth(c);
            INFO("(%d,%d) - (%d,%d):(%d,%d), charWidth: %d '%c", cursor_x, cursor_y, 
                windowrect.p1.x, windowrect.p1.y, windowrect.p2.x, windowrect.p2.y,
                charWidth, c);
            if (charRecord) {
                //cursor_x += advance;
                if (cursor_x + charWidth >= windowrect.p2.x) {
                    cursor_x = windowrect.p1.x;
                    cursor_y += charHeight;
                }
                if (cursor_y + charHeight >= windowrect.p2.y) {
                    cursor_y = windowrect.p1.y;               // @todo Should it scroll?
                }
                (void)character(cursor_x, cursor_y, c);
                cursor_x += charWidth * fontScaleX;
            }
        }
    }
    return c;
}


int RA8875::_internal_putc(int c)
{
    if (c) {
        unsigned char mwcr0;

        mwcr0 = ReadCommand(0x40);
        if ((mwcr0 & 0x80) == 0x00) {
            WriteCommand(RA8875_MWCR0, 0x80 | mwcr0);    // Put in Text mode if not already
        }
        if (c == '\r') {
            loc_t x;
            x = ReadCommand(0x30) | (ReadCommand(0x31) << 8);   // Left edge of active window
            WriteCommandW(RA8875_FCURXL, x);
        } else if (c == '\n') {
            loc_t y;
            y = ReadCommand(0x2C) | (ReadCommand(0x2D) << 8);   // current y location
            y += fontheight();
            if (y >= height())               // @TODO after bottom of active window, then scroll window?
                y = 0;
            WriteCommandW(RA8875_FCURYL, y);
        } else {
            WriteCommand(RA8875_MRWC);                 // RA8875 Internal Fonts
            _select(true);
            WriteData(c);
            _WaitWhileBusy(0x80);
            _select(false);
        }
    }
    return c;
}


RetCode_t RA8875::_StartGraphicsStream(void)
{
    WriteCommand(RA8875_MWCR0,0x00);    // Graphics write mode
    WriteCommand(RA8875_MRWC);         // Prepare for streaming data
    return noerror;
}


RetCode_t RA8875::_EndGraphicsStream(void)
{
    return noerror;
}


RetCode_t RA8875::_putp(color_t pixel)
{
    WriteDataW((pixel>>8) | (pixel<<8));
    return noerror;
}


void RA8875::puts(loc_t x, loc_t y, const char * string)
{
    SetTextCursor(x,y);
    puts(string);
}


void RA8875::puts(const char * string)
{
    if (font == NULL) {
        WriteCommand(RA8875_MWCR0,0x80);    // Put in Text mode if internal font
    }
    if (*string != '\0') {
        while (*string) {           // @TODO calling individual _putc is slower... optimizations?
            _putc(*string++);
        }
    }
}


RetCode_t RA8875::SetGraphicsCursor(loc_t x, loc_t y)
{
    WriteCommandW(RA8875_CURH0, x);
    WriteCommandW(RA8875_CURV0, y);
    return noerror;
}

RetCode_t RA8875::SetGraphicsCursor(point_t p)
{
    return SetGraphicsCursor(p.x, p.y);
}

point_t RA8875::GetGraphicsCursor(void)
{
    point_t p;
    
    p.x = ReadCommandW(0x46);
    p.y = ReadCommandW(0x48);
    return p;
}

RetCode_t RA8875::SetGraphicsCursorRead(loc_t x, loc_t y)
{
    WriteCommandW(RA8875_RCURH0, x);
    WriteCommandW(RA8875_RCURV0, y);
    return noerror;
}

RetCode_t RA8875::window(rect_t r)
{
    return window(r.p1.x, r.p1.y, r.p2.x + 1 - r.p1.x, r.p2.y + 1 - r.p1.y);
}

RetCode_t RA8875::window(loc_t x, loc_t y, dim_t width, dim_t height)
{
    INFO("window(%d,%d,%d,%d)", x, y, width, height);
    if (width == (dim_t)-1)
        width = screenwidth - x;
    if (height == (dim_t)-1)
        height = screenheight - y;
    windowrect.p1.x = x;
    windowrect.p1.y = y;
    windowrect.p2.x = x + width - 1;
    windowrect.p2.y = y + height - 1;
    GraphicsDisplay::window(x,y, width,height);
    WriteCommandW(RA8875_HSAW0, x);
    WriteCommandW(RA8875_VSAW0, y);
    WriteCommandW(RA8875_HEAW0, (x+width-1));
    WriteCommandW(RA8875_VEAW0, (y+height-1));
    //SetTextCursor(x,y);
    //SetGraphicsCursor(x,y);
    return noerror;
}


RetCode_t RA8875::cls(uint16_t layers)
{
    RetCode_t ret;

    PERFORMANCE_RESET;
    if (layers == 0) {
        ret = clsw(FULLWINDOW);
    } else if (layers > 3) {
        ret = bad_parameter;
    } else {
        uint16_t prevLayer = GetDrawingLayer();
        if (layers & 1) {
            SelectDrawingLayer(0);
            clsw(FULLWINDOW);
        }
        if (layers & 2) {
            SelectDrawingLayer(1);
            clsw(FULLWINDOW);
        }
        SelectDrawingLayer(prevLayer);
    }
    ret = SetTextCursor(0,0);
    ret = locate(0,0);
    REGISTERPERFORMANCE(PRF_CLS);
    return ret;
}


RetCode_t RA8875::clsw(RA8875::Region_t region)
{
    PERFORMANCE_RESET;
    WriteCommand(RA8875_MCLR, (region == ACTIVEWINDOW) ? 0xC0 : 0x80);
    if (!_WaitWhileReg(0x8E, 0x80)) {
        REGISTERPERFORMANCE(PRF_CLS);
        return external_abort;
    }
    REGISTERPERFORMANCE(PRF_CLS);
    return noerror;
}


RetCode_t RA8875::pixel(point_t p, color_t color)
{
    return pixel(p.x, p.y, color);
}

RetCode_t RA8875::pixel(point_t p)
{
    return pixel(p.x, p.y);
}

RetCode_t RA8875::pixel(loc_t x, loc_t y, color_t color)
{
    RetCode_t ret;

    PERFORMANCE_RESET;
    ret = pixelStream(&color, 1, x,y);
    REGISTERPERFORMANCE(PRF_DRAWPIXEL);
    return ret;
}


RetCode_t RA8875::pixel(loc_t x, loc_t y)
{
    RetCode_t ret;

    PERFORMANCE_RESET;
    color_t color = GetForeColor();
    ret = pixelStream(&color, 1, x, y);
    REGISTERPERFORMANCE(PRF_DRAWPIXEL);
    return ret;
}


RetCode_t RA8875::pixelStream(color_t * p, uint32_t count, loc_t x, loc_t y)
{
    PERFORMANCE_RESET;
    SetGraphicsCursor(x, y);
    _StartGraphicsStream();
    _select(true);
    _spiwrite(0x00);         // Cmd: write data
    while (count--) {
        if (screenbpp == 16) {
            _spiwrite(*p >> 8);
            _spiwrite(*p & 0xFF);
        } else {
            _spiwrite(_cvt16to8(*p));
        }
        p++;
    }
    _select(false);
    _EndGraphicsStream();
    REGISTERPERFORMANCE(PRF_PIXELSTREAM);
    return(noerror);
}

// With a font scale X = 1, a pixel stream is "abcdefg..."
// With a font scale X = 2, a pixel stream is "aabbccddeeffgg..."
// With a font scale Y = 2, a pixel stream is "abcdefg..."
//                                            "abcdefg..."
//
RetCode_t RA8875::booleanStream(loc_t x, loc_t y, dim_t w, dim_t h, const uint8_t * boolStream) 
{
    PERFORMANCE_RESET;
    const uint8_t * rowStream;
    rect_t restore = windowrect;
    window(x, y, w * fontScaleX, h * fontScaleY);       // Scale from font scale factors
    SetGraphicsCursor(x, y);
    _StartGraphicsStream();
    _select(true);
    _spiwrite(0x00);         // Cmd: write data
    while (h--) {
        for (int dy=0; dy<fontScaleY; dy++) {           // Vertical Font Scale Factor
            uint8_t pixels = w;
            uint8_t bitmask = 0x01;
            rowStream = boolStream;        
            while (pixels) {
                uint8_t byte = *rowStream;
                //INFO("byte, mask: %02X, %02X", byte, bitmask);
                color_t c = (byte & bitmask) ? _foreground : _background;
                
                for (int dx=0; dx<fontScaleX; dx++) {   // Horizontal Font Scale Factor
                    if (screenbpp == 16) {
                        _spiwrite(c >> 8);
                        _spiwrite(c & 0xFF);
                    } else {
                        _spiwrite(_cvt16to8(c));
                    }
                }
                bitmask <<= 1;
                if (pixels > 1 && bitmask == 0) {
                    bitmask = 0x01;
                    rowStream++;
                }
                pixels--;
            }
        }
        boolStream += (rowStream - boolStream + 1);
    }
    _select(false);
    _EndGraphicsStream();
    window(restore);
    REGISTERPERFORMANCE(PRF_BOOLSTREAM);
    return(noerror);
}

color_t RA8875::getPixel(loc_t x, loc_t y)
{
    color_t pixel;

    PERFORMANCE_RESET;
    WriteCommand(RA8875_MWCR0,0x00);    // Graphics write mode
    SetGraphicsCursorRead(x, y);
    WriteCommand(RA8875_MRWC);
    _select(true);
    _spiwrite(0x40);         // Cmd: read data
    _spiwrite(0x00);         // dummy read
    if (screenbpp == 16) {
        pixel  = _spiread();
        pixel |= (_spiread() << 8);
    } else {
        pixel = _cvt8to16(_spiread());
    }
    _select(false);
    REGISTERPERFORMANCE(PRF_READPIXEL);
    return pixel;
}


RetCode_t RA8875::getPixelStream(color_t * p, uint32_t count, loc_t x, loc_t y)
{
    color_t pixel;
    RetCode_t ret = noerror;

    PERFORMANCE_RESET;
    ret = WriteCommand(RA8875_MWCR0,0x00);    // Graphics write mode
    ret = SetGraphicsCursorRead(x, y);
    ret = WriteCommand(RA8875_MRWC);
    _select(true);
    _spiwrite(0x40);         // Cmd: read data
    _spiwrite(0x00);         // dummy read
    if (screenbpp == 16)
        _spiwrite(0x00);     // dummy read is only necessary when in 16-bit mode
    while (count--) {
        if (screenbpp == 16) {
            pixel  = _spiread();
            pixel |= (_spiread() << 8);
        } else {
            pixel = _cvt8to16(_spiread());
        }
        *p++ = pixel;
    }
    _select(false);
    REGISTERPERFORMANCE(PRF_READPIXELSTREAM);
    return ret;
}


RetCode_t RA8875::line(point_t p1, point_t p2)
{
    return line(p1.x, p1.y, p2.x, p2.y);
}


RetCode_t RA8875::line(point_t p1, point_t p2, color_t color)
{
    return line(p1.x, p1.y, p2.x, p2.y, color);
}


RetCode_t RA8875::line(loc_t x1, loc_t y1, loc_t x2, loc_t y2, color_t color)
{
    foreground(color);
    return line(x1,y1,x2,y2);
}


RetCode_t RA8875::line(loc_t x1, loc_t y1, loc_t x2, loc_t y2)
{
    PERFORMANCE_RESET;
    if (x1 == x2 && y1 == y2) {
        pixel(x1, y1);
    } else {
        WriteCommandW(RA8875_DLHSR0, x1);
        WriteCommandW(RA8875_DLVSR0, y1);
        WriteCommandW(RA8875_DLHER0, x2);
        WriteCommandW(RA8875_DLVER0, y2);
        unsigned char drawCmd = 0x00;       // Line
        WriteCommand(RA8875_DCR, drawCmd);
        WriteCommand(RA8875_DCR, 0x80 + drawCmd); // Start drawing.
        if (!_WaitWhileReg(0x90, 0x80)) {
            REGISTERPERFORMANCE(PRF_DRAWLINE);
            return external_abort;
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWLINE);
    return noerror;
}


RetCode_t RA8875::ThickLine(point_t p1, point_t p2, dim_t thickness, color_t color)
{
    if (thickness == 1) {
        line(p1,p2, color);
    } else {
        int dx = abs(p2.x-p1.x), sx = p1.x<p2.x ? 1 : -1;
        int dy = abs(p2.y-p1.y), sy = p1.y<p2.y ? 1 : -1;
        int err = (dx>dy ? dx : -dy)/2, e2;
        
        for (;;) {
            fillcircle(p1.x, p1.y, thickness/2, color);
            if (p1.x==p2.x && p1.y==p2.y) 
                break;
            e2 = err;
            if (e2 >-dx) 
                { err -= dy; p1.x += sx; }
            if (e2 < dy) 
                { err += dx; p1.y += sy; }
        }        
    }
    return noerror;
}


//
// Rectangle functions all mostly helpers to the basic rectangle function
//

RetCode_t RA8875::fillrect(rect_t r, color_t color, fill_t fillit)
{
    return rect(r.p1.x, r.p1.y, r.p2.x, r.p2.y, color, fillit);
}

RetCode_t RA8875::fillrect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                           color_t color, fill_t fillit)
{
    return rect(x1,y1,x2,y2,color,fillit);
}

RetCode_t RA8875::rect(rect_t r, color_t color, fill_t fillit)
{
    return rect(r.p1.x, r.p1.y, r.p2.x, r.p2.y, color, fillit);
}

RetCode_t RA8875::rect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                       color_t color, fill_t fillit)
{
    foreground(color);
    return rect(x1,y1,x2,y2,fillit);
}

RetCode_t RA8875::rect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                       fill_t fillit)
{
    RetCode_t ret = noerror;
    PERFORMANCE_RESET;
    // check for bad_parameter
    if (x1 < 0 || x1 >= screenwidth || x2 < 0 || x2 >= screenwidth 
    || y1 < 0 || y1 >= screenheight || y2 < 0 || y2 >= screenheight) {
        ret = bad_parameter;
    } else {
        if (x1 == x2 && y1 == y2) {
            pixel(x1, y1);
        } else if (x1 == x2) {
            line(x1, y1, x2, y2);
        } else if (y1 == y2) {
            line(x1, y1, x2, y2);
        } else {
            WriteCommandW(RA8875_DLHSR0, x1);
            WriteCommandW(RA8875_DLVSR0, y1);
            WriteCommandW(RA8875_DLHER0, x2);
            WriteCommandW(RA8875_DLVER0, y2);
            unsigned char drawCmd = 0x10;   // Rectangle
            if (fillit == FILL)
                drawCmd |= 0x20;
            WriteCommand(RA8875_DCR, drawCmd);
            ret = WriteCommand(RA8875_DCR, 0x80 + drawCmd); // Start drawing.
            if (!_WaitWhileReg(0x90, 0x80)) {
                REGISTERPERFORMANCE(PRF_DRAWRECTANGLE);
                return external_abort;
            }
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWRECTANGLE);
    return ret;
}


//
// rounded rectangle functions are mostly helpers to the base round rect
//

RetCode_t RA8875::fillroundrect(rect_t r, dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    return roundrect(r.p1.x, r.p1.y, r.p2.x, r.p2.y, radius1, radius2, color, fillit);
}

RetCode_t RA8875::fillroundrect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                                dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    foreground(color);
    return roundrect(x1,y1,x2,y2,radius1,radius2,fillit);
}

RetCode_t RA8875::roundrect(rect_t r, dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    return roundrect(r.p1.x, r.p1.y, r.p2.x, r.p2.y, radius1, radius2, color, fillit);
}

RetCode_t RA8875::roundrect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                            dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    foreground(color);
    return roundrect(x1,y1,x2,y2,radius1,radius2,fillit);
}


RetCode_t RA8875::roundrect(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                            dim_t radius1, dim_t radius2, fill_t fillit)
{
    RetCode_t ret = noerror;

    PERFORMANCE_RESET;
    if (x1 < 0 || x1 >= screenwidth || x2 < 0 || x2 >= screenwidth 
    || y1 < 0 || y1 >= screenheight || y2 < 0 || y2 >= screenheight) {
        ret = bad_parameter;
    } else if (x1 > x2 || y1 > y2 || (radius1 > (x2-x1)/2) || (radius2 > (y2-y1)/2) ) {
        ret = bad_parameter;
    } else if (x1 == x2 && y1 == y2) {
        pixel(x1, y1);
    } else if (x1 == x2) {
        line(x1, y1, x2, y2);
    } else if (y1 == y2) {
        line(x1, y1, x2, y2);
    } else {
        WriteCommandW(RA8875_DLHSR0, x1);
        WriteCommandW(RA8875_DLVSR0, y1);
        WriteCommandW(RA8875_DLHER0, x2);
        WriteCommandW(RA8875_DLVER0, y2);
        WriteCommandW(RA8875_ELLA0, radius1);
        WriteCommandW(RA8875_ELLB0, radius2);
        // Should not need this...
        WriteCommandW(RA8875_DEHR0, 0);
        WriteCommandW(RA8875_DEVR0, 0);
        unsigned char drawCmd = 0x20;       // Rounded Rectangle
        if (fillit == FILL)
            drawCmd |= 0x40;
        WriteCommand(RA8875_ELLIPSE, drawCmd);
        WriteCommand(RA8875_ELLIPSE, 0x80 + drawCmd); // Start drawing.
        if (!_WaitWhileReg(0xA0, 0x80)) {
            REGISTERPERFORMANCE(PRF_DRAWROUNDEDRECTANGLE);
            return external_abort;
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWROUNDEDRECTANGLE);
    return ret;
}


//
// triangle functions
//

RetCode_t RA8875::triangle(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                           loc_t x3, loc_t y3, color_t color, fill_t fillit)
{
    RetCode_t ret;

    if (x1 < 0 || x1 >= screenwidth || x2 < 0 || x2 >= screenwidth || x3 < 0 || x3 >= screenwidth
    || y1 < 0 || y1 >= screenheight || y2 < 0 || y2 >= screenheight || y3 < 0 || y3 >= screenheight)
        ret = bad_parameter;
    foreground(color);
    ret = triangle(x1,y1,x2,y2,x3,y3,fillit);
    return ret;
}


RetCode_t RA8875::filltriangle(loc_t x1, loc_t y1, loc_t x2, loc_t y2,
                               loc_t x3, loc_t y3, color_t color, fill_t fillit)
{
    RetCode_t ret;

    foreground(color);
    ret = triangle(x1,y1,x2,y2,x3,y3,fillit);
    return ret;
}


RetCode_t RA8875::triangle(loc_t x1, loc_t y1 ,loc_t x2, loc_t y2,
                           loc_t x3, loc_t y3, fill_t fillit)
{
    RetCode_t ret = noerror;

    PERFORMANCE_RESET;
    if (x1 == x2 && y1 == y2 && x1 == x3 && y1 == y3) {
        pixel(x1, y1);
    } else {
        WriteCommandW(RA8875_DLHSR0, x1);
        WriteCommandW(RA8875_DLVSR0, y1);
        WriteCommandW(RA8875_DLHER0, x2);
        WriteCommandW(RA8875_DLVER0, y2);
        WriteCommandW(RA8875_DTPH0, x3);
        WriteCommandW(RA8875_DTPV0, y3);
        unsigned char drawCmd = 0x01;       // Triangle
        if (fillit == FILL)
            drawCmd |= 0x20;
        WriteCommand(RA8875_DCR, drawCmd);
        WriteCommand(RA8875_DCR, 0x80 + drawCmd); // Start drawing.
        if (!_WaitWhileReg(0x90, 0x80)) {
            REGISTERPERFORMANCE(PRF_DRAWTRIANGLE);
            return external_abort;
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWTRIANGLE);
    return ret;
}


RetCode_t RA8875::circle(point_t p, dim_t radius,
                         color_t color, fill_t fillit)
{
    foreground(color);
    return circle(p.x,p.y,radius,fillit);
}


RetCode_t RA8875::fillcircle(point_t p, dim_t radius,
                             color_t color, fill_t fillit)
{
    foreground(color);
    return circle(p.x,p.y,radius,fillit);
}


RetCode_t RA8875::circle(point_t p, dim_t radius, fill_t fillit)
{
    return circle(p.x,p.y,radius,fillit);
}


RetCode_t RA8875::circle(loc_t x, loc_t y, dim_t radius,
                         color_t color, fill_t fillit)
{
    foreground(color);
    return circle(x,y,radius,fillit);
}


RetCode_t RA8875::fillcircle(loc_t x, loc_t y, dim_t radius,
                             color_t color, fill_t fillit)
{
    foreground(color);
    return circle(x,y,radius,fillit);
}


RetCode_t RA8875::circle(loc_t x, loc_t y, dim_t radius, fill_t fillit)
{
    RetCode_t ret = noerror;

    PERFORMANCE_RESET;
    if (radius <= 0 || (x - radius) < 0 || (x + radius) > screenwidth 
    || (y - radius) < 0 || (y + radius) > screenheight) {
        ret = bad_parameter;
    } else if (radius == 1) {
        pixel(x,y);
    } else {
        WriteCommandW(RA8875_DCHR0, x);
        WriteCommandW(RA8875_DCVR0, y);
        WriteCommand(RA8875_DCRR, radius & 0xFF);
        unsigned char drawCmd = 0x00;       // Circle
        if (fillit == FILL)
            drawCmd |= 0x20;
        WriteCommand(RA8875_DCR, drawCmd);
        WriteCommand(RA8875_DCR, 0x40 + drawCmd); // Start drawing.
        if (!_WaitWhileReg(0x90, 0x40)) {
            REGISTERPERFORMANCE(PRF_DRAWCIRCLE);
            return external_abort;
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWCIRCLE);
    return ret;
}


RetCode_t RA8875::ellipse(loc_t x, loc_t y, dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    foreground(color);
    return ellipse(x,y,radius1,radius2,fillit);
}


RetCode_t RA8875::fillellipse(loc_t x, loc_t y, dim_t radius1, dim_t radius2, color_t color, fill_t fillit)
{
    foreground(color);
    return ellipse(x,y,radius1,radius2,fillit);
}


RetCode_t RA8875::ellipse(loc_t x, loc_t y, dim_t radius1, dim_t radius2, fill_t fillit)
{
    RetCode_t ret = noerror;

    PERFORMANCE_RESET;
    if (radius1 <= 0 || radius2 <= 0 || (x - radius1) < 0 || (x + radius1) > screenwidth 
    || (y - radius2) < 0 || (y + radius2) > screenheight) {
        ret = bad_parameter;
    } else if (radius1 == 1 && radius2 == 1) {
        pixel(x, y);
    } else {
        WriteCommandW(RA8875_DEHR0, x);
        WriteCommandW(RA8875_DEVR0, y);
        WriteCommandW(RA8875_ELLA0, radius1);
        WriteCommandW(RA8875_ELLB0, radius2);
        unsigned char drawCmd = 0x00;   // Ellipse
        if (fillit == FILL)
            drawCmd |= 0x40;
        WriteCommand(RA8875_ELLIPSE, drawCmd);
        WriteCommand(RA8875_ELLIPSE, 0x80 + drawCmd); // Start drawing.
        if (!_WaitWhileReg(0xA0, 0x80)) {
            REGISTERPERFORMANCE(PRF_DRAWELLIPSE);
            return external_abort;
        }
    }
    REGISTERPERFORMANCE(PRF_DRAWELLIPSE);
    return ret;
}


RetCode_t RA8875::frequency(unsigned long Hz, unsigned long Hz2)
{
    spiwritefreq = Hz;
    if (Hz2 != 0)
        spireadfreq = Hz2;
    else
        spireadfreq = Hz/2;
    _setWriteSpeed(true);
    //       __   ___
    // Clock   ___A     Rising edge latched
    //       ___ ____
    // Data  ___X____
    spi.format(8, 3);           // 8 bits and clock to data phase 0
    return noerror;
}

void RA8875::_setWriteSpeed(bool writeSpeed)
{
    if (writeSpeed) {
        spi.frequency(spiwritefreq);
        spiWriteSpeed = true;
    } else {
        spi.frequency(spireadfreq);
        spiWriteSpeed = false;
    }
}



RetCode_t RA8875::BlockMove(uint8_t dstLayer, uint8_t dstDataSelect, point_t dstPoint,
    uint8_t srcLayer, uint8_t srcDataSelect, point_t srcPoint,
    dim_t bte_width, dim_t bte_height,
    uint8_t bte_op_code, uint8_t bte_rop_code)
{
    uint8_t cmd;

    PERFORMANCE_RESET;
    ///@todo range check and error return rather than to secretly fix
    srcPoint.x &= 0x3FF;    // prevent high bits from doing unexpected things
    srcPoint.y &= 0x1FF;
    dstPoint.x &= 0x3FF;
    dstPoint.y &= 0x1FF;
    WriteCommandW(RA8875_HSBE0, srcPoint.x);
    WriteCommandW(RA8875_VSBE0, ((dim_t)(srcLayer & 1) << 15) | srcPoint.y);
    WriteCommandW(RA8875_HDBE0, dstPoint.x);
    WriteCommandW(RA8875_VDBE0, ((dim_t)(dstLayer & 1) << 15) | dstPoint.y);
    WriteCommandW(RA8875_BEWR0, bte_width);
    WriteCommandW(RA8875_BEHR0, bte_height);
    WriteCommand(RA8875_BECR1,  ((bte_rop_code & 0x0F) << 4) | (bte_op_code & 0x0F));
    cmd = ((srcDataSelect & 1) << 6) | ((dstDataSelect & 1) << 5);
    WriteCommand(RA8875_BECR0, 0x80 | cmd);     // enable the BTE
    if (!_WaitWhileBusy(0x40)) {
        REGISTERPERFORMANCE(PRF_BLOCKMOVE);
        return external_abort;
    }
    REGISTERPERFORMANCE(PRF_BLOCKMOVE);
    return noerror;
}


RetCode_t RA8875::Power(bool on)
{
    WriteCommand(RA8875_PWRR, (on) ? 0x80 : 0x00);
    return noerror;
}


RetCode_t RA8875::Backlight_u8(uint8_t brightness)
{
    static bool is_enabled = false;
    
    if (brightness == 0) {
        WriteCommand(RA8875_P1CR); // Disable the PWM
        WriteData(0x00);
        is_enabled = false;
    } else if (!is_enabled) {
        WriteCommand(RA8875_P1CR); // Enable the PWM
        WriteData(0x80);
        WriteCommand(RA8875_P1CR); // Not sure why this is needed, but following the pattern
        WriteData(0x81);    // open PWM (SYS_CLK / 2 as best I can tell)
        is_enabled = true;
    }
    WriteCommand(RA8875_P1DCR, brightness);  // Brightness parameter 0xff-0x00
    return noerror;
}

uint8_t RA8875::GetBacklight_u8(void)
{
    return ReadCommand(0x8b);
}

RetCode_t RA8875::Backlight(float brightness)
{
    unsigned char b;

    if (brightness >= 1.0)
        b = 255;
    else if (brightness <= 0.0)
        b = 0;
    else
        b = (unsigned char)(brightness * 255);
    return Backlight_u8(b);
}

float RA8875::GetBacklight(void)
{
    return (float)(GetBacklight_u8())/255;
}

RetCode_t RA8875::SelectUserFont(const uint8_t * _font)
{
    INFO("Cursor(%d,%d)  %p", cursor_x, cursor_y, _font);
    INFO("Text C(%d,%d)", GetTextCursor_X(), GetTextCursor_Y());
    if (_font) {
        HexDump("Font Memory", _font, 16);
        extFontHeight = _font[6];
        uint32_t totalWidth = 0;
        uint16_t firstChar = _font[3] * 256 + _font[2];
        uint16_t lastChar  = _font[5] * 256 + _font[4];
        uint16_t i;
        
        for (i=firstChar; i<=lastChar; i++) {
            // 8 bytes of preamble to the first level lookup table
            uint16_t offsetToCharLookup = 8 + 4 * (i - firstChar);    // 4-bytes: width(pixels), 16-bit offset from table start, 0
            totalWidth += _font[offsetToCharLookup];
        }
        extFontWidth = totalWidth / (lastChar - firstChar);
        INFO("Font Metrics: Avg W: %2d, H: %2d, First:%d, Last:%d", extFontWidth, extFontHeight, firstChar, lastChar);
    }
    SetTextCursor(GetTextCursor_X(), GetTextCursor_Y());  // soft-font cursor -> hw cursor
    font = _font;
    return GraphicsDisplay::SelectUserFont(_font);
}

RetCode_t RA8875::background(color_t color)
{
    GraphicsDisplay::background(color);
    return _writeColorTrio(0x60, color);
}


RetCode_t RA8875::background(unsigned char r, unsigned char g, unsigned char b)
{
    background(RGB(r,g,b));
    return noerror;
}


RetCode_t RA8875::foreground(color_t color)
{
    GraphicsDisplay::foreground(color);
    return _writeColorTrio(0x63, color);
}


RetCode_t RA8875::foreground(unsigned char r, unsigned char g, unsigned char b)
{
    foreground(RGB(r,g,b));
    return noerror;
}


color_t RA8875::GetForeColor(void)
{
    return _readColorTrio(0x63);
}


color_t RA8875::DOSColor(int i)
{
    const color_t colors[16] = {
        Black,    Blue,       Green,       Cyan,
        Red,      Magenta,    Brown,       Gray,
        Charcoal, BrightBlue, BrightGreen, BrightCyan,
        Orange,   Pink,       Yellow,      White
    };
    if (i >= 0 && i < 16)
        return colors[i];
    else
        return 0;
}


const char * RA8875::DOSColorNames(int i)
{
    const char * names[16] = {
        "Black",    "Blue",       "Green",       "Cyan",
        "Red",      "Magenta",    "Brown",       "Gray",
        "Charcoal", "BrightBlue", "BrightGreen", "BrightCyan",
        "Orange",   "Pink",       "Yellow",      "White"
    };
    if (i >= 0 && i < 16)
        return names[i];
    else
        return NULL;
}


///////////////////////////////////////////////////////////////
// Private functions

unsigned char RA8875::_spiwrite(unsigned char data)
{
    unsigned char retval;

    if (!spiWriteSpeed)
        _setWriteSpeed(true);
    retval = spi.write(data);
    return retval;
}


unsigned char RA8875::_spiread(void)
{
    unsigned char retval;
    unsigned char data = 0;

    if (spiWriteSpeed)
        _setWriteSpeed(false);
    retval = spi.write(data);
    return retval;
}


RetCode_t RA8875::_select(bool chipsel)
{
    cs = (chipsel == true) ? 0 : 1;
    return noerror;
}


RetCode_t RA8875::PrintScreen(uint16_t layer, loc_t x, loc_t y, dim_t w, dim_t h, const char *Name_BMP)
{
    (void)layer;
    
    // AttachPrintHandler(this, RA8875::_printCallback);
    // return PrintScreen(x,y,w,h);
    return PrintScreen(x, y, w, h, Name_BMP);
}

RetCode_t RA8875::_printCallback(RA8875::filecmd_t cmd, uint8_t * buffer, uint16_t size)
{
    HexDump("CB", buffer, size);
    switch(cmd) {
        case RA8875::OPEN:
            //pc.printf("About to write %lu bytes\r\n", *(uint32_t *)buffer);
            _printFH = fopen("file.bmp", "w+b");
            if (_printFH == 0)
                return file_not_found;
            break;
        case RA8875::WRITE:
            //pc.printf("  Write %4u bytes\r\n", size);
            fwrite(buffer, 1, size, _printFH);
            break;
        case RA8875::CLOSE:
            //pc.printf("  close\r\n");
            fclose(_printFH);
            _printFH = 0;
            break;
        default:
            //pc.printf("Unexpected callback %d\r\n", cmd);
            return file_not_found;
            //break;
    }
    return noerror;
}

int RA8875::RoundUp(int value, int roundTo)
{
    if (roundTo == 0) 
        return 0;
    return ((value + roundTo - 1) / roundTo) * roundTo;
}

RetCode_t RA8875::PrintScreen(loc_t x, loc_t y, dim_t w, dim_t h, uint8_t bitsPerPixel)
{
    BITMAPFILEHEADER BMP_Header;
    BITMAPINFOHEADER BMP_Info;
    uint8_t * lineBuffer = NULL;
    color_t * pixelBuffer = NULL;
    color_t * pixelBuffer2 = NULL;
    
    INFO("(%d,%d)-(%d,%d)x%d", x,y,w,h,bitsPerPixel);
    if (x >= 0 && x < screenwidth
            && y >= 0 && y < screenheight
            && w > 0 && x + w <= screenwidth
            && h > 0 && y + h <= screenheight) {
        BMP_Header.bfType = BF_TYPE;
        BMP_Header.bfReserved1 = 0;
        BMP_Header.bfReserved2 = 0;
        switch (bitsPerPixel) {
            case 24:
                BMP_Header.bfOffBits = sizeof(BMP_Header) + sizeof(BMP_Info);
                BMP_Header.bfSize = (h * RoundUp(w * sizeof(RGBQUAD),4)) + BMP_Header.bfOffBits;
                break;
            case 8:
            default:
                BMP_Header.bfOffBits = sizeof(BMP_Header) + sizeof(BMP_Info) + sizeof(WebColorPalette);
                INFO("Initial Offset to Bitstream %X", BMP_Header.bfOffBits);
                //if (BMP_Header.bfOffBits & 0x03) {
                //    BMP_Header.bfOffBits += (4 - (BMP_Header.bfOffBits & 0x03));
                //}
                BMP_Header.bfSize = (h * RoundUp(w * 1,4)) + BMP_Header.bfOffBits;
                break;
        }
        INFO("Offset to Bitstream %X", BMP_Header.bfOffBits);

        // Bytes in the line buffer
        int lineBufSize = RoundUp(((bitsPerPixel == 24) ? 3 : 1) * w, 4);
        INFO("LineBufSize: %d", lineBufSize);

        BMP_Info.biSize = sizeof(BMP_Info);
        BMP_Info.biWidth = w;
        BMP_Info.biHeight = h;
        BMP_Info.biPlanes = 1;
        BMP_Info.biBitCount = bitsPerPixel;
        BMP_Info.biCompression = BI_RGB;
        BMP_Info.biSizeImage = lineBufSize * h;
        BMP_Info.biXPelsPerMeter = 0;
        BMP_Info.biYPelsPerMeter = 0;
        // for 24-bit, there is no palette, so these are zero
        // for 8-bit, there can be up to 256 RGB values in the palette
        
        BMP_Info.biClrUsed = (bitsPerPixel == 24) ? 0 : sizeof(WebColorPalette)/sizeof(WebColorPalette[0]);    // for 8b/pixel
        BMP_Info.biClrImportant = BMP_Info.biClrUsed;

        // Allocate the memory we need to proceed
        lineBuffer = (uint8_t *)swMalloc(lineBufSize);
        if (lineBuffer == NULL) {
            ERR("Not enough RAM for PrintScreen lineBuffer");
            return(not_enough_ram);
        }
        memset(lineBuffer, 0, lineBufSize); // zero-Fill

        #define DOUBLEBUF /* one larger buffer instead of two */
        
        #ifdef DOUBLEBUF
        // In the "#else", pixelBuffer2 malloc returns a value, 
        // but is actually causing a failure later. 
        // This test helps determine if it is truly out of memory,
        // or if malloc is broken.
        pixelBuffer = (color_t *)swMalloc(2 * w * sizeof(color_t));
        pixelBuffer2 = pixelBuffer + (w * sizeof(color_t));
        #else
        pixelBuffer = (color_t *)swMalloc(w * sizeof(color_t));
        pixelBuffer2 = (color_t *)swMalloc(w * sizeof(color_t));
        #endif
        if (pixelBuffer == NULL || pixelBuffer2 == NULL) {
            ERR("Not enough RAM for pixelBuffer");
            #ifndef DOUBLEBUF
            if (pixelBuffer2)
                swFree(pixelBuffer2);
            #endif
            if (pixelBuffer)
                swFree(pixelBuffer);
            swFree(lineBuffer);
            return(not_enough_ram);
        }

        // Get the file primed...
        /// @todo check return value for possibility of a fatal error
        privateCallback(OPEN, (uint8_t *)&BMP_Header.bfSize, 4);

        // Be optimistic - don't check for errors.
        HexDump("BMP_Header", (uint8_t *)&BMP_Header, sizeof(BMP_Header));
        //fwrite(&BMP_Header, sizeof(char), sizeof(BMP_Header), Image);
        privateCallback(WRITE, (uint8_t *)&BMP_Header, sizeof(BMP_Header));

        HexDump("BMP_Info", (uint8_t *)&BMP_Info, sizeof(BMP_Info));
        //fwrite(&BMP_Info, sizeof(char), sizeof(BMP_Info), Image);
        privateCallback(WRITE, (uint8_t *)&BMP_Info, sizeof(BMP_Info));
        if (bitsPerPixel != 24) {
            HexDump("Palette", (uint8_t *)&WebColorPalette, sizeof(WebColorPalette));
            //fwrite(&WebColorPalette, sizeof(char), sizeof(WebColorPalette), Image);
            privateCallback(WRITE, (uint8_t *)&WebColorPalette, sizeof(WebColorPalette));
            if (0 && sizeof(WebColorPalette) % 4) {
                const uint8_t padd[] = { 0, 0, 0 };
                //fwrite(&padd, sizeof(char), (sizeof(BMP_Header) + sizeof(BMP_Info) + sizeof(WebColorPalette)) % 4, Image);
                privateCallback(WRITE, (uint8_t *)&padd, (sizeof(BMP_Header) + sizeof(BMP_Info) + sizeof(WebColorPalette)) % 4);
            }
        }
        //color_t transparency = GetBackgroundTransparencyColor();
        LayerMode_T ltpr0 = GetLayerMode();

        uint16_t prevLayer = GetDrawingLayer();
        // If only one of the layers is visible, select that layer
        switch(ltpr0) {
            case ShowLayer0:
                SelectDrawingLayer(0);
                break;
            case ShowLayer1:
                SelectDrawingLayer(1);
                break;
            default:
                break;
        }

        // Read the display from the last line toward the top
        // so we can write the file in one pass.
        for (int j = h - 1; j >= 0; j--) {
            if (idle_callback) {
                (*idle_callback)(progress, (h - 1 - j) * 100 / (h - 1));
            }

            if (ltpr0 >= 2)             // Need to combine the layers...
                SelectDrawingLayer(0);  // so read layer 0 first
            // Read one line of pixels to a local buffer
            if (getPixelStream(pixelBuffer, w, x,y+j) != noerror) {
                ERR("getPixelStream error, and no recovery handler...");
            }
            if (ltpr0 >= 2) {           // Need to combine the layers...
                SelectDrawingLayer(1);  // so read layer 1 next
                if (getPixelStream(pixelBuffer2, w, x,y+j) != noerror) {
                    ERR("getPixelStream error, and no recovery handler...");
                }
            }
            INFO("Line: %3d", j);
            //HexDump("Raster", (uint8_t *)pixelBuffer, w * sizeof(color_t));
            // Convert the local buffer to RGBQUAD format
            int lb = 0;
            for (int i=0; i<w; i++) {
                color_t tColor = pixelBuffer[x+i];
                tColor = (tColor >> 8) | (tColor << 8);     // Byte Swap
                RGBQUAD q0 = RGB16ToRGBQuad(tColor);        // Scale to 24-bits
                tColor = pixelBuffer2[x+i];
                tColor = (tColor >> 8) | (tColor << 8);     // Byte Swap
                RGBQUAD q1 = RGB16ToRGBQuad(tColor);        // Scale to 24-bits
                switch (ltpr0) {
                    case 0:
                    case 1:
                    case 2: // lighten-overlay  (@TODO Not supported yet)
                    case 6: // Floating Windows     (@TODO not sure how to support)
                    default: // Reserved...
                        //lineBuffer[lb++] = q0.rgbBlue;
                        //lineBuffer[lb++] = q0.rgbGreen;
                        //lineBuffer[lb++] = q0.rgbRed;
                        break;
                    case 3: // transparent mode (@TODO Read the background color register for transparent)
                    case 4: // boolean or
                        q0.rgbBlue = q0.rgbBlue | q1.rgbBlue;
                        q0.rgbGreen = q0.rgbGreen | q1.rgbGreen;
                        q0.rgbRed = q0.rgbRed | q1.rgbRed;
                        break;
                    case 5: // boolean AND
                        q0.rgbBlue = q0.rgbBlue & q1.rgbBlue;
                        q0.rgbGreen = q0.rgbGreen & q1.rgbGreen;
                        q0.rgbRed = q0.rgbRed & q1.rgbRed;
                        break;
                }
                switch (bitsPerPixel) {
                    case 24:
                        lineBuffer[lb++] = q0.rgbBlue;
                        lineBuffer[lb++] = q0.rgbGreen;
                        lineBuffer[lb++] = q0.rgbRed;
                        break;
                    case 8:
                    default:
                        lineBuffer[lb++] = FindNearestWebColor(q0.rgbRed,q0.rgbGreen,q0.rgbBlue);
                        break;
                }
            }
            //if (j == h - 1) {
            //    HexDump("Line", lineBuffer, lineBufSize);
            //}
            // Write to disk
            privateCallback(WRITE, (uint8_t *)lineBuffer, lineBufSize);
        }
        SelectDrawingLayer(prevLayer);
        privateCallback(CLOSE, NULL, 0);
        #ifndef DOUBLEBUF
        if (pixelBuffer2)
            swFree(pixelBuffer2);
        #endif
        if (pixelBuffer)
            swFree(pixelBuffer);
        swFree(lineBuffer);
        INFO("Image closed");
        return noerror;
    } else {
        return bad_parameter;
    }
}



RetCode_t RA8875::PrintScreen(loc_t x, loc_t y, dim_t w, dim_t h, const char *Name_BMP, uint8_t bitsPerPixel)
{
    BITMAPFILEHEADER BMP_Header;
    BITMAPINFOHEADER BMP_Info;
    uint8_t * lineBuffer = NULL;
    color_t * pixelBuffer = NULL;
    color_t * pixelBuffer2 = NULL;
    
    INFO("(%d,%d)-(%d,%d)x%d %s", x,y,w,h,bitsPerPixel,Name_BMP);
    if (x >= 0 && x < screenwidth
            && y >= 0 && y < screenheight
            && w > 0 && x + w <= screenwidth
            && h > 0 && y + h <= screenheight) {
        BMP_Header.bfType = BF_TYPE;
        BMP_Header.bfReserved1 = 0;
        BMP_Header.bfReserved2 = 0;
        switch (bitsPerPixel) {
            case 24:
                BMP_Header.bfOffBits = sizeof(BMP_Header) + sizeof(BMP_Info);
                BMP_Header.bfSize = (h * RoundUp(w * sizeof(RGBQUAD),4)) + BMP_Header.bfOffBits;
                break;
            case 8:
            default:
                BMP_Header.bfOffBits = sizeof(BMP_Header) + sizeof(BMP_Info) + sizeof(WebColorPalette);
                INFO("Initial Offset to Bitstream %X", BMP_Header.bfOffBits);
                //if (BMP_Header.bfOffBits & 0x03) {
                //    BMP_Header.bfOffBits += (4 - (BMP_Header.bfOffBits & 0x03));
                //}
                BMP_Header.bfSize = (h * RoundUp(w * 1,4)) + BMP_Header.bfOffBits;
                break;
        }
        INFO("Offset to Bitstream %X", BMP_Header.bfOffBits);

        // Bytes in the line buffer
        int lineBufSize = RoundUp(((bitsPerPixel == 24) ? 3 : 1) * w, 4);
        INFO("LineBufSize: %d", lineBufSize);

        BMP_Info.biSize = sizeof(BMP_Info);
        BMP_Info.biWidth = w;
        BMP_Info.biHeight = h;
        BMP_Info.biPlanes = 1;
        BMP_Info.biBitCount = bitsPerPixel;
        BMP_Info.biCompression = BI_RGB;
        BMP_Info.biSizeImage = lineBufSize * h;
        BMP_Info.biXPelsPerMeter = 0;
        BMP_Info.biYPelsPerMeter = 0;
        // for 24-bit, there is no palette, so these are zero
        // for 8-bit, there can be up to 256 RGB values in the palette
        
        BMP_Info.biClrUsed = (bitsPerPixel == 24) ? 0 : sizeof(WebColorPalette)/sizeof(WebColorPalette[0]);    // for 8b/pixel
        BMP_Info.biClrImportant = BMP_Info.biClrUsed;

        // Allocate the memory we need to proceed
        lineBuffer = (uint8_t *)swMalloc(lineBufSize);
        if (lineBuffer == NULL) {
            ERR("Not enough RAM for PrintScreen lineBuffer");
            return(not_enough_ram);
        }
        memset(lineBuffer, 0, lineBufSize); // zero-Fill

        #define DOUBLEBUF /* one larger buffer instead of two */
        
        #ifdef DOUBLEBUF
        // In the "#else", pixelBuffer2 malloc returns a value, 
        // but is actually causing a failure later. 
        // This test helps determine if it is truly out of memory,
        // or if malloc is broken.
        pixelBuffer = (color_t *)swMalloc(2 * w * sizeof(color_t));
        pixelBuffer2 = pixelBuffer + (w * sizeof(color_t));
        #else
        pixelBuffer = (color_t *)swMalloc(w * sizeof(color_t));
        pixelBuffer2 = (color_t *)swMalloc(w * sizeof(color_t));
        #endif
        if (pixelBuffer == NULL || pixelBuffer2 == NULL) {
            ERR("Not enough RAM for pixelBuffer");
            #ifndef DOUBLEBUF
            if (pixelBuffer2)
                swFree(pixelBuffer2);
            #endif
            if (pixelBuffer)
                swFree(pixelBuffer);
            swFree(lineBuffer);
            return(not_enough_ram);
        }

        FILE *Image = fopen(Name_BMP, "wb");
        if (!Image) {
            ERR("Can't open file for write");
            #ifndef DOUBLEBUF
            if (pixelBuffer2)
                swFree(pixelBuffer2);
            #endif
            if (pixelBuffer)
                swFree(pixelBuffer);
            swFree(lineBuffer);
            return(file_not_found);
        }

        // Be optimistic - don't check for errors.
        HexDump("BMP_Header", (uint8_t *)&BMP_Header, sizeof(BMP_Header));
        fwrite(&BMP_Header, sizeof(char), sizeof(BMP_Header), Image);

        HexDump("BMP_Info", (uint8_t *)&BMP_Info, sizeof(BMP_Info));
        fwrite(&BMP_Info, sizeof(char), sizeof(BMP_Info), Image);
        
        if (bitsPerPixel != 24) {
            HexDump("Palette", (uint8_t *)&WebColorPalette, sizeof(WebColorPalette));
            fwrite(&WebColorPalette, sizeof(char), sizeof(WebColorPalette), Image);
            if (0 && sizeof(WebColorPalette) % 4) {
                const uint8_t padd[] = { 0, 0, 0 };
                fwrite(&padd, sizeof(char), 
                    (sizeof(BMP_Header) + sizeof(BMP_Info) + sizeof(WebColorPalette)) % 4, Image);
            }
        }
        //color_t transparency = GetBackgroundTransparencyColor();
        LayerMode_T ltpr0 = GetLayerMode();

        uint16_t prevLayer = GetDrawingLayer();
        // If only one of the layers is visible, select that layer
        switch(ltpr0) {
            case ShowLayer0:
                SelectDrawingLayer(0);
                break;
            case ShowLayer1:
                SelectDrawingLayer(1);
                break;
            default:
                break;
        }

        // Read the display from the last line toward the top
        // so we can write the file in one pass.
        for (int j = h - 1; j >= 0; j--) {
            if (idle_callback) {
                (*idle_callback)(progress, (h - 1 - j) * 100 / (h - 1));
            }

            if (ltpr0 >= 2)             // Need to combine the layers...
                SelectDrawingLayer(0);  // so read layer 0 first
            // Read one line of pixels to a local buffer
            if (getPixelStream(pixelBuffer, w, x,y+j) != noerror) {
                ERR("getPixelStream error, and no recovery handler...");
            }
            if (ltpr0 >= 2) {           // Need to combine the layers...
                SelectDrawingLayer(1);  // so read layer 1 next
                if (getPixelStream(pixelBuffer2, w, x,y+j) != noerror) {
                    ERR("getPixelStream error, and no recovery handler...");
                }
            }
            INFO("Line: %3d", j);
            //HexDump("Raster", (uint8_t *)pixelBuffer, w * sizeof(color_t));
            // Convert the local buffer to RGBQUAD format
            int lb = 0;
            for (int i=0; i<w; i++) {
                color_t tColor = pixelBuffer[x+i];
                tColor = (tColor >> 8) | (tColor << 8);     // Byte Swap
                RGBQUAD q0 = RGB16ToRGBQuad(tColor);        // Scale to 24-bits
                tColor = pixelBuffer2[x+i];
                tColor = (tColor >> 8) | (tColor << 8);     // Byte Swap
                RGBQUAD q1 = RGB16ToRGBQuad(tColor);        // Scale to 24-bits
                switch (ltpr0) {
                    case 0:
                    case 1:
                    case 2: // lighten-overlay  (@TODO Not supported yet)
                    case 6: // Floating Windows     (@TODO not sure how to support)
                    default: // Reserved...
                        //lineBuffer[lb++] = q0.rgbBlue;
                        //lineBuffer[lb++] = q0.rgbGreen;
                        //lineBuffer[lb++] = q0.rgbRed;
                        break;
                    case 3: // transparent mode (@TODO Read the background color register for transparent)
                    case 4: // boolean or
                        q0.rgbBlue = q0.rgbBlue | q1.rgbBlue;
                        q0.rgbGreen = q0.rgbGreen | q1.rgbGreen;
                        q0.rgbRed = q0.rgbRed | q1.rgbRed;
                        break;
                    case 5: // boolean AND
                        q0.rgbBlue = q0.rgbBlue & q1.rgbBlue;
                        q0.rgbGreen = q0.rgbGreen & q1.rgbGreen;
                        q0.rgbRed = q0.rgbRed & q1.rgbRed;
                        break;
                }
                switch (bitsPerPixel) {
                    case 24:
                        lineBuffer[lb++] = q0.rgbBlue;
                        lineBuffer[lb++] = q0.rgbGreen;
                        lineBuffer[lb++] = q0.rgbRed;
                        break;
                    case 8:
                    default:
                        lineBuffer[lb++] = FindNearestWebColor(q0.rgbRed,q0.rgbGreen,q0.rgbBlue);
                        break;
                }
            }
            //if (j == h - 1) {
            //    HexDump("Line", lineBuffer, lineBufSize);
            //}
            // Write to disk
            fwrite(lineBuffer, sizeof(char), lineBufSize, Image);
        }
        SelectDrawingLayer(prevLayer);
        fclose(Image);
        #ifndef DOUBLEBUF
        if (pixelBuffer2)
            swFree(pixelBuffer2);
        #endif
        if (pixelBuffer)
            swFree(pixelBuffer);
        swFree(lineBuffer);
        INFO("Image closed");
        return noerror;
    } else {
        return bad_parameter;
    }
}


// ##########################################################################
// ##########################################################################
// ##########################################################################

#ifdef TESTENABLE

#include "BPG_Arial08x08.h"
#include "BPG_Arial20x20.h"

//      ______________  ______________  ______________  _______________
//     /_____   _____/ /  ___________/ /  ___________/ /_____   ______/
//          /  /      /  /            /  /                  /  /
//         /  /      /  /___         /  /__________        /  /
//        /  /      /  ____/        /__________   /       /  /
//       /  /      /  /                       /  /       /  /
//      /  /      /  /__________  ___________/  /       /  /
//     /__/      /_____________/ /_____________/       /__/
//
//    Everything from here down is test code.
//
bool SuppressSlowStuff = false;

void TextWrapTest(RA8875 & display, Serial & pc)
{
    if (!SuppressSlowStuff)
        pc.printf("Text Wrap Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.Backlight_u8(255);
    display.puts("Text Wrap Test.\r\n");
    for (int i=1; i<60; i++) {
        display.printf("L%2d\n", i % 17);
        if (!SuppressSlowStuff)
            wait_ms(100);
    }
    if (!SuppressSlowStuff)
        wait_ms(3000);
}


void ShowKey(RA8875 & display, int key)
{
    loc_t col, row;
    dim_t r1 = 25;
    color_t color = (key & 0x80) ? Red : Green;

    key &= 0x7F;        // remove the long-press flag
    row = (key - 1) / 5;
    col = (key - 1) % 5;
    if (col > 5) col = 5;
    if (row > 4) row = 4;
    display.circle(450 - + (2 * r1) * col, 200 - (2 * r1) * row, r1-2, color, FILL);
}

void HideKey(RA8875 & display, int key)
{
    loc_t col, row;
    dim_t r1 = 25;

    row = (key - 1) / 5;
    col = (key - 1) % 5;
    if (col > 5) col = 5;
    if (row > 4) row = 4;
    display.background(Black);
    display.circle(450 - (2 * r1) * col, 200 - (2 * r1) * row, r1-2, Black, FILL);
    display.circle(450 - (2 * r1) * col, 200 - (2 * r1) * row, r1-2, Blue);
}

void KeyPadTest(RA8875 & display, Serial & pc)
{
    const uint8_t myMap[22] = {
        0,
        'a', 'b', 'c', 'd', 'e',
        'f', 'g', 'h', 'i', 'j',
        'k', 'l', 'm', 'n', 'o',
        'p', 'q', 'r', 's', 't',
        'x'
    };

    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.Backlight_u8(255);
    display.puts("KeyPad Test. Touch the keypad...");
    pc.printf("\r\n"
              "Raw KeyPad Test. Keypad returns the key-number.\r\n"
              "Press [most] any PC keyboard key to advance to next test.\r\n");
    RetCode_t ret = display.KeypadInit(true, true, 3, 7, 3);
    if (ret != noerror)
        pc.printf("returncode from KeypadInit is %d\r\n", ret);
    int lastKey = 0;
    while (!pc.readable()) {
        if (display.readable()) {
            int key = display.getc();
            if (key) {
                if (((key & 0x7F) != lastKey) && (lastKey != 0))
                    HideKey(display, lastKey);
                ShowKey(display, key);
                lastKey = key & 0x7F;
            } else {
                // erase the last one
                if (lastKey)
                    HideKey(display, lastKey);
            }
        }
    }
    (void)pc.getc();
    pc.printf("\r\n"
              "Map KeyPad Test. Keypad returns the remapped key 'a' - 't'.\r\n"
              "Press [most] any PC keyboard key to advance to exit test.\r\n");
    display.SetKeyMap(myMap);
    while (!pc.readable()) {
        if (display.readable()) {
            int key = display.getc();
            bool longPress = key & 0x80;
            display.SetTextCursor(0, 120);
            display.printf("Long Press: %d\r\n", longPress);
            display.printf("  Remapped: %c %02X\r\n", (key) ? key & 0x7F : ' ', key);
        }
    }
    (void)pc.getc();
    display.SetKeyMap();
    pc.printf("\r\n");
}

void TextCursorTest(RA8875 & display, Serial & pc)
{
    const char * iCursor  = "The I-Beam cursor should be visible for this text.\r\n";
    const char * uCursor  = "The Underscore cursor should be visible for this text.\r\n";
    const char * bCursor  = "The Block cursor should be visible for this text.\r\n";
    const char * bbCursor = "The Blinking Block cursor should be visible for this text.\r\n";
    const char * p;
    int delay = 60;

    if (!SuppressSlowStuff)
        pc.printf("Text Cursor Test\r\n");
    else
        delay = 0;
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.Backlight_u8(255);
    display.puts("Text Cursor Test.");

    // visible, non-blinking
    display.SetTextCursor(0,20);
    display.SetTextCursorControl(RA8875::IBEAM, false);
    p = iCursor;
    while (*p) {
        display._putc(*p++);
        wait_ms(delay);
    }

    display.SetTextCursorControl(RA8875::UNDER, false);
    p = uCursor;
    while (*p) {
        display._putc(*p++);
        wait_ms(delay);
    }

    display.SetTextCursorControl(RA8875::BLOCK, false);
    p = bCursor;
    while (*p) {
        display._putc(*p++);
        wait_ms(delay);
    }

    display.SetTextCursorControl(RA8875::BLOCK, true);
    p = bbCursor;
    while (*p) {
        display._putc(*p++);
        wait_ms(delay);
    }
    wait_ms(delay * 20);
    display.SetTextCursorControl(RA8875::NOCURSOR, false);
}


void BacklightTest(RA8875 & display, Serial & pc, float ramptime)
{
    char buf[60];
    unsigned int w = (ramptime * 1000)/ 256;
    int delay = 200;

    if (!SuppressSlowStuff)
        pc.printf("Backlight Test - ramp over %f sec.\r\n", ramptime);
    else {
        delay = 0;
        w = 0;
    }
    display.Backlight_u8(0);
    display.background(White);
    display.foreground(Blue);
    display.cls();
    display.puts("RA8875 Backlight Test - Ramp up.");
    wait_ms(delay);
    for (int i=0; i <= 255; i++) {
        snprintf(buf, sizeof(buf), "%3d, %4d", i, w);
        display.puts(100,100,buf);
        display.Backlight_u8(i);
        wait_ms(w);
    }
}


void BacklightTest2(RA8875 & display, Serial & pc)
{
    int delay = 20;

    if (!SuppressSlowStuff)
        pc.printf("Backlight Test 2\r\n");
    else
        delay = 0;

    // Dim it out at the end of the tests.
    display.foreground(Blue);
    display.puts(0,0, "Ramp Backlight down.");
    // Ramp it off
    for (int i=255; i != 0; i--) {
        display.Backlight_u8(i);
        wait_ms(delay);
    }
    display.Backlight_u8(0);
}


void ExternalFontTest(RA8875 & display, Serial & pc)
{
    if (!SuppressSlowStuff)
        pc.printf("External Font Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("External Font Test.");
    display.Backlight(1);

    display.SelectUserFont(BPG_Arial08x08);
    display.puts(0,30, "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz\r\n");

    display.SelectUserFont(BPG_Arial20x20);
    display.puts("0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz\r\n");
    
    display.SelectUserFont();

    display.puts("Normal font again.");
    //display.window(0,0, display.width(), display.height());
}


void DOSColorTest(RA8875 & display, Serial & pc)
{
    if (!SuppressSlowStuff)
        pc.printf("DOS Color Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("DOS Colors - Fore");
    display.puts(280,0, "Back");
    display.background(Gray);
    for (int i=0; i<16; i++) {
        display.foreground(display.DOSColor(i));
        display.puts(160, i*16, display.DOSColorNames(i));
        display.background(Black);
    }
    display.foreground(White);
    for (int i=0; i<16; i++) {
        display.background(display.DOSColor(i));
        display.puts(360, i*16, display.DOSColorNames(i));
        display.foreground(White);
    }
}


void WebColorTest(RA8875 & display, Serial & pc)
{
    if (!SuppressSlowStuff)
        pc.printf("Web Color Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.window(0,0, display.width(), display.height());
    display.cls();
    display.SetTextFontSize(1,1);
    display.puts(200,0, "Web Color Test");
    display.SetTextCursor(0,0);
    display.puts("  ");
    for (int i=0; i<16; i++)
        display.printf("%X", i&0xF);
    display.puts("\r\n0 ");
    for (int i=0; i<sizeof(WebColors)/sizeof(WebColors[0]); i++) {
        display.background(WebColors[i]);
        display.puts(" ");
        if (i % 16 == 15 && i < 255) {
            display.printf("\r\n%X ", ((i+1)/16));
        }
    }
    display.SetTextFontSize(1,1);
}


void PixelTest(RA8875 & display, Serial & pc)
{
    int i, c, x, y;

    if (!SuppressSlowStuff)
        pc.printf("Pixel Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Pixel Test");
    for (i=0; i<1000; i++) {
        x = rand() % 480;
        y = 16 + rand() % (272-16);
        c = rand() % 16;
        //pc.printf("  (%d,%d) - %d\r\n", x,y,r1);
        display.pixel(x,y, display.DOSColor(c));
    }
}


void LineTest(RA8875 & display, Serial & pc)
{
    int i, x, y, x2, y2;

    if (!SuppressSlowStuff)
        pc.printf("Line Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Line Test");
    for (i=0; i<16; i++) {
        // Lines
        x = rand() % 480;
        y = rand() % 272;
        x2 = rand() % 480;
        y2 = rand() % 272;
        display.line(x,y, x2,y2, display.DOSColor(i));
    }
    display.foreground(BrightRed);
    display.foreground(BrightGreen);
    display.foreground(BrightBlue);
    display.line(55,50, 79,74, BrightRed);
    display.line(57,50, 81,74, BrightGreen);
    display.line(59,50, 83,74, BrightBlue);
    // horz
    display.line(30,40, 32,40, BrightRed);
    display.line(30,42, 32,42, BrightGreen);
    display.line(30,44, 32,44, BrightBlue);
    // vert
    display.line(20,40, 20,42, BrightRed);
    display.line(22,40, 22,42, BrightGreen);
    display.line(24,40, 24,42, BrightBlue);
    // compare point to line-point
    display.pixel(20,50, BrightRed);
    display.pixel(22,50, BrightGreen);
    display.pixel(24,50, BrightBlue);
    display.line(20,52, 20,52, BrightRed);
    display.line(22,52, 22,52, BrightGreen);
    display.line(24,52, 24,52, BrightBlue);

    // point
    display.line(50,50, 50,50, Red);
    display.line(52,52, 52,52, Green);
    display.line(54,54, 54,54, Blue);
    display.line(60,60, 60,60, BrightRed);
    display.line(62,62, 62,62, BrightGreen);
    display.line(64,64, 64,64, BrightBlue);
    display.line(70,70, 70,70, DarkRed);
    display.line(72,72, 72,72, DarkGreen);
    display.line(74,74, 74,74, DarkBlue);
}


void RectangleTest(RA8875 & display, Serial & pc)
{
    int i, x1,y1, x2,y2;

    if (!SuppressSlowStuff)
        pc.printf("Rectangle Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Rectangle Test");
    for (i=0; i<16; i++) {
        x1 = rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = rand() % 240;
        y2 = 50 + rand() % 200;
        display.rect(x1,y1, x2,y2, display.DOSColor(i));

        x1 = 240 + rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = 240 + rand() % 240;
        y2 = 50 + rand() % 200;
        display.rect(x1,y1, x2,y2, FILL);
    }
}


void LayerTest(RA8875 & display, Serial & pc)
{
    loc_t i, x1,y1, x2,y2, r1,r2;

    if (!SuppressSlowStuff)
        pc.printf("Layer Test\r\n");

    display.SelectDrawingLayer(0);
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Layer 0");
    for (i=0; i<16; i++) {
        x1 = rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = x1 + rand() % 100;
        y2 = y1 + rand() % 100;
        r1 = rand() % (x2 - x1)/2;
        r2 = rand() % (y2 - y1)/2;
        display.roundrect(x1,y1, x2,y2, r1,r2, display.DOSColor(i));
        if (!SuppressSlowStuff)
            wait_ms(20);
    }
    if (!SuppressSlowStuff)
        wait_ms(1000);

    display.SelectDrawingLayer(1);
    display.background(Black);
    display.foreground(Yellow);
    display.cls();
    display.puts(240,0, "Layer 1");
    for (i=0; i<16; i++) {
        x1 = 300 + rand() % 100;
        y1 = 70 + rand() % 200;
        r1 = rand() % min(y1 - 20, 100);
        display.circle(x1,y1,r1, display.DOSColor(i));
        if (!SuppressSlowStuff)
            wait_ms(20);
    }
    display.SetLayerMode(RA8875::ShowLayer1);        // Show it after the build-up
    if (!SuppressSlowStuff)
        wait_ms(2000);

    display.SelectDrawingLayer(0);
    display.SetLayerMode(RA8875::ShowLayer0);        // Show Layer 0 again
    if (!SuppressSlowStuff)
        wait_ms(1000);
    display.SetLayerMode(RA8875::TransparentMode);        // Transparent mode
    if (!SuppressSlowStuff)
        wait_ms(1000);
    for (i=0; i<=8; i++) {
        display.SetLayerTransparency(i, 8-i);
        if (!SuppressSlowStuff)
            wait_ms(200);
    }

    // Restore before we exit
    display.SetLayerTransparency(0, 0);
    display.SetLayerMode(RA8875::ShowLayer0);        // Restore to layer 0
}


void RoundRectTest(RA8875 & display, Serial & pc)
{
    loc_t i, x1,y1, x2,y2, r1,r2;

    if (!SuppressSlowStuff)
        pc.printf("Round Rectangle Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Rounded Rectangle Test");

    for (i=0; i<16; i++) {
        x1 = rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = x1 + rand() % 100;
        y2 = y1 + rand() % 100;
        r1 = rand() % (x2 - x1)/2;
        r2 = rand() % (y2 - y1)/2;
        display.roundrect(x1,y1, x2,y2, 5,8, display.DOSColor(i));

        x1 = 240 + rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = x1 + rand() % 100;
        y2 = y1 + rand() % 100;
        r1 = rand() % (x2 - x1)/2;
        r2 = rand() % (y2 - y1)/2;
        display.roundrect(x1,y1, x2,y2, r1,r2, FILL);
    }
}


void TriangleTest(RA8875 & display, Serial & pc)
{
    int i, x1, y1, x2, y2, x3, y3;

    if (!SuppressSlowStuff)
        pc.printf("Triangle Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts(0,0, "Triangle Test");

    x1 = 150;
    y1 = 2;
    x2 = 190;
    y2 = 7;
    x3 = 170;
    y3 = 16;
    display.triangle(x1,y1, x2,y2, x3,y3);

    x1 = 200;
    y1 = 2;
    x2 = 240;
    y2 = 7;
    x3 = 220;
    y3 = 16;
    display.filltriangle(x1,y1, x2,y2, x3,y3, BrightRed);

    x1 = 300;
    y1 = 2;
    x2 = 340;
    y2 = 7;
    x3 = 320;
    y3 = 16;
    display.triangle(x1,y1, x2,y2, x3,y3, NOFILL);

    x1 = 400;
    y1 = 2;
    x2 = 440;
    y2 = 7;
    x3 = 420;
    y3 = 16;
    display.triangle(x1,y1, x2,y2, x3,y3, Blue);

    for (i=0; i<16; i++) {
        x1 = rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = rand() % 240;
        y2 = 50 + rand() % 200;
        x3 = rand() % 240;
        y3 = 50 + rand() % 200;
        display.triangle(x1,y1, x2,y2, x3,y3, display.DOSColor(i));
        x1 = 240 + rand() % 240;
        y1 = 50 + rand() % 200;
        x2 = 240 + rand() % 240;
        y2 = 50 + rand() % 200;
        x3 = 240 + rand() % 240;
        y3 = 50 + rand() % 200;
        display.triangle(x1,y1, x2,y2, x3,y3, FILL);
    }
}


void CircleTest(RA8875 & display, Serial & pc)
{
    int i, x, y, r1;

    if (!SuppressSlowStuff)
        pc.printf("Circle Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Circle Test");
    for (i=0; i<16; i++) {
        x = 100 + rand() % 100;
        y = 70 + rand() % 200;
        r1 = rand() % min(y - 20, 100);
        //pc.printf("  (%d,%d) - %d\r\n", x,y,r1);
        display.circle(x,y,r1, display.DOSColor(i));

        x = 300 + rand() % 100;
        y = 70 + rand() % 200;
        r1 = rand() % min(y - 20, 100);
        //pc.printf("  (%d,%d) - %d FILL\r\n", x,y,r1);
        display.circle(x,y,r1, display.DOSColor(i), FILL);
    }
}


void EllipseTest(RA8875 & display, Serial & pc)
{
    int i,x,y,r1,r2;

    if (!SuppressSlowStuff)
        pc.printf("Ellipse Test\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Ellipse Test");
    for (i=0; i<16; i++) {
        x = 100 + rand() % 100;
        y = 70 + rand() % 200;
        r1 = rand() % min(y - 20, 100);
        r2 = rand() % min(y - 20, 100);
        display.ellipse(x,y,r1,r2, display.DOSColor(i));

        x = 300 + rand() % 100;
        y = 70 + rand() % 200;
        r1 = rand() % min(y - 20, 100);
        r2 = rand() % min(y - 20, 100);
        display.ellipse(x,y,r1,r2, FILL);
    }
}


void TestGraphicsBitmap(RA8875 & display, Serial & pc)
{
    LocalFileSystem local("local");
    if (!SuppressSlowStuff)
        pc.printf("Bitmap File Load\r\n");
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Graphics Test, loading /local/TestPat.bmp");
    wait(3);

    int r = display.RenderBitmapFile(0,0, "/local/TestPat.bmp");
    if (!SuppressSlowStuff)
        pc.printf("  returned %d\r\n", r);
}


void TouchPanelTest(RA8875 & display, Serial & pc)
{
    Timer t;
    int x, y;
    tpMatrix_t calmatrix;
    
    display.background(Black);
    display.foreground(Blue);
    display.cls();
    display.puts("Touch Panel Test\r\n");
    pc.printf("Touch Panel Test\r\n");
    display.TouchPanelInit();
    pc.printf("  TP: c - calibrate, r - restore, t - test\r\n");
    int c = pc.getc();
    if (c == 'c') {
        point_t pTest[3] =
        { { 50, 50 }, {450, 150}, {225,250} };
        point_t pSample[3];
        for (int i=0; i<3; i++) {
            display.foreground(Blue);
            display.printf(" (%3d,%3d) => ", pTest[i].x, pTest[i].y);
            display.line(pTest[i].x-10, pTest[i].y, pTest[i].x+10, pTest[i].y, White);
            display.line(pTest[i].x, pTest[i].y-10, pTest[i].x, pTest[i].y+10, White);
            while (!display.TouchPanelA2DFiltered(&x, &y))
                wait_ms(20);
            pSample[i].x = x;
            pSample[i].y = y;
            display.line(pTest[i].x-10, pTest[i].y, pTest[i].x+10, pTest[i].y, Black);
            display.line(pTest[i].x, pTest[i].y-10, pTest[i].x, pTest[i].y+10, Black);
            display.foreground(Blue);
            display.printf(" (%4d,%4d)\r\n", x,y);
            while (display.TouchPanelA2DFiltered(&x, &y))
                wait_ms(20);
            wait(2);
        }
        display.TouchPanelComputeCalibration(pTest, pSample, &calmatrix);
        display.printf(" Writing calibration to tpcal.cfg\r\n");
        FILE * fh = fopen("/local/tpcal.cfg", "wb");
        if (fh) {
            fwrite(&calmatrix, sizeof(calmatrix), 1, fh);
            fclose(fh);
        }
        display.printf(" Calibration is complete.");
    } else if (c == 'r') {
        display.printf(" Reading calibration from tpcal.cfg\r\n");
        FILE * fh = fopen("/local/tpcal.cfg", "rb");
        if (fh) {
            fread(&calmatrix, sizeof(calmatrix), 1, fh);
            fclose(fh);
        }
        display.printf(" Calibration is complete.");
        display.TouchPanelSetMatrix(&calmatrix);
    }
    t.start();
    do {
        point_t point = {0, 0};
        if (display.TouchPanelReadable(&point)) {
            display.pixel(point.x, point.y, Red);
        }
    } while (t.read_ms() < 30000);
    pc.printf(">");
}


void SpeedTest(RA8875 & display, Serial & pc)
{
    Timer t;
    SuppressSlowStuff = true;
    pc.printf("\r\nSpeedTest disables delays, runs tests, reports overall time.\r\n");
    t.start();
    // do stuff fast
    TextCursorTest(display, pc);
    TextWrapTest(display, pc);
    BacklightTest(display, pc, 0);
    BacklightTest2(display, pc);
    ExternalFontTest(display, pc);
    DOSColorTest(display, pc);
    WebColorTest(display, pc);
    PixelTest(display, pc);
    LineTest(display, pc);
    RectangleTest(display, pc);
    RoundRectTest(display, pc);
    TriangleTest(display, pc);
    CircleTest(display, pc);
    EllipseTest(display, pc);
    LayerTest(display, pc);
    //TestGraphicsBitmap(display, pc);
    pc.printf("SpeedTest completed in %d msec\r\n", t.read_ms());
#ifdef PERF_METRICS
    display.ReportPerformance(pc);
#endif
    SuppressSlowStuff = false;
}


void PrintScreen(RA8875 & display, Serial & pc)
{
    if (!SuppressSlowStuff)
        pc.printf("PrintScreen\r\n");
    display.PrintScreen( 0,0, 480,272, "/local/Capture.bmp");
}


void RunTestSet(RA8875 & lcd, Serial & pc)
{
    int q = 0;
    int automode = 0;
    const unsigned char modelist[] = "BDWtGLlFROTPCEbw";   // auto-test in this order.

    while(1) {
        pc.printf("\r\n"
                  "B - Backlight up      b - backlight dim\r\n"
                  "D - DOS Colors        W - Web Colors\r\n"
                  "t - text cursor       G - Graphics Bitmap\r\n"
                  "L - Lines             F - external Font\r\n"
                  "R - Rectangles        O - rOund rectangles\r\n"
                  "T - Triangles         P - Pixels  \r\n"
                  "C - Circles           E - Ellipses\r\n"
                  "A - Auto Test mode    S - Speed Test\r\n"
                  "K - Keypad Test       s - touch screen test\r\n"
                  "p - print screen      r - reset  \r\n"
                  "l - layer test        w - wrapping text \r\n"
#ifdef PERF_METRICS
                  "0 - clear performance 1 - report performance\r\n"
#endif
                  "> ");
        if (automode == -1 || pc.readable()) {
            automode = -1;
            q = pc.getc();
            while (pc.readable())
                pc.getc();
        } else if (automode >= 0) {
            q = modelist[automode];
        }
        switch(q) {
#ifdef PERF_METRICS
            case '0':
                lcd.ClearPerformance();
                break;
            case '1':
                lcd.ReportPerformance(pc);
                break;
#endif
            case 'A':
                automode = 0;
                break;
            case 'B':
                BacklightTest(lcd, pc, 2);
                break;
            case 'b':
                BacklightTest2(lcd, pc);
                break;
            case 'D':
                DOSColorTest(lcd, pc);
                break;
            case 'K':
                KeyPadTest(lcd, pc);
                break;
            case 'W':
                WebColorTest(lcd, pc);
                break;
            case 't':
                TextCursorTest(lcd, pc);
                break;
            case 'w':
                TextWrapTest(lcd, pc);
                break;
            case 'F':
                ExternalFontTest(lcd, pc);
                break;
            case 'L':
                LineTest(lcd, pc);
                break;
            case 'l':
                LayerTest(lcd, pc);
                break;
            case 'R':
                RectangleTest(lcd, pc);
                break;
            case 'O':
                RoundRectTest(lcd, pc);
                break;
            case 'p':
                PrintScreen(lcd, pc);
                break;
            case 'S':
                SpeedTest(lcd, pc);
                break;
            case 's':
                TouchPanelTest(lcd, pc);
                break;
            case 'T':
                TriangleTest(lcd, pc);
                break;
            case 'P':
                PixelTest(lcd, pc);
                break;
            case 'G':
                TestGraphicsBitmap(lcd, pc);
                break;
            case 'C':
                CircleTest(lcd, pc);
                break;
            case 'E':
                EllipseTest(lcd, pc);
                break;
            case 'r':
                pc.printf("Resetting ...\r\n");
                wait_ms(20);
                mbed_reset();
                break;
            case ' ':
                break;
            default:
                printf("huh?\n");
                break;
        }
        if (automode >= 0) {
            automode++;
            if (automode >= sizeof(modelist))
                automode = 0;
            wait_ms(2000);
        }
        wait_ms(200);
    }
}

#endif // TESTENABLE