// SpeedTest written by Peter Loveday
//
// http://forum.pjrc.com/threads/15576-Teensy3-ST7735-Library?p=21355&viewfull=1#post21355

// This Teensy3 native optimized version requires specific pins
//
#define sclk 13  // SCLK can also use pin 14
#define mosi 11  // MOSI can also use pin 7
#define cs   10  // CS & DC can use pins 2, 6, 9, 10, 15, 20, 21, 22, 23
#define dc   9   //  but certain pairs must NOT be used: 2+10, 6+9, 20+23, 21+22
#define rst  8   // RST can use any pin
#define sdcs 4   // CS for SD card, can use any pin

#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <SPI.h>

Adafruit_ST7735 disp = Adafruit_ST7735(cs, dc, mosi, sclk, rst);

#define RGB(r,g,b) (b<<11|g<<6|r)

float pi = 3.1415926f;

void testlines(uint16_t color)
{
	disp.fillScreen(ST7735_BLACK);

	for (int16_t x=0; x < disp.width(); x+=6)
		disp.drawLine(0, 0, x, disp.height()-1, color);

	for (int16_t y=0; y < disp.height(); y+=6)
		disp.drawLine(0, 0, disp.width()-1, y, color);

	for (int16_t x=0; x < disp.width(); x+=6)
		disp.drawLine(disp.width()-1, 0, x, disp.height()-1, color);

	for (int16_t y=0; y < disp.height(); y+=6)
		disp.drawLine(disp.width()-1, 0, 0, y, color);

	for (int16_t x=0; x < disp.width(); x+=6)
		disp.drawLine(0, disp.height()-1, x, 0, color);
	
	for (int16_t y=0; y < disp.height(); y+=6)
		disp.drawLine(0, disp.height()-1, disp.width()-1, y, color);

	for (int16_t x=0; x < disp.width(); x+=6)
		disp.drawLine(disp.width()-1, disp.height()-1, x, 0, color);

	for (int16_t y=0; y < disp.height(); y+=6)
		disp.drawLine(disp.width()-1, disp.height()-1, 0, y, color);
}

void testdrawtext(char *text, uint16_t color, uint16_t bgcolor)
{
	disp.setCursor(0, 0);
	disp.setTextColor(color, bgcolor);
	disp.setTextWrap(true);
	disp.print(text);
}

void testfastlines(uint16_t color1, uint16_t color2)
{
	disp.fillScreen(ST7735_BLACK);

	for (int16_t y=0; y < disp.height(); y+=5)
		disp.drawFastHLine(0, y, disp.width(), color1);

	for (int16_t x=0; x < disp.width(); x+=5)
		disp.drawFastVLine(x, 0, disp.height(), color2);
}

void testdrawrects(uint16_t color)
{
	disp.fillScreen(ST7735_BLACK);

	for (int16_t x=0; x < disp.width(); x+=6)
		disp.drawRect(disp.width()/2 -x/2, disp.height()/2 -x/2 , x, x, color);
}

void testfillrects(uint16_t color1, uint16_t color2)
{
	disp.fillScreen(ST7735_BLACK);

	for (int16_t x=disp.width()-1; x > 6; x-=6)
	{
		disp.fillRect(disp.width()/2 -x/2, disp.height()/2 -x/2 , x, x, color1);
		disp.drawRect(disp.width()/2 -x/2, disp.height()/2 -x/2 , x, x, color2);
	}
}

void testfillcircles(uint8_t radius, uint16_t color)
{
	for (int16_t x=radius; x < disp.width(); x+=radius*2)
		for (int16_t y=radius; y < disp.height(); y+=radius*2)
			disp.fillCircle(x, y, radius, color);
}

void testdrawcircles(uint8_t radius, uint16_t color)
{
	for (int16_t x=0; x < disp.width()+radius; x+=radius*2)
		for (int16_t y=0; y < disp.height()+radius; y+=radius*2)
			disp.drawCircle(x, y, radius, color);
}

