/*
 Adapted from the Adafruit and Xark's PDQ graphicstest sketch.

 See end of file for original header text and MIT license info.
 
 This sketch uses the GLCD font only.

 Make sure all the display driver and pin comnenctions are correct by
 editting the User_Setup.h file in the TFT_eSPI library folder.

 #########################################################################
 ###### DON'T FORGET TO UPDATE THE User_Setup.h FILE IN THE LIBRARY ######
 #########################################################################
 */

#include <M5Stack.h>

unsigned long total = 0;
unsigned long tn = 0;

void setup() {
	M5.begin();
	M5.Power.begin();
	M5.Lcd.setRotation(0);
	Serial.println(""); Serial.println("");
	Serial.println("Bodmer's M5Stack library Test!"); 
}

void loop(void)
{

	Serial.println(F("Benchmark                Time (microseconds)"));

	uint32_t usecHaD = testHaD();
	Serial.print(F("HaD pushColor            "));
	Serial.println(usecHaD);
	delay(100);

	uint32_t usecFillScreen = testFillScreen();
	Serial.print(F("Screen fill              "));
	Serial.println(usecFillScreen);
	delay(100);

	uint32_t usecText = testText();
	Serial.print(F("Text                     "));
	Serial.println(usecText);
	delay(100);

	uint32_t usecPixels = testPixels();
	Serial.print(F("Pixels                   "));
	Serial.println(usecPixels);
	delay(100);

	uint32_t usecLines = testLines(TFT_BLUE);
	Serial.print(F("Lines                    "));
	Serial.println(usecLines);
	delay(100);

	uint32_t usecFastLines = testFastLines(TFT_RED, TFT_BLUE);
	Serial.print(F("Horiz/Vert Lines         "));
	Serial.println(usecFastLines);
	delay(100);

	uint32_t usecRects = testRects(TFT_GREEN);
	Serial.print(F("Rectangles (outline)     "));
	Serial.println(usecRects);
	delay(100);

	uint32_t usecFilledRects = testFilledRects(TFT_YELLOW, TFT_MAGENTA);
	Serial.print(F("Rectangles (filled)      "));
	Serial.println(usecFilledRects);
	delay(100);

	uint32_t usecFilledCircles = testFilledCircles(10, TFT_MAGENTA);
	Serial.print(F("Circles (filled)         "));
	Serial.println(usecFilledCircles);
	delay(100);

	uint32_t usecCircles = testCircles(10, TFT_WHITE);
	Serial.print(F("Circles (outline)        "));
	Serial.println(usecCircles);
	delay(100);

	uint32_t usecTriangles = testTriangles();
	Serial.print(F("Triangles (outline)      "));
	Serial.println(usecTriangles);
	delay(100);

	uint32_t usecFilledTrangles = testFilledTriangles();
	Serial.print(F("Triangles (filled)       "));
	Serial.println(usecFilledTrangles);
	delay(100);

	uint32_t usecRoundRects = testRoundRects();
	Serial.print(F("Rounded rects (outline)  "));
	Serial.println(usecRoundRects);
	delay(100);

	uint32_t usedFilledRoundRects = testFilledRoundRects();
	Serial.print(F("Rounded rects (filled)   "));
	Serial.println(usedFilledRoundRects);
	delay(100);

	Serial.println(F("Done!"));

	uint16_t c = 4;
	int8_t d = 1;
	for (int32_t i = 0; i < M5.Lcd.height(); i++)
	{
		M5.Lcd.drawFastHLine(0, i, M5.Lcd.width(), c);
		c += d;
		if (c <= 4 || c >= 11)
			d = -d;
	}
	
	M5.Lcd.setCursor(0, 0);
	M5.Lcd.setTextColor(TFT_MAGENTA);
	M5.Lcd.setTextSize(2);

 	M5.Lcd.println(F("   M5Stack test"));

	M5.Lcd.setTextSize(1);
	M5.Lcd.setTextColor(TFT_WHITE);
	M5.Lcd.println(F(""));
	M5.Lcd.setTextSize(1);
	M5.Lcd.println(F(""));
	M5.Lcd.setTextColor(M5.Lcd.color565(0x80, 0x80, 0x80));

	// M5.Lcd.println(F(""));


	M5.Lcd.setTextColor(TFT_GREEN);
	M5.Lcd.println(F(" Benchmark               microseconds"));
	M5.Lcd.println(F(""));
	M5.Lcd.setTextColor(TFT_YELLOW);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("HaD pushColor      "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecHaD);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Screen fill        "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecFillScreen);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Text               "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecText);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Pixels             "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecPixels);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Lines              "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecLines);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Horiz/Vert Lines   "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecFastLines);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Rectangles         "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecRects);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Rectangles-filled  "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecFilledRects);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Circles            "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecCircles);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Circles-filled     "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecFilledCircles);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Triangles          "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecTriangles);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Triangles-filled   "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecFilledTrangles);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Rounded rects      "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usecRoundRects);

	M5.Lcd.setTextColor(TFT_CYAN); M5.Lcd.setTextSize(1);
	M5.Lcd.print(F("Rounded rects-fill "));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	printnice(usedFilledRoundRects);

	M5.Lcd.setTextSize(1);
	M5.Lcd.println(F(""));
	M5.Lcd.setTextColor(TFT_GREEN); M5.Lcd.setTextSize(2);
	M5.Lcd.print(F("Benchmark Complete!"));

	delay(60 * 1000L);
}

