Guillermo Stedile / RA8875

Forked para SNOCC

Dependencies:   GPS

Dependents:   SNOCC_V1 SNOCC_V2

Fork of RA8875 by SNOCC

Diff: GraphicsDisplay.cpp

Revision:
29:422616aa04bd
Parent:
23:a50ded45dbaf
Child:
31:c72e12cd5c67
--- a/GraphicsDisplay.cpp	Fri Jan 17 17:24:05 2014 +0000
+++ b/GraphicsDisplay.cpp	Sun Jan 19 04:24:16 2014 +0000
@@ -5,121 +5,164 @@
 
 #include "GraphicsDisplay.h"
 
+#define DEBUG "GD"
+// ...
+// INFO("Stuff to show %d", var); // new-line is automatically appended
+//
+#if (defined(DEBUG) && !defined(TARGET_LPC11U24))
+#define INFO(x, ...) std::printf("[INF %s %3d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
+#define WARN(x, ...) std::printf("[WRN %s %3d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
+#define ERR(x, ...)  std::printf("[ERR %s %3d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
+#else
+#define INFO(x, ...)
+#define WARN(x, ...)
+#define ERR(x, ...)
+#endif
+
+// #define LOCALFONT
+
+#ifdef LOCALFONT
 const unsigned char FONT8x8[97][8] = {
-    0x08,0x08,0x08,0x00,0x00,0x00,0x00,0x00, // columns, rows, num_bytes_per_char
-    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // space 0x20
-    0x30,0x78,0x78,0x30,0x30,0x00,0x30,0x00, // !
-    0x6C,0x6C,0x6C,0x00,0x00,0x00,0x00,0x00, // "
-    0x6C,0x6C,0xFE,0x6C,0xFE,0x6C,0x6C,0x00, // #
-    0x18,0x3E,0x60,0x3C,0x06,0x7C,0x18,0x00, // $
-    0x00,0x63,0x66,0x0C,0x18,0x33,0x63,0x00, // %
-    0x1C,0x36,0x1C,0x3B,0x6E,0x66,0x3B,0x00, // &
-    0x30,0x30,0x60,0x00,0x00,0x00,0x00,0x00, // '
-    0x0C,0x18,0x30,0x30,0x30,0x18,0x0C,0x00, // (
-    0x30,0x18,0x0C,0x0C,0x0C,0x18,0x30,0x00, // )
-    0x00,0x66,0x3C,0xFF,0x3C,0x66,0x00,0x00, // *
-    0x00,0x30,0x30,0xFC,0x30,0x30,0x00,0x00, // +
-    0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x30, // ,
-    0x00,0x00,0x00,0x7E,0x00,0x00,0x00,0x00, // -
-    0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00, // .
-    0x03,0x06,0x0C,0x18,0x30,0x60,0x40,0x00, // / (forward slash)
-    0x3E,0x63,0x63,0x6B,0x63,0x63,0x3E,0x00, // 0 0x30
-    0x18,0x38,0x58,0x18,0x18,0x18,0x7E,0x00, // 1
-    0x3C,0x66,0x06,0x1C,0x30,0x66,0x7E,0x00, // 2
-    0x3C,0x66,0x06,0x1C,0x06,0x66,0x3C,0x00, // 3
-    0x0E,0x1E,0x36,0x66,0x7F,0x06,0x0F,0x00, // 4
-    0x7E,0x60,0x7C,0x06,0x06,0x66,0x3C,0x00, // 5
-    0x1C,0x30,0x60,0x7C,0x66,0x66,0x3C,0x00, // 6
-    0x7E,0x66,0x06,0x0C,0x18,0x18,0x18,0x00, // 7
-    0x3C,0x66,0x66,0x3C,0x66,0x66,0x3C,0x00, // 8
-    0x3C,0x66,0x66,0x3E,0x06,0x0C,0x38,0x00, // 9
-    0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x00, // :
-    0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x30, // ;
-    0x0C,0x18,0x30,0x60,0x30,0x18,0x0C,0x00, // <
-    0x00,0x00,0x7E,0x00,0x00,0x7E,0x00,0x00, // =
-    0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x00, // >
-    0x3C,0x66,0x06,0x0C,0x18,0x00,0x18,0x00, // ?
-    0x3E,0x63,0x6F,0x69,0x6F,0x60,0x3E,0x00, // @ 0x40
-    0x18,0x3C,0x66,0x66,0x7E,0x66,0x66,0x00, // A
-    0x7E,0x33,0x33,0x3E,0x33,0x33,0x7E,0x00, // B
-    0x1E,0x33,0x60,0x60,0x60,0x33,0x1E,0x00, // C
-    0x7C,0x36,0x33,0x33,0x33,0x36,0x7C,0x00, // D
-    0x7F,0x31,0x34,0x3C,0x34,0x31,0x7F,0x00, // E
-    0x7F,0x31,0x34,0x3C,0x34,0x30,0x78,0x00, // F
-    0x1E,0x33,0x60,0x60,0x67,0x33,0x1F,0x00, // G
-    0x66,0x66,0x66,0x7E,0x66,0x66,0x66,0x00, // H
-    0x3C,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // I
-    0x0F,0x06,0x06,0x06,0x66,0x66,0x3C,0x00, // J
-    0x73,0x33,0x36,0x3C,0x36,0x33,0x73,0x00, // K
-    0x78,0x30,0x30,0x30,0x31,0x33,0x7F,0x00, // L
-    0x63,0x77,0x7F,0x7F,0x6B,0x63,0x63,0x00, // M
-    0x63,0x73,0x7B,0x6F,0x67,0x63,0x63,0x00, // N
-    0x3E,0x63,0x63,0x63,0x63,0x63,0x3E,0x00, // O
-    0x7E,0x33,0x33,0x3E,0x30,0x30,0x78,0x00, // P 0x50
-    0x3C,0x66,0x66,0x66,0x6E,0x3C,0x0E,0x00, // Q
-    0x7E,0x33,0x33,0x3E,0x36,0x33,0x73,0x00, // R
-    0x3C,0x66,0x30,0x18,0x0C,0x66,0x3C,0x00, // S
-    0x7E,0x5A,0x18,0x18,0x18,0x18,0x3C,0x00, // T
-    0x66,0x66,0x66,0x66,0x66,0x66,0x7E,0x00, // U
-    0x66,0x66,0x66,0x66,0x66,0x3C,0x18,0x00, // V
-    0x63,0x63,0x63,0x6B,0x7F,0x77,0x63,0x00, // W
-    0x63,0x63,0x36,0x1C,0x1C,0x36,0x63,0x00, // X
-    0x66,0x66,0x66,0x3C,0x18,0x18,0x3C,0x00, // Y
-    0x7F,0x63,0x46,0x0C,0x19,0x33,0x7F,0x00, // Z
-    0x3C,0x30,0x30,0x30,0x30,0x30,0x3C,0x00, // [
-    0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00, // \ (back slash)
-    0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x3C,0x00, // ]
-    0x08,0x1C,0x36,0x63,0x00,0x00,0x00,0x00, // ^
-    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF, // _
-    0x18,0x18,0x0C,0x00,0x00,0x00,0x00,0x00, // ` 0x60
-    0x00,0x00,0x3C,0x06,0x3E,0x66,0x3B,0x00, // a
-    0x70,0x30,0x3E,0x33,0x33,0x33,0x6E,0x00, // b
-    0x00,0x00,0x3C,0x66,0x60,0x66,0x3C,0x00, // c
-    0x0E,0x06,0x3E,0x66,0x66,0x66,0x3B,0x00, // d
-    0x00,0x00,0x3C,0x66,0x7E,0x60,0x3C,0x00, // e
-    0x1C,0x36,0x30,0x78,0x30,0x30,0x78,0x00, // f
-    0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x7C, // g
-    0x70,0x30,0x36,0x3B,0x33,0x33,0x73,0x00, // h
-    0x18,0x00,0x38,0x18,0x18,0x18,0x3C,0x00, // i
-    0x06,0x00,0x06,0x06,0x06,0x66,0x66,0x3C, // j
-    0x70,0x30,0x33,0x36,0x3C,0x36,0x73,0x00, // k
-    0x38,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // l
-    0x00,0x00,0x66,0x7F,0x7F,0x6B,0x63,0x00, // m
-    0x00,0x00,0x7C,0x66,0x66,0x66,0x66,0x00, // n
-    0x00,0x00,0x3C,0x66,0x66,0x66,0x3C,0x00, // o
-    0x00,0x00,0x6E,0x33,0x33,0x3E,0x30,0x78, // p
-    0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x0F, // q
-    0x00,0x00,0x6E,0x3B,0x33,0x30,0x78,0x00, // r
-    0x00,0x00,0x3E,0x60,0x3C,0x06,0x7C,0x00, // s
-    0x08,0x18,0x3E,0x18,0x18,0x1A,0x0C,0x00, // t
-    0x00,0x00,0x66,0x66,0x66,0x66,0x3B,0x00, // u
-    0x00,0x00,0x66,0x66,0x66,0x3C,0x18,0x00, // v
-    0x00,0x00,0x63,0x6B,0x7F,0x7F,0x36,0x00, // w
-    0x00,0x00,0x63,0x36,0x1C,0x36,0x63,0x00, // x
-    0x00,0x00,0x66,0x66,0x66,0x3E,0x06,0x7C, // y
-    0x00,0x00,0x7E,0x4C,0x18,0x32,0x7E,0x00, // z
-    0x0E,0x18,0x18,0x70,0x18,0x18,0x0E,0x00, // {
-    0x0C,0x0C,0x0C,0x00,0x0C,0x0C,0x0C,0x00, // |
-    0x70,0x18,0x18,0x0E,0x18,0x18,0x70,0x00, // }
-    0x3B,0x6E,0x00,0x00,0x00,0x00,0x00,0x00, // ~
-    0x1C,0x36,0x36,0x1C,0x00,0x00,0x00,0x00  // DEL
+    0x08, 0x08, 0x08, 0X00, 0X00, 0X00, 0X00, 0X00, // columns, rows, num_bytes_per_char
+    0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, // space 0x20
+    0x30, 0x78, 0x78, 0x30, 0x30, 0X00, 0x30, 0X00, // !
+    0x6C, 0x6C, 0x6C, 0X00, 0X00, 0X00, 0X00, 0X00, // "
+    0x6C, 0x6C, 0xFE, 0x6C, 0xFE, 0x6C, 0x6C, 0X00, // #
+    0x18, 0x3E, 0x60, 0x3C, 0x06, 0x7C, 0x18, 0X00, // $
+    0X00, 0x63, 0x66, 0x0C, 0x18, 0x33, 0x63, 0X00, // %
+    0x1C, 0x36, 0x1C, 0x3B, 0x6E, 0x66, 0x3B, 0X00, // &
+    0x30, 0x30, 0x60, 0X00, 0X00, 0X00, 0X00, 0X00, // '
+    0x0C, 0x18, 0x30, 0x30, 0x30, 0x18, 0x0C, 0X00, // (
+    0x30, 0x18, 0x0C, 0x0C, 0x0C, 0x18, 0x30, 0X00, // )
+    0X00, 0x66, 0x3C, 0xFF, 0x3C, 0x66, 0X00, 0X00, // *
+    0X00, 0x30, 0x30, 0xFC, 0x30, 0x30, 0X00, 0X00, // +
+    0X00, 0X00, 0X00, 0X00, 0X00, 0x18, 0x18, 0x30, // ,
+    0X00, 0X00, 0X00, 0x7E, 0X00, 0X00, 0X00, 0X00, // -
+    0X00, 0X00, 0X00, 0X00, 0X00, 0x18, 0x18, 0X00, // .
+    0x03, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x40, 0X00, // / (forward slash)
+    0x3E, 0x63, 0x63, 0x6B, 0x63, 0x63, 0x3E, 0X00, // 0 0x30
+    0x18, 0x38, 0x58, 0x18, 0x18, 0x18, 0x7E, 0X00, // 1
+    0x3C, 0x66, 0x06, 0x1C, 0x30, 0x66, 0x7E, 0X00, // 2
+    0x3C, 0x66, 0x06, 0x1C, 0x06, 0x66, 0x3C, 0X00, // 3
+    0x0E, 0x1E, 0x36, 0x66, 0x7F, 0x06, 0x0F, 0X00, // 4
+    0x7E, 0x60, 0x7C, 0x06, 0x06, 0x66, 0x3C, 0X00, // 5
+    0x1C, 0x30, 0x60, 0x7C, 0x66, 0x66, 0x3C, 0X00, // 6
+    0x7E, 0x66, 0x06, 0x0C, 0x18, 0x18, 0x18, 0X00, // 7
+    0x3C, 0x66, 0x66, 0x3C, 0x66, 0x66, 0x3C, 0X00, // 8
+    0x3C, 0x66, 0x66, 0x3E, 0x06, 0x0C, 0x38, 0X00, // 9
+    0X00, 0x18, 0x18, 0X00, 0X00, 0x18, 0x18, 0X00, // :
+    0X00, 0x18, 0x18, 0X00, 0X00, 0x18, 0x18, 0x30, // ;
+    0x0C, 0x18, 0x30, 0x60, 0x30, 0x18, 0x0C, 0X00, // <
+    0X00, 0X00, 0x7E, 0X00, 0X00, 0x7E, 0X00, 0X00, // =
+    0x30, 0x18, 0x0C, 0x06, 0x0C, 0x18, 0x30, 0X00, // >
+    0x3C, 0x66, 0x06, 0x0C, 0x18, 0X00, 0x18, 0X00, // ?
+    0x3E, 0x63, 0x6F, 0x69, 0x6F, 0x60, 0x3E, 0X00, // @ 0x40
+    0x18, 0x3C, 0x66, 0x66, 0x7E, 0x66, 0x66, 0X00, // A
+    0x7E, 0x33, 0x33, 0x3E, 0x33, 0x33, 0x7E, 0X00, // B
+    0x1E, 0x33, 0x60, 0x60, 0x60, 0x33, 0x1E, 0X00, // C
+    0x7C, 0x36, 0x33, 0x33, 0x33, 0x36, 0x7C, 0X00, // D
+    0x7F, 0x31, 0x34, 0x3C, 0x34, 0x31, 0x7F, 0X00, // E
+    0x7F, 0x31, 0x34, 0x3C, 0x34, 0x30, 0x78, 0X00, // F
+    0x1E, 0x33, 0x60, 0x60, 0x67, 0x33, 0x1F, 0X00, // G
+    0x66, 0x66, 0x66, 0x7E, 0x66, 0x66, 0x66, 0X00, // H
+    0x3C, 0x18, 0x18, 0x18, 0x18, 0x18, 0x3C, 0X00, // I
+    0x0F, 0x06, 0x06, 0x06, 0x66, 0x66, 0x3C, 0X00, // J
+    0x73, 0x33, 0x36, 0x3C, 0x36, 0x33, 0x73, 0X00, // K
+    0x78, 0x30, 0x30, 0x30, 0x31, 0x33, 0x7F, 0X00, // L
+    0x63, 0x77, 0x7F, 0x7F, 0x6B, 0x63, 0x63, 0X00, // M
+    0x63, 0x73, 0x7B, 0x6F, 0x67, 0x63, 0x63, 0X00, // N
+    0x3E, 0x63, 0x63, 0x63, 0x63, 0x63, 0x3E, 0X00, // O
+    0x7E, 0x33, 0x33, 0x3E, 0x30, 0x30, 0x78, 0X00, // P 0x50
+    0x3C, 0x66, 0x66, 0x66, 0x6E, 0x3C, 0x0E, 0X00, // Q
+    0x7E, 0x33, 0x33, 0x3E, 0x36, 0x33, 0x73, 0X00, // R
+    0x3C, 0x66, 0x30, 0x18, 0x0C, 0x66, 0x3C, 0X00, // S
+    0x7E, 0x5A, 0x18, 0x18, 0x18, 0x18, 0x3C, 0X00, // T
+    0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x7E, 0X00, // U
+    0x66, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x18, 0X00, // V
+    0x63, 0x63, 0x63, 0x6B, 0x7F, 0x77, 0x63, 0X00, // W
+    0x63, 0x63, 0x36, 0x1C, 0x1C, 0x36, 0x63, 0X00, // X
+    0x66, 0x66, 0x66, 0x3C, 0x18, 0x18, 0x3C, 0X00, // Y
+    0x7F, 0x63, 0x46, 0x0C, 0x19, 0x33, 0x7F, 0X00, // Z
+    0x3C, 0x30, 0x30, 0x30, 0x30, 0x30, 0x3C, 0X00, // [
+    0x60, 0x30, 0x18, 0x0C, 0x06, 0x03, 0x01, 0X00, // \ (back slash)
+    0x3C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x3C, 0X00, // ]
+    0x08, 0x1C, 0x36, 0x63, 0X00, 0X00, 0X00, 0X00, // ^
+    0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0xFF, // _
+    0x18, 0x18, 0x0C, 0X00, 0X00, 0X00, 0X00, 0X00, // ` 0x60
+    0X00, 0X00, 0x3C, 0x06, 0x3E, 0x66, 0x3B, 0X00, // a
+    0x70, 0x30, 0x3E, 0x33, 0x33, 0x33, 0x6E, 0X00, // b
+    0X00, 0X00, 0x3C, 0x66, 0x60, 0x66, 0x3C, 0X00, // c
+    0x0E, 0x06, 0x3E, 0x66, 0x66, 0x66, 0x3B, 0X00, // d
+    0X00, 0X00, 0x3C, 0x66, 0x7E, 0x60, 0x3C, 0X00, // e
+    0x1C, 0x36, 0x30, 0x78, 0x30, 0x30, 0x78, 0X00, // f
+    0X00, 0X00, 0x3B, 0x66, 0x66, 0x3E, 0x06, 0x7C, // g
+    0x70, 0x30, 0x36, 0x3B, 0x33, 0x33, 0x73, 0X00, // h
+    0x18, 0X00, 0x38, 0x18, 0x18, 0x18, 0x3C, 0X00, // i
+    0x06, 0X00, 0x06, 0x06, 0x06, 0x66, 0x66, 0x3C, // j
+    0x70, 0x30, 0x33, 0x36, 0x3C, 0x36, 0x73, 0X00, // k
+    0x38, 0x18, 0x18, 0x18, 0x18, 0x18, 0x3C, 0X00, // l
+    0X00, 0X00, 0x66, 0x7F, 0x7F, 0x6B, 0x63, 0X00, // m
+    0X00, 0X00, 0x7C, 0x66, 0x66, 0x66, 0x66, 0X00, // n
+    0X00, 0X00, 0x3C, 0x66, 0x66, 0x66, 0x3C, 0X00, // o
+    0X00, 0X00, 0x6E, 0x33, 0x33, 0x3E, 0x30, 0x78, // p
+    0X00, 0X00, 0x3B, 0x66, 0x66, 0x3E, 0x06, 0x0F, // q
+    0X00, 0X00, 0x6E, 0x3B, 0x33, 0x30, 0x78, 0X00, // r
+    0X00, 0X00, 0x3E, 0x60, 0x3C, 0x06, 0x7C, 0X00, // s
+    0x08, 0x18, 0x3E, 0x18, 0x18, 0x1A, 0x0C, 0X00, // t
+    0X00, 0X00, 0x66, 0x66, 0x66, 0x66, 0x3B, 0X00, // u
+    0X00, 0X00, 0x66, 0x66, 0x66, 0x3C, 0x18, 0X00, // v
+    0X00, 0X00, 0x63, 0x6B, 0x7F, 0x7F, 0x36, 0X00, // w
+    0X00, 0X00, 0x63, 0x36, 0x1C, 0x36, 0x63, 0X00, // x
+    0X00, 0X00, 0x66, 0x66, 0x66, 0x3E, 0x06, 0x7C, // y
+    0X00, 0X00, 0x7E, 0x4C, 0x18, 0x32, 0x7E, 0X00, // z
+    0x0E, 0x18, 0x18, 0x70, 0x18, 0x18, 0x0E, 0X00, // {
+    0x0C, 0x0C, 0x0C, 0X00, 0x0C, 0x0C, 0x0C, 0X00, // |
+    0x70, 0x18, 0x18, 0x0E, 0x18, 0x18, 0x70, 0X00, // }
+    0x3B, 0x6E, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, // ~
+    0x1C, 0x36, 0x36, 0x1C, 0X00, 0X00, 0X00, 0X00  // DEL
 }; 
