#include "Console.h"

Console::Console() {

	tft.init(240, 320);
	tft.setRotation(2);
	tft.setTextSize(2);   // 12px
	tft.setTextWrap(false);
	tft.fillScreen(ST77XX_BLACK);

	width  = tft.width();
	height = tft.height();
	cols   = width  / FONT_WIDTH;
	rows   = height / FONT_HEIGHT;
	lines  = 0;

	stack = (uint8_t*)malloc(STACK_SIZE);
	if (!stack) {
		tft.setCursor(0, 0);
		tft.setTextColor(TFT_RED);
		tft.println("memory allocate failed!");
		while(1);
	} else {
		from = stack;
		to   = stack;
		memset(stack,'\0',sizeof(char)*(cols+1)*rows);
	}
}

Console::~Console() {}

void Console::backlight(uint8_t pin, uint8_t val)
{
	pinMode(pin,OUTPUT);
	analogWrite(pin, 64); // 0 - 255
}

void Console::print(char *text, uint16_t color)
{
	int       skip, cnt, line;
	uint16_t  c;
	uint8_t   *pos;
	uint8_t   cH, cL;
	char      prn_buff[512];
	boolean   nullflag;

	if (*text == 0x0) return;

	// stack text to the ring buffer
	*to = (uint8_t)(color>>8);
	if ((to+1)<(stack+STACK_SIZE)) to++; else to = stack;
	*to = (uint8_t)(color&0xff);

	skip = 0;
	cnt  = 0;
	while (1) {
		if ((to+1)<(stack+STACK_SIZE)) to++; else to = stack;
		*to = *text;
		if (++cnt == cols) {
			skip++;
			cnt = 0;
		}
		if (*text == 0x0) {
			if (cnt > 1) skip++;
			break;
		}
		text++;
	}
	if ((to+1)<(stack+STACK_SIZE)) to++; else to = stack;

	lines += skip;
	if (lines > rows) {
		skip = lines - rows;
		lines = rows;
	} else {
		skip = 0;
	}

	// shift the display start position
	while (skip > 0) {
		cH = *from;
		if ((from+1)<(stack+STACK_SIZE)) from++; else from=stack;
		cL = *from;
		if ((from+1)<(stack+STACK_SIZE)) from++; else from=stack;
		nullflag = false;
		while (skip > 0) {
			if (nullflag) break;
			cnt = 1;
			while (1) {
				if (*from == 0x0) {
					if (cnt > 1) --skip;
					if ((from+1)<(stack+STACK_SIZE)) from++; else from=stack;
					nullflag = true;
					break;
				} else {
					if (cnt == cols) {
						if ((--skip) == 0) {
							*from = cL;
							if (from == stack) from = stack + STACK_SIZE - 1; else --from;
							*from = cH;
							break;
						}
						cnt = 0;
					}
				}
				if ((from+1)<(stack+STACK_SIZE)) from++; else from=stack;
				cnt++;
			}
		}
	}

	tft.fillScreen(ST77XX_BLACK);
	line = 0;
	pos  = from;
	while(1) {
		c = *pos;
		if ((pos+1)<(stack+STACK_SIZE)) pos++; else pos=stack;
		c = (c<<8)&0xff00 + *pos;
		if ((pos+1)<(stack+STACK_SIZE)) pos++; else pos=stack;
		tft.setTextColor(c);

		cnt = 1;
		while (1) {
			prn_buff[cnt-1] = *pos;
			if (*pos == 0x0) {
				if (cnt > 1) {
					tft.setCursor(0, line*FONT_HEIGHT);
					tft.print(prn_buff);
					line++;
				}
				if ((pos+1)<(stack+STACK_SIZE)) pos++; else pos=stack;
				break;
			}
			if (!(cnt%cols)) {
				prn_buff[cnt] = 0x0;
				tft.setCursor(0, line*FONT_HEIGHT);
				tft.print(prn_buff);
				line++;
				cnt = 0;
			}
			if ((pos+1)<(stack+STACK_SIZE)) pos++; else pos=stack;
			cnt++;
		}
		if (pos == to) break;
	}
}

#ifndef __CONSOLE_H
#define __CONSOLE_H

#include <Adafruit_GFX.h>    // Core graphics library
//#include <Adafruit_ST7735.h> // Hardware-specific library for ST7735
#include <Adafruit_ST7789.h> // Hardware-specific library for ST7789
#include <SPI.h>
#include "stdint.h"

#define TFT_CS         A10
#define TFT_RST        A11
#define TFT_DC         A12
#define TFT_LIT        A13

#define TFT_BLACK       0x0000      /*   0,   0,   0 */
#define TFT_NAVY        0x000F      /*   0,   0, 128 */
#define TFT_DARKGREEN   0x03E0      /*   0, 128,   0 */
#define TFT_DARKCYAN    0x03EF      /*   0, 128, 128 */
#define TFT_MAROON      0x7800      /* 128,   0,   0 */
#define TFT_PURPLE      0x780F      /* 128,   0, 128 */
#define TFT_OLIVE       0x7BE0      /* 128, 128,   0 */
#define TFT_LIGHTGREY   0xC618      /* 192, 192, 192 */
#define TFT_DARKGREY    0x7BEF      /* 128, 128, 128 */
#define TFT_BLUE        0x001F      /*   0,   0, 255 */
#define TFT_GREEN       0x07E0      /*   0, 255,   0 */
#define TFT_CYAN        0x07FF      /*   0, 255, 255 */
#define TFT_RED         0xF800      /* 255,   0,   0 */
#define TFT_MAGENTA     0xF81F      /* 255,   0, 255 */
#define TFT_WHITE       0xFFFF      /* 255, 255, 255 */
#define TFT_ORANGE      0xFDA0      /* 255, 180,   0 */
#define TFT_YELLOW      0xB7E0      /* 180, 255,   0 */
#define TFT_PINK        0xFC9F
#define TFT_AQUAMARINE  0x6675      /* 102,205,170 */
#define TFT_WHEAT       0xF756      /* 245,222,179 */

#define STACK_SIZE 512
#define FONT_WIDTH  12
#define FONT_HEIGHT 16

class Console
{
	private:
		uint8_t  cols, rows, lines;
		uint16_t width, height;
		uint8_t  *stack;
		uint8_t  *from, *to;
		char     prn_buff[256];

	public:
		Console();
		~Console();
		Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);
    void print(char *text, uint16_t color);
    void backlight(uint8_t pin, uint8_t val);
};

#endif /*__CONSOLE_H*/