void testtriangles()
{
	disp.fillScreen(ST7735_BLACK);

	int color = 0xF800;
	int t;
	int w = 63;
	int x = 159;
	int y = 0;
	int z = 127;
	
	for (t = 0 ; t <= 15; t+=1)
	{
		disp.drawTriangle(w, y, y, x, z, x, color);
		x-=4;
		y+=4;
		z-=4;
		color+=100;
	}
}

void testroundrects()
{
	disp.fillScreen(ST7735_BLACK);

	int color = 100;
	int i;
	int t;
	
	for(t = 0 ; t <= 4; t+=1)
	{
		int x = 0;
		int y = 0;
		int w = 127;
		int h = 159;
		for(i = 0 ; i <= 24; i+=1)
		{
			disp.drawRoundRect(x, y, w, h, 5, color);
			x+=2;
			y+=3;
			w-=4;
			h-=6;
			color+=1100;
		}
		color+=100;
	}
}

#define SETCOLOR(c) disp.setTextColor(c, bg ? ST7735_BLACK : c);

void tftPrintTest(bool bg)
{
	disp.setTextWrap(false);
	disp.fillScreen(ST7735_BLACK);
	disp.setCursor(0, 30);
	SETCOLOR(ST7735_RED);
	disp.setTextSize(1);
	disp.println("Hello World!");
	SETCOLOR(ST7735_YELLOW);
	disp.setTextSize(2);
	disp.println("Hello World!");
	SETCOLOR(ST7735_GREEN);
	disp.setTextSize(3);
	disp.println("Hello World!");
	SETCOLOR(ST7735_BLUE);
	disp.setTextSize(4);
	disp.print(1234.567);
  
	disp.setCursor(0, 0);
	disp.fillScreen(ST7735_BLACK);
	SETCOLOR(ST7735_WHITE);
	disp.setTextSize(0);
	disp.println("Hello World!");
	disp.setTextSize(1);
	SETCOLOR(ST7735_GREEN);
	disp.print(pi, 6);
	disp.println(" Want pi?");
	disp.println(" ");
	disp.print(8675309, HEX);
	disp.println(" Print HEX!");
	disp.println(" ");
	SETCOLOR(ST7735_WHITE);
	disp.println("Sketch has been");
	disp.println("running for: ");
	SETCOLOR(ST7735_MAGENTA);
	disp.print(millis() / 1000);
	SETCOLOR(ST7735_WHITE);
	disp.print(" seconds.");
}

void mediabuttons()
{
	// play
	disp.fillScreen(ST7735_BLACK);
	disp.fillRoundRect(25, 10, 78, 60, 8, ST7735_WHITE);
	disp.fillTriangle(42, 20, 42, 60, 90, 40, ST7735_RED);
  
	// pause
	disp.fillRoundRect(25, 90, 78, 60, 8, ST7735_WHITE);
	disp.fillRoundRect(39, 98, 20, 45, 5, ST7735_GREEN);
	disp.fillRoundRect(69, 98, 20, 45, 5, ST7735_GREEN);
  
	// play color
	disp.fillTriangle(42, 20, 42, 60, 90, 40, ST7735_BLUE);
  
	// pause color
	disp.fillRoundRect(39, 98, 20, 45, 5, ST7735_RED);
	disp.fillRoundRect(69, 98, 20, 45, 5, ST7735_RED);
	// play color
	disp.fillTriangle(42, 20, 42, 60, 90, 40, ST7735_GREEN);
}

int t[20];
int n = 0;