-
+#endif // LOCALFONT
 
-GraphicsDisplay::GraphicsDisplay(const char *name):TextDisplay(name)
+GraphicsDisplay::GraphicsDisplay(const char *name) 
+    : TextDisplay(name)
 {
+    font = NULL;
     //foreground(0xFFFF);
     //background(0x0000);
 }
 
-
-void GraphicsDisplay::character(int column, int row, int value)
+RetCode_t GraphicsDisplay::set_font(const unsigned char * _font)
 {
-    blitbit(column * 8, row * 8, 8, 8, (char*)&(FONT8x8[value - 0x1F][0]));
+    font = _font;
+    INFO("set_font(%lu) %lu", _font, font);
+    return noerror;     // trusting them, but it might be good to put some checks in here...
 }
 
+#ifdef LOCALFONT
+int GraphicsDisplay::character(int x, int y, int value)
+{
+    INFO("character(%d,%d,%c)", x, t, value);
+    if (value <= 0x1F && value >= 7F)
+        return 0;
+    
+    return blitbit(x, y, FONT8X8[0][0], FONT8X8[0][1], 
+        (char *)&(FONT8x8[value - 0x1F][0]));
+}
+#else
+int GraphicsDisplay::character(int x, int y, int c)
+{
+    unsigned int offset;
+    const unsigned char * charRecord;
+ 
+    if (c <= 0x1F || c >= 0x7F)
+        return 0;
+    offset = font[0];                    // bytes / char
+    charRecord = &font[((c - ' ') * offset) + 4];      // start of char bitmap
+    return fontblit(x, y, font, charRecord);
+}
+#endif
 