void printnice(int32_t v)
{
	char	str[32] = { 0 };
	sprintf(str, "%lu", v);
	for (char *p = (str+strlen(str))-3; p > str; p -= 3)
	{
		memmove(p+1, p, strlen(p)+1);
		*p = ',';
		
	}
	while (strlen(str) < 10)
	{
		memmove(str+1, str, strlen(str)+1);
		*str = ' ';
	}
	M5.Lcd.println(str);
}

static inline uint32_t micros_start() __attribute__ ((always_inline));
static inline uint32_t micros_start()
{
	uint8_t oms = millis();
	while ((uint8_t)millis() == oms)
		;
	return micros();
}

uint32_t testHaD()
{
	// pseudo-code for cheesy RLE
	// start with color1
	// while more input data remaining
	// 	count =  0nnnnnnn = 1 byte or 1nnnnnnn nnnnnnnn 2 bytes (0 - 32767)
	// 	repeat color count times
	// 	toggle color1/color2
	static const uint8_t HaD_240x320[] PROGMEM =
	{
		0xb9, 0x50, 0x0e, 0x80, 0x93, 0x0e, 0x41, 0x11, 0x80, 0x8d, 0x11, 0x42, 0x12, 0x80, 0x89, 0x12, 
		0x45, 0x12, 0x80, 0x85, 0x12, 0x48, 0x12, 0x80, 0x83, 0x12, 0x4a, 0x13, 0x7f, 0x13, 0x4c, 0x13, 
		0x7d, 0x13, 0x4e, 0x13, 0x7b, 0x13, 0x50, 0x13, 0x79, 0x13, 0x52, 0x13, 0x77, 0x13, 0x54, 0x13, 
		0x75, 0x13, 0x57, 0x11, 0x75, 0x11, 0x5a, 0x11, 0x73, 0x11, 0x5c, 0x11, 0x71, 0x11, 0x5e, 0x10, 
		0x71, 0x10, 0x60, 0x10, 0x6f, 0x10, 0x61, 0x10, 0x6f, 0x10, 0x60, 0x11, 0x6f, 0x11, 0x5e, 0x13, 
		0x6d, 0x13, 0x5c, 0x14, 0x6d, 0x14, 0x5a, 0x15, 0x6d, 0x15, 0x58, 0x17, 0x6b, 0x17, 0x37, 0x01, 
		0x1f, 0x17, 0x6b, 0x17, 0x1f, 0x01, 0x17, 0x02, 0x1d, 0x18, 0x6b, 0x18, 0x1d, 0x02, 0x17, 0x03, 
		0x1b, 0x19, 0x6b, 0x19, 0x1b, 0x03, 0x17, 0x05, 0x18, 0x1a, 0x6b, 0x1a, 0x18, 0x05, 0x17, 0x06, 
		0x16, 0x1b, 0x6b, 0x1b, 0x16, 0x06, 0x17, 0x07, 0x14, 0x1c, 0x6b, 0x1c, 0x14, 0x07, 0x17, 0x08, 
		0x12, 0x1d, 0x6b, 0x1d, 0x12, 0x08, 0x17, 0x09, 0x10, 0x1e, 0x6b, 0x1e, 0x10, 0x09, 0x17, 0x0a, 
		0x0e, 0x1f, 0x6b, 0x1f, 0x0e, 0x0a, 0x17, 0x0b, 0x0c, 0x20, 0x6b, 0x20, 0x0c, 0x0b, 0x17, 0x0c, 
		0x0b, 0x21, 0x69, 0x21, 0x0b, 0x0c, 0x18, 0x0d, 0x08, 0x23, 0x67, 0x23, 0x08, 0x0d, 0x19, 0x0e, 
		0x06, 0x26, 0x63, 0x26, 0x06, 0x0e, 0x19, 0x0f, 0x04, 0x28, 0x61, 0x28, 0x04, 0x0f, 0x19, 0x10, 
		0x02, 0x2a, 0x5f, 0x2a, 0x02, 0x10, 0x1a, 0x3c, 0x5d, 0x3c, 0x1b, 0x3d, 0x5b, 0x3d, 0x1c, 0x3d, 
		0x59, 0x3d, 0x1d, 0x3e, 0x57, 0x3e, 0x1e, 0x3e, 0x55, 0x3e, 0x1f, 0x40, 0x51, 0x40, 0x20, 0x40, 
		0x4f, 0x40, 0x22, 0x40, 0x22, 0x09, 0x22, 0x40, 0x24, 0x40, 0x1a, 0x17, 0x1a, 0x40, 0x26, 0x40, 
		0x16, 0x1d, 0x16, 0x40, 0x28, 0x40, 0x12, 0x23, 0x12, 0x40, 0x2a, 0x40, 0x0f, 0x27, 0x0f, 0x40, 
		0x2c, 0x41, 0x0b, 0x2b, 0x0b, 0x41, 0x2f, 0x3f, 0x09, 0x2f, 0x09, 0x3f, 0x32, 0x3d, 0x08, 0x33, 
		0x08, 0x3d, 0x35, 0x3a, 0x08, 0x35, 0x08, 0x3a, 0x3a, 0x36, 0x07, 0x39, 0x07, 0x36, 0x41, 0x09, 
		0x05, 0x23, 0x07, 0x3b, 0x07, 0x23, 0x05, 0x09, 0x54, 0x21, 0x07, 0x3d, 0x07, 0x21, 0x64, 0x1f, 
		0x06, 0x41, 0x06, 0x1f, 0x66, 0x1d, 0x06, 0x43, 0x06, 0x1d, 0x68, 0x1b, 0x06, 0x45, 0x06, 0x1b, 
		0x6b, 0x18, 0x06, 0x47, 0x06, 0x18, 0x6e, 0x16, 0x06, 0x49, 0x06, 0x16, 0x70, 0x14, 0x06, 0x4b, 
		0x06, 0x14, 0x72, 0x13, 0x06, 0x4b, 0x06, 0x13, 0x74, 0x11, 0x06, 0x4d, 0x06, 0x11, 0x76, 0x0f, 
		0x06, 0x4f, 0x06, 0x0f, 0x78, 0x0e, 0x05, 0x51, 0x05, 0x0e, 0x7a, 0x0c, 0x06, 0x51, 0x06, 0x0c, 
		0x7d, 0x09, 0x06, 0x53, 0x06, 0x09, 0x80, 0x80, 0x08, 0x05, 0x55, 0x05, 0x08, 0x80, 0x82, 0x06, 
		0x05, 0x57, 0x05, 0x06, 0x80, 0x84, 0x05, 0x05, 0x57, 0x05, 0x05, 0x80, 0x86, 0x03, 0x05, 0x59, 
		0x05, 0x03, 0x80, 0x88, 0x02, 0x05, 0x59, 0x05, 0x02, 0x80, 0x8f, 0x5b, 0x80, 0x95, 0x5b, 0x80, 
		0x94, 0x5d, 0x80, 0x93, 0x5d, 0x80, 0x92, 0x5e, 0x80, 0x92, 0x5f, 0x80, 0x91, 0x5f, 0x80, 0x90, 
		0x61, 0x80, 0x8f, 0x61, 0x80, 0x8f, 0x61, 0x80, 0x8e, 0x63, 0x80, 0x8d, 0x63, 0x80, 0x8d, 0x63, 
		0x80, 0x8d, 0x63, 0x80, 0x8c, 0x19, 0x07, 0x25, 0x07, 0x19, 0x80, 0x8b, 0x16, 0x0d, 0x1f, 0x0d, 
		0x16, 0x80, 0x8b, 0x14, 0x11, 0x1b, 0x11, 0x14, 0x80, 0x8b, 0x13, 0x13, 0x19, 0x13, 0x13, 0x80, 
		0x8b, 0x12, 0x15, 0x17, 0x15, 0x12, 0x80, 0x8a, 0x12, 0x17, 0x15, 0x17, 0x12, 0x80, 0x89, 0x11, 
		0x19, 0x13, 0x19, 0x11, 0x80, 0x89, 0x11, 0x19, 0x13, 0x19, 0x11, 0x80, 0x89, 0x10, 0x1b, 0x11, 
		0x1b, 0x10, 0x80, 0x89, 0x0f, 0x1c, 0x11, 0x1c, 0x0f, 0x80, 0x89, 0x0f, 0x1c, 0x11, 0x1c, 0x0f, 
		0x80, 0x89, 0x0f, 0x1c, 0x11, 0x1c, 0x0f, 0x80, 0x89, 0x0e, 0x1d, 0x11, 0x1d, 0x0e, 0x80, 0x89, 
		0x0e, 0x1c, 0x13, 0x1c, 0x0e, 0x80, 0x89, 0x0e, 0x1b, 0x15, 0x1b, 0x0e, 0x80, 0x89, 0x0e, 0x1b, 
		0x15, 0x1b, 0x0e, 0x80, 0x89, 0x0e, 0x1a, 0x17, 0x1a, 0x0e, 0x80, 0x89, 0x0e, 0x18, 0x1b, 0x18, 
		0x0e, 0x80, 0x89, 0x0e, 0x16, 0x1f, 0x16, 0x0e, 0x80, 0x89, 0x0e, 0x14, 0x23, 0x14, 0x0e, 0x80, 
		0x89, 0x0f, 0x11, 0x27, 0x11, 0x0f, 0x80, 0x89, 0x0f, 0x0e, 0x2d, 0x0e, 0x0f, 0x80, 0x89, 0x0f, 
		0x0c, 0x31, 0x0c, 0x0f, 0x80, 0x89, 0x0f, 0x0b, 0x33, 0x0b, 0x0f, 0x80, 0x8a, 0x0f, 0x09, 0x35, 
		0x09, 0x0f, 0x80, 0x8b, 0x10, 0x08, 0x35, 0x08, 0x10, 0x80, 0x8b, 0x10, 0x07, 0x37, 0x07, 0x10, 
		0x80, 0x8b, 0x11, 0x06, 0x37, 0x06, 0x11, 0x80, 0x8b, 0x12, 0x05, 0x37, 0x05, 0x12, 0x80, 0x8c, 
		0x13, 0x03, 0x1b, 0x01, 0x1b, 0x03, 0x13, 0x80, 0x8d, 0x30, 0x03, 0x30, 0x80, 0x8d, 0x30, 0x04, 
		0x2f, 0x80, 0x8d, 0x2f, 0x05, 0x2f, 0x80, 0x8e, 0x2e, 0x06, 0x2d, 0x80, 0x8f, 0x2d, 0x07, 0x2d, 
		0x80, 0x8f, 0x2d, 0x07, 0x2d, 0x80, 0x90, 0x2c, 0x08, 0x2b, 0x80, 0x91, 0x2b, 0x09, 0x2b, 0x80, 
		0x8c, 0x01, 0x05, 0x2a, 0x09, 0x2a, 0x05, 0x01, 0x80, 0x85, 0x03, 0x05, 0x2a, 0x09, 0x2a, 0x05, 
		0x03, 0x80, 0x82, 0x04, 0x05, 0x2a, 0x09, 0x2a, 0x04, 0x05, 0x80, 0x80, 0x06, 0x05, 0x29, 0x04, 
		0x02, 0x03, 0x29, 0x05, 0x06, 0x7e, 0x07, 0x05, 0x29, 0x03, 0x03, 0x03, 0x29, 0x05, 0x07, 0x7c, 
		0x09, 0x05, 0x28, 0x02, 0x05, 0x02, 0x28, 0x05, 0x09, 0x7a, 0x0a, 0x05, 0x28, 0x02, 0x05, 0x02, 
		0x28, 0x05, 0x0a, 0x78, 0x0c, 0x05, 0x27, 0x02, 0x05, 0x02, 0x27, 0x05, 0x0c, 0x76, 0x0d, 0x06, 
		0x26, 0x01, 0x07, 0x01, 0x26, 0x06, 0x0d, 0x73, 0x10, 0x05, 0x55, 0x05, 0x10, 0x70, 0x12, 0x05, 
		0x53, 0x05, 0x12, 0x6e, 0x13, 0x06, 0x51, 0x06, 0x13, 0x6c, 0x15, 0x05, 0x51, 0x05, 0x15, 0x6a, 
		0x16, 0x06, 0x4f, 0x06, 0x16, 0x68, 0x18, 0x06, 0x4d, 0x06, 0x18, 0x66, 0x1a, 0x06, 0x4b, 0x06, 
		0x1a, 0x64, 0x1c, 0x06, 0x49, 0x06, 0x1c, 0x55, 0x07, 0x05, 0x1e, 0x06, 0x49, 0x06, 0x1e, 0x05, 
		0x07, 0x42, 0x30, 0x06, 0x47, 0x06, 0x30, 0x3a, 0x34, 0x06, 0x45, 0x06, 0x34, 0x35, 0x37, 0x06, 
		0x43, 0x06, 0x37, 0x32, 0x39, 0x07, 0x3f, 0x07, 0x39, 0x2f, 0x3c, 0x07, 0x3d, 0x07, 0x3c, 0x2c, 
		0x3e, 0x07, 0x3b, 0x07, 0x3e, 0x2a, 0x40, 0x06, 0x3b, 0x06, 0x40, 0x28, 0x40, 0x06, 0x3c, 0x07, 
		0x40, 0x26, 0x3f, 0x08, 0x3d, 0x08, 0x3f, 0x24, 0x3f, 0x09, 0x3d, 0x09, 0x3f, 0x22, 0x3f, 0x0a, 
		0x14, 0x01, 0x13, 0x02, 0x13, 0x0a, 0x3f, 0x20, 0x3f, 0x0b, 0x14, 0x01, 0x13, 0x02, 0x13, 0x0b, 
		0x3f, 0x1f, 0x3e, 0x0c, 0x14, 0x01, 0x13, 0x02, 0x13, 0x0c, 0x3e, 0x1e, 0x3e, 0x0d, 0x13, 0x02, 
		0x13, 0x02, 0x13, 0x0d, 0x3e, 0x1d, 0x3d, 0x0e, 0x13, 0x02, 0x13, 0x02, 0x13, 0x0e, 0x3d, 0x1c, 
		0x3c, 0x11, 0x11, 0x04, 0x11, 0x04, 0x11, 0x11, 0x3c, 0x1b, 0x10, 0x01, 0x2a, 0x12, 0x11, 0x04, 
		0x11, 0x04, 0x11, 0x12, 0x2a, 0x01, 0x10, 0x1a, 0x0f, 0x04, 0x28, 0x14, 0x0f, 0x06, 0x0f, 0x06, 
		0x0f, 0x14, 0x28, 0x04, 0x0f, 0x19, 0x0e, 0x06, 0x26, 0x16, 0x0d, 0x08, 0x0d, 0x08, 0x0d, 0x16, 
		0x26, 0x06, 0x0e, 0x19, 0x0d, 0x07, 0x25, 0x18, 0x0b, 0x0a, 0x0b, 0x0a, 0x0b, 0x18, 0x25, 0x07, 
		0x0d, 0x19, 0x0c, 0x09, 0x23, 0x1c, 0x06, 0x0f, 0x05, 0x10, 0x05, 0x1c, 0x23, 0x09, 0x0c, 0x18, 
		0x0c, 0x0b, 0x21, 0x69, 0x21, 0x0b, 0x0c, 0x17, 0x0b, 0x0d, 0x1f, 0x6b, 0x1f, 0x0d, 0x0b, 0x17, 
		0x0a, 0x0f, 0x1e, 0x6b, 0x1e, 0x0f, 0x0a, 0x17, 0x09, 0x11, 0x1d, 0x6b, 0x1d, 0x11, 0x09, 0x17, 
		0x07, 0x14, 0x1c, 0x6b, 0x1c, 0x14, 0x07, 0x17, 0x06, 0x16, 0x1b, 0x6b, 0x1b, 0x16, 0x06, 0x17, 
		0x05, 0x18, 0x1a, 0x6b, 0x1a, 0x18, 0x05, 0x17, 0x04, 0x1a, 0x19, 0x6b, 0x19, 0x1a, 0x04, 0x17, 
		0x03, 0x1b, 0x19, 0x6b, 0x19, 0x1b, 0x03, 0x17, 0x02, 0x1d, 0x18, 0x6b, 0x18, 0x1d, 0x02, 0x37, 
		0x17, 0x6b, 0x17, 0x58, 0x16, 0x6b, 0x16, 0x5a, 0x14, 0x6d, 0x14, 0x5c, 0x13, 0x6d, 0x13, 0x5e, 
		0x12, 0x6d, 0x12, 0x60, 0x10, 0x6f, 0x10, 0x61, 0x10, 0x6f, 0x10, 0x60, 0x11, 0x6f, 0x11, 0x5e, 
		0x11, 0x71, 0x11, 0x5c, 0x12, 0x71, 0x12, 0x5a, 0x12, 0x73, 0x12, 0x58, 0x12, 0x75, 0x12, 0x56, 
		0x13, 0x75, 0x13, 0x54, 0x13, 0x77, 0x13, 0x51, 0x14, 0x79, 0x14, 0x4e, 0x14, 0x7b, 0x14, 0x4c, 
		0x14, 0x7d, 0x14, 0x4a, 0x14, 0x7f, 0x14, 0x48, 0x13, 0x80, 0x83, 0x13, 0x46, 0x13, 0x80, 0x85, 
		0x13, 0x44, 0x12, 0x80, 0x89, 0x12, 0x42, 0x11, 0x80, 0x8d, 0x11, 0x40, 0x0f, 0x80, 0x93, 0x0f, 
		0x45, 0x04, 0x80, 0x9d, 0x04, 0xb9, 0x56, 
	};
	
	M5.Lcd.fillScreen(TFT_BLACK);

	uint32_t start = micros_start();
	
	for (int i = 0; i < 0x10; i++)
	{
		M5.Lcd.setAddrWindow(0, 0, 240, 320);

		uint16_t cnt = 0;
		uint16_t color = M5.Lcd.color565((i << 4) | i, (i << 4) | i, (i << 4) | i);
		uint16_t curcolor = 0;

		const uint8_t *cmp = &HaD_240x320[0];

		while (cmp < &HaD_240x320[sizeof(HaD_240x320)])
		{
			cnt = pgm_read_byte(cmp++);
			if (cnt & 0x80) cnt = ((cnt & 0x7f) << 8) | pgm_read_byte(cmp++);
			M5.Lcd.pushColor(curcolor, cnt);	// PDQ_GFX has count
			curcolor ^= color;
		}
	}

	uint32_t t = micros() - start;

	M5.Lcd.setTextColor(TFT_YELLOW);
	M5.Lcd.setTextSize(2);
	M5.Lcd.setCursor(8, 285);
	M5.Lcd.print(F("http://hackaday.io/"));
	M5.Lcd.setCursor(96, 302);
	M5.Lcd.print(F("Xark"));

	delay(3 * 1000L);
	
	return t;
}