void setup()
{
	pinMode(sdcs, INPUT_PULLUP);  // keep SD CS high when not using SD card
	disp.initR(INITR_REDTAB);
	disp.setRotation(0);
	disp.setTextWrap(true);
	disp.setTextColor(RGB(31,31,31), RGB(0,0,0));
	disp.setCursor(0, 0);

	disp.fillScreen(RGB(0,0,0));

	t[n++] = millis();
	disp.fillScreen(ST7735_BLACK);
	t[n++] = millis();
	testdrawtext("Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabitur adipiscing ante sed nibh tincidunt feugiat. Maecenas enim massa, fringilla sed malesuada et, malesuada sit amet turpis. Sed porttitor neque ut ante pretium vitae malesuada nunc bibendum. Nullam aliquet ultrices massa eu hendrerit. Ut sed nisi lorem. In vestibulum purus a tortor imperdiet posuere. ", ST7735_WHITE, ST7735_WHITE);
	t[n++] = millis();
	tftPrintTest(false);
	t[n++] = millis();
	testdrawtext("Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabitur adipiscing ante sed nibh tincidunt feugiat. Maecenas enim massa, fringilla sed malesuada et, malesuada sit amet turpis. Sed porttitor neque ut ante pretium vitae malesuada nunc bibendum. Nullam aliquet ultrices massa eu hendrerit. Ut sed nisi lorem. In vestibulum purus a tortor imperdiet posuere. ", ST7735_WHITE, ST7735_BLACK);
	t[n++] = millis();
	tftPrintTest(true);
	t[n++] = millis();
	testlines(ST7735_YELLOW);
	t[n++] = millis();
	testfastlines(ST7735_RED, ST7735_BLUE);
	t[n++] = millis();
	testdrawrects(ST7735_GREEN);
	t[n++] = millis();
	testfillrects(ST7735_YELLOW, ST7735_MAGENTA);
	t[n++] = millis();
	disp.fillScreen(ST7735_BLACK);
	t[n++] = millis();
	testfillcircles(10, ST7735_BLUE);
	t[n++] = millis();
	testdrawcircles(10, ST7735_WHITE);
	t[n++] = millis();
	testroundrects();
	t[n++] = millis();
	testtriangles();
	t[n++] = millis();
	mediabuttons();
	t[n++] = millis();
	disp.fillScreen(RGB(0,0,0));
}

// These are the results for the adafruit library, so we can report results as Nx

uint16_t normalize[] =
{
	129,
	216,
	348,
	955,
	630,
	1658,
	185,
	168,
	955,
	129,
	195,
	163,
	604,
	362,
	253,
	6950
};

void loop()
{
	static float fps = 0.0;
	static int h = 0;
	
	uint8_t r,g,b;
	if (h < 32)
	{
		r = 31; g = h; b = 0;
	}
	else if (h < 64)
	{
		r = 63-h; g = 31; b = 0;
	}
	else if (h < 96)
	{
		r = 0; g = 31; b = h-64;
	}
	else if (h < 128)
	{
		r = 0; g = 127-h; b = 31;
	}
	else if (h < 160)
	{
		r = h-128; g = 0; b = 31;
	}
	else if (h < 192)
	{
		r = 31; g = 0; b = 191-h;
	}
	else
	{
		r = 31; g = 0; b = 0;
		h = 0;
	}

	h++;
	
	int start = micros();
	disp.fillRect(120, 0, 8, 160, RGB(r,g,b));

	disp.setTextWrap(false);
	disp.setTextColor(RGB(31,31,31), RGB(0,0,0));
	disp.setCursor(0, 0);

	for (int i=1; i < n; i++)
	{
		disp.print(i < 10 ? "Test  " : "Test ");
		disp.print(i, DEC);
		disp.print(": ");
		disp.println(float(normalize[i-1])/float(t[i]-t[i-1]), 2);
	}

	disp.println("");
	disp.print("Total  : ");
	disp.println(float(normalize[n-1])/float(t[n-1]-t[0]), 2);

	disp.println("");
	disp.print("FPS    : ");
	disp.println(fps, 2);

	int end = micros();
	
	fps = 1000000.0f/float(end-start);
}