-void GraphicsDisplay::window(unsigned int x,unsigned  int y,unsigned  int w,unsigned  int h)
+void GraphicsDisplay::window(unsigned int x, unsigned int y, unsigned int w, unsigned int h)
 {
     // current pixel location
     _x = x;
@@ -160,7 +203,7 @@
     return noerror;
 }
 
-void GraphicsDisplay::blit(int x, int y, int w, int h, const int *colour)
+void GraphicsDisplay::blit(int x, int y, int w, int h, const int * colour)
 {
     window(x, y, w, h);
     for(int i=0; i<w*h; i++) {
@@ -168,19 +211,53 @@
     }
 }
 
-void GraphicsDisplay::blitbit(int x, int y, int w, int h, const char* colour)
+int GraphicsDisplay::blitbit(int x, int y, int w, int h, const char * colour)
 {
+    _foreground = 0xFFFF;
+    INFO("blitbit(%d,%d, %d,%d, %02X) [%04X,%04X]", x,y, w,h, *colour, _foreground, _background);
+    INFO("%lu  %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
+        colour,
+        colour[0], colour[1], colour[2], colour[3], colour[4], colour[5], colour[6], colour[7], 
+        colour[8], colour[9], colour[10], colour[11], colour[12], colour[13], colour[14], colour[15]);
     window(x, y, w, h);
-    for(int i = 0; i < w*h; i++) {
+    for (int i = 0; i < w*h; i++) {
         char byte = colour[i >> 3];
         int offset = i & 0x7;
+        if (offset == 0)
+            INFO(" %2d = %02X", i>>3, byte);
         int c = ((byte << offset) & 0x80) ? _foreground : _background;
         putp(c);
+        //wait_ms(100);
     }
+    return w;
+}
+
+
+int GraphicsDisplay::fontblit(int x, int y, const unsigned char * fontTable, const unsigned char * fontChar)
+{
     _foreground = 0xFFFF;
-    printf("blitbit colors %d,%d\r\n", _foreground, _background);
+    //int fontWidth = font[1];                       // get hor size of font
+    int fontHeight = font[2];                      // get vert size of font
+    int bytesPerLine = font[3];                       // bytes per line
+    int charWidth = fontChar[0];        // width of this character
+    int px, py;
+    window(x, y, charWidth, fontHeight);
+    
+    for (py = 0; py < fontHeight; py++) {
+        int bitmask = 1 << (py & 7);
+        
+        for (px = 0; px < charWidth; px++) {
+            int offset = (py / 8) + px * bytesPerLine;
+            unsigned char byte = fontChar[offset + 1];  // skip the char's # bits wide value
+            int c = (byte & bitmask) ? _foreground : _background;
+            putp(c);
+        }
+    }
+    return charWidth;
 }
 
+
+
 int GraphicsDisplay::columns()
 {
     return width() / 8;