uint32_t testFillScreen()
{
	uint32_t start = micros_start();
    // Shortened this tedious test!
		M5.Lcd.fillScreen(TFT_WHITE);
		M5.Lcd.fillScreen(TFT_RED);
		M5.Lcd.fillScreen(TFT_GREEN);
		M5.Lcd.fillScreen(TFT_BLUE);
		M5.Lcd.fillScreen(TFT_BLACK);

	return (micros() - start)/5;
}

uint32_t testText()
{
	M5.Lcd.fillScreen(TFT_BLACK);
	uint32_t start = micros_start();
	M5.Lcd.setCursor(0, 0);
	M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);	M5.Lcd.setTextSize(1);
	M5.Lcd.println(F("Hello World!"));
	M5.Lcd.setTextSize(2);
	M5.Lcd.setTextColor(M5.Lcd.color565(0xff, 0x00, 0x00));
	M5.Lcd.print(F("RED "));
	M5.Lcd.setTextColor(M5.Lcd.color565(0x00, 0xff, 0x00));
	M5.Lcd.print(F("GREEN "));
	M5.Lcd.setTextColor(M5.Lcd.color565(0x00, 0x00, 0xff));
	M5.Lcd.println(F("BLUE"));
	M5.Lcd.setTextColor(TFT_YELLOW); M5.Lcd.setTextSize(2);
	M5.Lcd.println(1234.56);
	M5.Lcd.setTextColor(TFT_RED);		M5.Lcd.setTextSize(3);
	M5.Lcd.println(0xDEADBEEF, HEX);
	M5.Lcd.println();
	M5.Lcd.setTextColor(TFT_GREEN);
	M5.Lcd.setTextSize(5);
	M5.Lcd.println(F("Groop"));
	M5.Lcd.setTextSize(2);
	M5.Lcd.println(F("I implore thee,"));
  M5.Lcd.setTextColor(TFT_GREEN);
	M5.Lcd.setTextSize(1);
	M5.Lcd.println(F("my foonting turlingdromes."));
	M5.Lcd.println(F("And hooptiously drangle me"));
	M5.Lcd.println(F("with crinkly bindlewurdles,"));
	M5.Lcd.println(F("Or I will rend thee"));
	M5.Lcd.println(F("in the gobberwarts"));
	M5.Lcd.println(F("with my blurglecruncheon,"));
	M5.Lcd.println(F("see if I don't!"));
	M5.Lcd.println(F(""));
	M5.Lcd.println(F(""));
	M5.Lcd.setTextColor(TFT_MAGENTA);
	M5.Lcd.setTextSize(6);
	M5.Lcd.println(F("Woot!"));
	uint32_t t = micros() - start;
	delay(1000);
	return t;
}

uint32_t testPixels()
{
	int32_t	w = M5.Lcd.width();
	int32_t	h = M5.Lcd.height();

	uint32_t start = micros_start();

	for (uint16_t y = 0; y < h; y++)
	{
		for (uint16_t x = 0; x < w; x++)
		{
			M5.Lcd.drawPixel(x, y, M5.Lcd.color565(x<<3, y<<3, x*y));
		}
	}
	
	return micros() - start;
}


uint32_t testLines(uint16_t color)
{
	uint32_t start, t;
	int32_t	x1, y1, x2, y2;
	int32_t	w = M5.Lcd.width();
	int32_t	h = M5.Lcd.height();

	M5.Lcd.fillScreen(TFT_BLACK);

	x1 = y1 = 0;
	y2 = h - 1;

	start = micros_start();

	for (x2 = 0; x2 < w; x2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	x2 = w - 1;

	for (y2 = 0; y2 < h; y2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	t = micros() - start; // fillScreen doesn't count against timing

	M5.Lcd.fillScreen(TFT_BLACK);

	x1 = w - 1;
	y1 = 0;
	y2 = h - 1;

	start = micros_start();

	for (x2 = 0; x2 < w; x2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	x2 = 0;
	for (y2 = 0; y2 < h; y2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	t += micros() - start;

	M5.Lcd.fillScreen(TFT_BLACK);

	x1 = 0;
	y1 = h - 1;
	y2 = 0;

	start = micros_start();

	for (x2 = 0; x2 < w; x2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}
	x2 = w - 1;
	for (y2 = 0; y2 < h; y2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}
	t += micros() - start;

	M5.Lcd.fillScreen(TFT_BLACK);

	x1 = w - 1;
	y1 = h - 1;
	y2 = 0;

	start = micros_start();

	for (x2 = 0; x2 < w; x2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	x2 = 0;
	for (y2 = 0; y2 < h; y2 += 6)
	{
		M5.Lcd.drawLine(x1, y1, x2, y2, color);
	}

	t += micros() - start;

	return t;
}

uint32_t testFastLines(uint16_t color1, uint16_t color2)
{
	uint32_t start;
	int32_t x, y;
	int32_t w = M5.Lcd.width();
	int32_t h = M5.Lcd.height();

	M5.Lcd.fillScreen(TFT_BLACK);

	start = micros_start();

	for (y = 0; y < h; y += 5)
		M5.Lcd.drawFastHLine(0, y, w, color1);
	for (x = 0; x < w; x += 5)
		M5.Lcd.drawFastVLine(x, 0, h, color2);

	return micros() - start;
}

uint32_t testRects(uint16_t color)
{
	uint32_t start;
	int32_t n, i, i2;
	int32_t cx = M5.Lcd.width() / 2;
	int32_t cy = M5.Lcd.height() / 2;

	M5.Lcd.fillScreen(TFT_BLACK);
	n = min(M5.Lcd.width(), M5.Lcd.height());
	start = micros_start();
	for (i = 2; i < n; i += 6)
	{
		i2 = i / 2;
		M5.Lcd.drawRect(cx-i2, cy-i2, i, i, color);
	}

	return micros() - start;
}

uint32_t testFilledRects(uint16_t color1, uint16_t color2)
{
	uint32_t start, t = 0;
	int32_t n, i, i2;
	int32_t cx = M5.Lcd.width() / 2 - 1;
	int32_t cy = M5.Lcd.height() / 2 - 1;

	M5.Lcd.fillScreen(TFT_BLACK);
	n = min(M5.Lcd.width(), M5.Lcd.height());
	for (i = n; i > 0; i -= 6)
	{
		i2 = i / 2;

		start = micros_start();

		M5.Lcd.fillRect(cx-i2, cy-i2, i, i, color1);

		t += micros() - start;

		// Outlines are not included in timing results
		M5.Lcd.drawRect(cx-i2, cy-i2, i, i, color2);
	}

	return t;
}

uint32_t testFilledCircles(uint8_t radius, uint16_t color)
{
	uint32_t start;
	int32_t x, y, w = M5.Lcd.width(), h = M5.Lcd.height(), r2 = radius * 2;

	M5.Lcd.fillScreen(TFT_BLACK);

	start = micros_start();

	for (x = radius; x < w; x += r2)
	{
		for (y = radius; y < h; y += r2)
		{
			M5.Lcd.fillCircle(x, y, radius, color);
		}
	}

	return micros() - start;
}

uint32_t testCircles(uint8_t radius, uint16_t color)
{
	uint32_t start;
	int32_t x, y, r2 = radius * 2;
	int32_t w = M5.Lcd.width() + radius;
	int32_t h = M5.Lcd.height() + radius;

	// Screen is not cleared for this one -- this is
	// intentional and does not affect the reported time.
	start = micros_start();

	for (x = 0; x < w; x += r2)
	{
		for (y = 0; y < h; y += r2)
		{
			M5.Lcd.drawCircle(x, y, radius, color);
		}
	}

	return micros() - start;
}

uint32_t testTriangles()
{
	uint32_t start;
	int32_t n, i;
	int32_t cx = M5.Lcd.width()/ 2 - 1;
	int32_t cy = M5.Lcd.height() / 2 - 1;

	M5.Lcd.fillScreen(TFT_BLACK);
	n = min(cx, cy);

	start = micros_start();

	for (i = 0; i < n; i += 5)
	{
		M5.Lcd.drawTriangle(
			cx		, cy - i, // peak
			cx - i, cy + i, // bottom left
			cx + i, cy + i, // bottom right
			M5.Lcd.color565(0, 0, i));
	}

	return micros() - start;
}

uint32_t testFilledTriangles()
{
	uint32_t start, t = 0;
	int32_t i;
	int32_t cx = M5.Lcd.width() / 2 - 1;
	int32_t cy = M5.Lcd.height() / 2 - 1;

	M5.Lcd.fillScreen(TFT_BLACK);

	start = micros_start();

	for (i = min(cx,cy); i > 10; i -= 5) {
		start = micros_start();
		M5.Lcd.fillTriangle(cx, cy - i, cx - i, cy + i, cx + i, cy + i,
			M5.Lcd.color565(0, i, i));
		t += micros() - start;
		M5.Lcd.drawTriangle(cx, cy - i, cx - i, cy + i, cx + i, cy + i,
			M5.Lcd.color565(i, i, 0));
	}

	return t;
}

uint32_t testRoundRects()
 {
	uint32_t start;
	int32_t w, i, i2;
	int32_t cx = M5.Lcd.width() / 2 - 1;
	int32_t cy = M5.Lcd.height() / 2 - 1;

	M5.Lcd.fillScreen(TFT_BLACK);
	
	w = min(M5.Lcd.width(), M5.Lcd.height());
	
	start = micros_start();

	for (i = 0; i < w; i += 6)
	{
		i2 = i / 2;
		M5.Lcd.drawRoundRect(cx-i2, cy-i2, i, i, i/8, M5.Lcd.color565(i, 0, 0));
	}

	return micros() - start;
}

uint32_t testFilledRoundRects()
{
	uint32_t start;
	int32_t i, i2;
	int32_t cx = M5.Lcd.width() / 2 - 1;
	int32_t cy = M5.Lcd.height() / 2 - 1;

	M5.Lcd.fillScreen(TFT_BLACK);

	start = micros_start();

	for (i = min(M5.Lcd.width(), M5.Lcd.height()); i > 20; i -= 6)
	{
		i2 = i / 2;
		M5.Lcd.fillRoundRect(cx-i2, cy-i2, i, i, i/8, M5.Lcd.color565(0, i, 0));
	}

	return micros() - start;
}

/***************************************************
  Original sketch text:

  This is an example sketch for the Adafruit 2.2" SPI display.
  This library works with the Adafruit 2.2" TFT Breakout w/SD card
  ----> http://www.adafruit.com/products/1480
 
  Check out the links above for our tutorials and wiring diagrams
  These displays use SPI to communicate, 4 or 5 pins are required to
  interface (RST is optional)
  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.
  MIT license, all text above must be included in any redistribution
 ****************************************************/
 
