components/tft/tft.c

/* TFT module
 *
 *  Author: LoBo (loboris@gmail.com, loboris.github)
 *
 *  Module supporting SPI TFT displays based on ILI9341 & ILI9488 controllers
*/

#include <stdio.h>
#include <errno.h>
#include <sys/stat.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "tft.h"
#include "time.h"
#include <math.h>
#include "esp32/rom/tjpgd.h"
#include "esp_heap_caps.h"
#include "tftspi.h"

#define DEG_TO_RAD 0.01745329252
#define RAD_TO_DEG 57.295779513
#define deg_to_rad 0.01745329252 + 3.14159265359
#define swap(a, b) { int16_t t = a; a = b; b = t; }
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#if !defined(max)
#define max(A,B) ( (A) > (B) ? (A):(B))
#endif
#if !defined(min)
#define min(A,B) ( (A) < (B) ? (A):(B))
#endif

// Embedded fonts
extern uint8_t tft_SmallFont[];
extern uint8_t tft_DefaultFont[];
extern uint8_t tft_Dejavu18[];
extern uint8_t tft_Dejavu24[];
extern uint8_t tft_Ubuntu16[];
extern uint8_t tft_Comic24[];
extern uint8_t tft_minya24[];
extern uint8_t tft_tooney32[];
extern uint8_t tft_def_small[];

// ==== Color definitions constants ==============
const color_t TFT_BLACK       = {   0,   0,   0 };	///< Black
const color_t TFT_NAVY        = {   0,   0, 128 };	///< Navy blue
const color_t TFT_DARKGREEN   = {   0, 128,   0 };	///< Dark green
const color_t TFT_DARKCYAN    = {   0, 128, 128 };	///< Dark cyan
const color_t TFT_MAROON      = { 128,   0,   0 };	///< Maroon red
const color_t TFT_PURPLE      = { 128,   0, 128 };	///< Purple
const color_t TFT_OLIVE       = { 128, 128,   0 };	///< Olive 
const color_t TFT_LIGHTGREY   = { 192, 192, 192 };	///< Light gray
const color_t TFT_DARKGREY    = { 128, 128, 128 };	///< Dark gray
const color_t TFT_BLUE        = {   0,   0, 255 };	///< Blue
const color_t TFT_GREEN       = {   0, 255,   0 };	///< Green
const color_t TFT_CYAN        = {   0, 255, 255 };	///< Cyan
const color_t TFT_RED         = { 255,   0,   0 };	///< Red
const color_t TFT_MAGENTA     = { 255,   0, 255 };	///< Magenta
const color_t TFT_YELLOW      = { 255, 255,   0 };	///< Yellow
const color_t TFT_WHITE       = { 255, 255, 255 };	///< White
const color_t TFT_ORANGE      = { 255, 164,   0 };	///< Orange
const color_t TFT_GREENYELLOW = { 172, 255,  44 };	///< Green-yellow
const color_t TFT_PINK        = { 255, 192, 202 };	///< Pink
// ===============================================

// ==============================================================
// ==== Set default values of global variables ==================
uint8_t orientation = LANDSCAPE;// screen orientation
uint16_t font_rotate = 0;		// font rotation
uint8_t	font_transparent = 0;
uint8_t	font_forceFixed = 0;
uint8_t	text_wrap = 0;			// character wrapping to new line
color_t	_fg = {  0, 255,   0};
color_t _bg = {  0,   0,   0};
uint8_t image_debug = 0;

float _angleOffset = DEFAULT_ANGLE_OFFSET;

int	TFT_X = 0;
int	TFT_Y = 0;

uint16_t tp_xleft = 300;
uint16_t tp_xright = 3550;
uint16_t tp_ytop = 3800;
uint16_t tp_ybottom = 300;

dispWin_t dispWin = {
  .x1 = 0,
  .y1 = 0,
  .x2 = DEFAULT_TFT_DISPLAY_WIDTH -1,
  .y2 = DEFAULT_TFT_DISPLAY_HEIGHT -1,
};

Font cfont = {
	.font = tft_DefaultFont,
	.x_size = 0,
	.y_size = 0x0B,
	.offset = 0,
	.numchars = 95,
	.bitmap = 1,
};

uint8_t font_buffered_char = 1;
uint8_t font_line_space = 0;
// ==============================================================


typedef struct {
      uint8_t charCode;
      int adjYOffset;
      int width;
      int height;
      int xOffset;
      int xDelta;
      uint16_t dataPtr;
} propFont;

static dispWin_t dispWinTemp;

static uint8_t *userfont = NULL;
static int TFT_OFFSET = 0;
static propFont	fontChar;


// =========================================================================
// ** All drawings are clipped to 'dispWin' **
// ** All x,y coordinates in public functions are relative to clip window **
// =========== : Public functions
// ----------- : Local functions
// =========================================================================


// draw color pixel on screen
//------------------------------------------------------------------------
static void _drawPixel(int16_t x, int16_t y, color_t color, uint8_t sel) {

	if ((x < dispWin.x1) || (y < dispWin.y1) || (x > dispWin.x2) || (y > dispWin.y2)) return;
	drawPixel(x, y, color, sel);
}

//====================================================================
void TFT_drawPixel(int16_t x, int16_t y, color_t color, uint8_t sel) {

	_drawPixel(x+dispWin.x1, y+dispWin.y1, color, sel);
}

//===========================================
color_t TFT_readPixel(int16_t x, int16_t y) {

  if ((x < dispWin.x1) || (y < dispWin.y1) || (x > dispWin.x2) || (y > dispWin.y2)) return TFT_BLACK;

  return readPixel(x, y);
}

//--------------------------------------------------------------------------
static void _drawFastVLine(int16_t x, int16_t y, int16_t h, color_t color) {
	// clipping
	if ((x < dispWin.x1) || (x > dispWin.x2) || (y > dispWin.y2)) return;
	if (y < dispWin.y1) {
		h -= (dispWin.y1 - y);
		y = dispWin.y1;
	}
	if (h < 0) h = 0;
	if ((y + h) > (dispWin.y2+1)) h = dispWin.y2 - y + 1;
	if (h == 0) h = 1;
	TFT_pushColorRep(x, y, x, y+h-1, color, (uint32_t)h);
}

//--------------------------------------------------------------------------
static void _drawFastHLine(int16_t x, int16_t y, int16_t w, color_t color) {
	// clipping
	if ((y < dispWin.y1) || (x > dispWin.x2) || (y > dispWin.y2)) return;
	if (x < dispWin.x1) {
		w -= (dispWin.x1 - x);
		x = dispWin.x1;
	}
	if (w < 0) w = 0;
	if ((x + w) > (dispWin.x2+1)) w = dispWin.x2 - x + 1;
	if (w == 0) w = 1;

	TFT_pushColorRep(x, y, x+w-1, y, color, (uint32_t)w);
}

//======================================================================
void TFT_drawFastVLine(int16_t x, int16_t y, int16_t h, color_t color) {
	_drawFastVLine(x+dispWin.x1, y+dispWin.y1, h, color);
}

//======================================================================
void TFT_drawFastHLine(int16_t x, int16_t y, int16_t w, color_t color) {
	_drawFastHLine(x+dispWin.x1, y+dispWin.y1, w, color);
}

// Bresenham's algorithm - thx wikipedia - speed enhanced by Bodmer this uses
// the eficient FastH/V Line draw routine for segments of 2 pixels or more
//----------------------------------------------------------------------------------
static void _drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, color_t color)
{
  if (x0 == x1) {
	  if (y0 <= y1) _drawFastVLine(x0, y0, y1-y0, color);
	  else _drawFastVLine(x0, y1, y0-y1, color);
	  return;
  }
  if (y0 == y1) {
	  if (x0 <= x1) _drawFastHLine(x0, y0, x1-x0, color);
	  else _drawFastHLine(x1, y0, x0-x1, color);
	  return;
  }

  int steep = 0;
  if (abs(y1 - y0) > abs(x1 - x0)) steep = 1;
  if (steep) {
    swap(x0, y0);
    swap(x1, y1);
  }
  if (x0 > x1) {
    swap(x0, x1);
    swap(y0, y1);
  }

  int16_t dx = x1 - x0, dy = abs(y1 - y0);
  int16_t err = dx >> 1, ystep = -1, xs = x0, dlen = 0;

  if (y0 < y1) ystep = 1;

  // Split into steep and not steep for FastH/V separation
  if (steep) {
    for (; x0 <= x1; x0++) {
      dlen++;
      err -= dy;
      if (err < 0) {
        err += dx;
        if (dlen == 1) _drawPixel(y0, xs, color, 1);
        else _drawFastVLine(y0, xs, dlen, color);
        dlen = 0; y0 += ystep; xs = x0 + 1;
      }
    }
    if (dlen) _drawFastVLine(y0, xs, dlen, color);
  }
  else
  {
    for (; x0 <= x1; x0++) {
      dlen++;
      err -= dy;
      if (err < 0) {
        err += dx;
        if (dlen == 1) _drawPixel(xs, y0, color, 1);
        else _drawFastHLine(xs, y0, dlen, color);
        dlen = 0; y0 += ystep; xs = x0 + 1;
      }
    }
    if (dlen) _drawFastHLine(xs, y0, dlen, color);
  }
}

//==============================================================================
void TFT_drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, color_t color)
{
	_drawLine(x0+dispWin.x1, y0+dispWin.y1, x1+dispWin.x1, y1+dispWin.y1, color);
}

// fill a rectangle
//--------------------------------------------------------------------------------
static void _fillRect(int16_t x, int16_t y, int16_t w, int16_t h, color_t color) {
	// clipping
	if ((x >= dispWin.x2) || (y > dispWin.y2)) return;

	if (x < dispWin.x1) {
		w -= (dispWin.x1 - x);
		x = dispWin.x1;
	}
	if (y < dispWin.y1) {
		h -= (dispWin.y1 - y);
		y = dispWin.y1;
	}
	if (w < 0) w = 0;
	if (h < 0) h = 0;

	if ((x + w) > (dispWin.x2+1)) w = dispWin.x2 - x + 1;
	if ((y + h) > (dispWin.y2+1)) h = dispWin.y2 - y + 1;
	if (w == 0) w = 1;
	if (h == 0) h = 1;
	TFT_pushColorRep(x, y, x+w-1, y+h-1, color, (uint32_t)(h*w));
}

//============================================================================
void TFT_fillRect(int16_t x, int16_t y, int16_t w, int16_t h, color_t color) {
	_fillRect(x+dispWin.x1, y+dispWin.y1, w, h, color);
}

//==================================
void TFT_fillScreen(color_t color) {
	TFT_pushColorRep(0, 0, _width-1, _height-1, color, (uint32_t)(_height*_width));
}

//==================================
void TFT_fillWindow(color_t color) {
	TFT_pushColorRep(dispWin.x1, dispWin.y1, dispWin.x2, dispWin.y2,
			color, (uint32_t)((dispWin.x2-dispWin.x1+1) * (dispWin.y2-dispWin.y1+1)));
}

// ^^^============= Basics drawing functions ================================^^^


// ================ Graphics drawing functions ==================================

//-----------------------------------------------------------------------------------
static void _drawRect(uint16_t x1,uint16_t y1,uint16_t w,uint16_t h, color_t color) {
  _drawFastHLine(x1,y1,w, color);
  _drawFastVLine(x1+w-1,y1,h, color);
  _drawFastHLine(x1,y1+h-1,w, color);
  _drawFastVLine(x1,y1,h, color);
}

//===============================================================================
void TFT_drawRect(uint16_t x1,uint16_t y1,uint16_t w,uint16_t h, color_t color) {
	_drawRect(x1+dispWin.x1, y1+dispWin.y1, w, h, color);
}

//-------------------------------------------------------------------------------------------------
static void drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, color_t color)
{
	int16_t f = 1 - r;
	int16_t ddF_x = 1;
	int16_t ddF_y = -2 * r;
	int16_t x = 0;
	int16_t y = r;

	disp_select();
	while (x < y) {
		if (f >= 0) {
			y--;
			ddF_y += 2;
			f += ddF_y;
		}
		x++;
		ddF_x += 2;
		f += ddF_x;
		if (cornername & 0x4) {
			_drawPixel(x0 + x, y0 + y, color, 0);
			_drawPixel(x0 + y, y0 + x, color, 0);
		}
		if (cornername & 0x2) {
			_drawPixel(x0 + x, y0 - y, color, 0);
			_drawPixel(x0 + y, y0 - x, color, 0);
		}
		if (cornername & 0x8) {
			_drawPixel(x0 - y, y0 + x, color, 0);
			_drawPixel(x0 - x, y0 + y, color, 0);
		}
		if (cornername & 0x1) {
			_drawPixel(x0 - y, y0 - x, color, 0);
			_drawPixel(x0 - x, y0 - y, color, 0);
		}
	}
	disp_deselect();
}

// Used to do circles and roundrects
//----------------------------------------------------------------------------------------------------------------
static void fillCircleHelper(int16_t x0, int16_t y0, int16_t r,	uint8_t cornername, int16_t delta, color_t color)
{
	int16_t f = 1 - r;
	int16_t ddF_x = 1;
	int16_t ddF_y = -2 * r;
	int16_t x = 0;
	int16_t y = r;
	int16_t ylm = x0 - r;

	while (x < y) {
		if (f >= 0) {
			if (cornername & 0x1) _drawFastVLine(x0 + y, y0 - x, 2 * x + 1 + delta, color);
			if (cornername & 0x2) _drawFastVLine(x0 - y, y0 - x, 2 * x + 1 + delta, color);
			ylm = x0 - y;
			y--;
			ddF_y += 2;
			f += ddF_y;
		}
		x++;
		ddF_x += 2;
		f += ddF_x;

		if ((x0 - x) > ylm) {
			if (cornername & 0x1) _drawFastVLine(x0 + x, y0 - y, 2 * y + 1 + delta, color);
			if (cornername & 0x2) _drawFastVLine(x0 - x, y0 - y, 2 * y + 1 + delta, color);
		}
	}
}

// Draw a rounded rectangle
//=============================================================================================
void TFT_drawRoundRect(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t r, color_t color)
{
	x += dispWin.x1;
	y += dispWin.y1;

	// smarter version
	_drawFastHLine(x + r, y, w - 2 * r, color);			// Top
	_drawFastHLine(x + r, y + h - 1, w - 2 * r, color);	// Bottom
	_drawFastVLine(x, y + r, h - 2 * r, color);			// Left
	_drawFastVLine(x + w - 1, y + r, h - 2 * r, color);	// Right

	// draw four corners
	drawCircleHelper(x + r, y + r, r, 1, color);
	drawCircleHelper(x + w - r - 1, y + r, r, 2, color);
	drawCircleHelper(x + w - r - 1, y + h - r - 1, r, 4, color);
	drawCircleHelper(x + r, y + h - r - 1, r, 8, color);
}

// Fill a rounded rectangle
//=============================================================================================
void TFT_fillRoundRect(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t r, color_t color)
{
	x += dispWin.x1;
	y += dispWin.y1;

	// smarter version
	_fillRect(x + r, y, w - 2 * r, h, color);

	// draw four corners
	fillCircleHelper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);
	fillCircleHelper(x + r, y + r, r, 2, h - 2 * r - 1, color);
}


//-----------------------------------------------------------------------------------------------

// Draw a triangle
//--------------------------------------------------------------------------------------------------------------------
static void _drawTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
{
	_drawLine(x0, y0, x1, y1, color);
	_drawLine(x1, y1, x2, y2, color);
	_drawLine(x2, y2, x0, y0, color);
}

// Fill a triangle
//--------------------------------------------------------------------------------------------------------------------
static void _fillTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
{
  int16_t a, b, y, last;

  // Sort coordinates by Y order (y2 >= y1 >= y0)
  if (y0 > y1) {
    swap(y0, y1); swap(x0, x1);
  }
  if (y1 > y2) {
    swap(y2, y1); swap(x2, x1);
  }
  if (y0 > y1) {
    swap(y0, y1); swap(x0, x1);
  }

  if(y0 == y2) { // Handle awkward all-on-same-line case as its own thing
    a = b = x0;
    if(x1 < a)      a = x1;
    else if(x1 > b) b = x1;
    if(x2 < a)      a = x2;
    else if(x2 > b) b = x2;
    _drawFastHLine(a, y0, b-a+1, color);
    return;
  }

  int16_t
    dx01 = x1 - x0,
    dy01 = y1 - y0,
    dx02 = x2 - x0,
    dy02 = y2 - y0,
    dx12 = x2 - x1,
    dy12 = y2 - y1;
  int32_t
    sa   = 0,
    sb   = 0;

  // For upper part of triangle, find scanline crossings for segments
  // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
  // is included here (and second loop will be skipped, avoiding a /0
  // error there), otherwise scanline y1 is skipped here and handled
  // in the second loop...which also avoids a /0 error here if y0=y1
  // (flat-topped triangle).
  if(y1 == y2) last = y1;   // Include y1 scanline
  else         last = y1-1; // Skip it

  for(y=y0; y<=last; y++) {
    a   = x0 + sa / dy01;
    b   = x0 + sb / dy02;
    sa += dx01;
    sb += dx02;
    /* longhand:
    a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if(a > b) swap(a,b);
    _drawFastHLine(a, y, b-a+1, color);
  }

  // For lower part of triangle, find scanline crossings for segments
  // 0-2 and 1-2.  This loop is skipped if y1=y2.
  sa = dx12 * (y - y1);
  sb = dx02 * (y - y0);
  for(; y<=y2; y++) {
    a   = x1 + sa / dy12;
    b   = x0 + sb / dy02;
    sa += dx12;
    sb += dx02;
    /* longhand:
    a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if(a > b) swap(a,b);
    _drawFastHLine(a, y, b-a+1, color);
  }
}

//====================================================================
void TFT_drawCircle(int16_t x, int16_t y, int radius, color_t color) {
	x += dispWin.x1;
	y += dispWin.y1;
	int f = 1 - radius;
	int ddF_x = 1;
	int ddF_y = -2 * radius;
	int x1 = 0;
	int y1 = radius;

	disp_select();
	_drawPixel(x, y + radius, color, 0);
	_drawPixel(x, y - radius, color, 0);
	_drawPixel(x + radius, y, color, 0);
	_drawPixel(x - radius, y, color, 0);
	while(x1 < y1) {
		if (f >= 0) {
			y1--;
			ddF_y += 2;
			f += ddF_y;
		}
		x1++;
		ddF_x += 2;
		f += ddF_x;
		_drawPixel(x + x1, y + y1, color, 0);
		_drawPixel(x - x1, y + y1, color, 0);
		_drawPixel(x + x1, y - y1, color, 0);
		_drawPixel(x - x1, y - y1, color, 0);
		_drawPixel(x + y1, y + x1, color, 0);
		_drawPixel(x - y1, y + x1, color, 0);
		_drawPixel(x + y1, y - x1, color, 0);
		_drawPixel(x - y1, y - x1, color, 0);
	}
  disp_deselect();
}

//====================================================================
void TFT_fillCircle(int16_t x, int16_t y, int radius, color_t color) {
	x += dispWin.x1;
	y += dispWin.y1;

	_drawFastVLine(x, y-radius, 2*radius+1, color);
	fillCircleHelper(x, y, radius, 3, 0, color);
}


// ================ Font and string functions ==================================

//--------------------------------------------------------
static int load_file_font(const char * fontfile, int info)
{
	int err = 0;
	char err_msg[256] = {'\0'};

	if (userfont != NULL) {
		free(userfont);
		userfont = NULL;
	}

    struct stat sb;

    // Open the file
    FILE *fhndl = fopen(fontfile, "r");
    if (!fhndl) {
    	sprintf(err_msg, "Error open font '%s'", fontfile);
		err = 1;
		goto exit;
    }

	// Get file size
    if (stat(fontfile, &sb) != 0) {
    	sprintf(err_msg, "stat error");
		err = 2;
		goto exit;
    }
	int fsize = sb.st_size;
	if (fsize < 30) {
		sprintf(err_msg, "Error font file size");
		err = 3;
		goto exit;
	}

	userfont = malloc(fsize+4);
	if (userfont == NULL) {
		sprintf(err_msg, "Font malloc error");
		fclose(fhndl);
		err = 4;
		goto exit;
	}

	int read = fread(userfont, 1, fsize, fhndl);

	fclose(fhndl);

	if (read != fsize) {
		sprintf(err_msg, "Font read error");
		err = 5;
		goto exit;
	}

	userfont[read] = 0;
	if (strstr((char *)(userfont+read-8), "RPH_font") == NULL) {
		sprintf(err_msg, "Font ID not found");
		err = 6;
		goto exit;
	}

	// Check size
	int size = 0;
	int numchar = 0;
	int width = userfont[0];
	int height = userfont[1];
	uint8_t first = 255;
	uint8_t last = 0;
	//int offst = 0;
	int pminwidth = 255;
	int pmaxwidth = 0;

	if (width != 0) {
		// Fixed font
		numchar = userfont[3];
		first = userfont[2];
		last = first + numchar - 1;
		size = ((width * height * numchar) / 8) + 4;
	}
	else {
		// Proportional font
		size = 4; // point at first char data
		uint8_t charCode;
		int charwidth;

		do {
		    charCode = userfont[size];
		    charwidth = userfont[size+2];

		    if (charCode != 0xFF) {
		    	numchar++;
		    	if (charwidth != 0) size += ((((charwidth * userfont[size+3])-1) / 8) + 7);
		    	else size += 6;

		    	if (info) {
	    			if (charwidth > pmaxwidth) pmaxwidth = charwidth;
	    			if (charwidth < pminwidth) pminwidth = charwidth;
	    			if (charCode < first) first = charCode;
	    			if (charCode > last) last = charCode;
	    		}
		    }
		    else size++;
		  } while ((size < (read-8)) && (charCode != 0xFF));
	}

	if (size != (read-8)) {
		sprintf(err_msg, "Font size error: found %d expected %d)", size, (read-8));
		err = 7;
		goto exit;
	}

	if (info) {
		if (width != 0) {
			printf("Fixed width font:\r\n  size: %d  width: %d  height: %d  characters: %d (%d~%d)\n",
					size, width, height, numchar, first, last);
		}
		else {
			printf("Proportional font:\r\n  size: %d  width: %d~%d  height: %d  characters: %d (%d~%d)\n",
					size, pminwidth, pmaxwidth, height, numchar, first, last);
		}
	}

exit:
	if (err) {
		if (userfont) {
			free(userfont);
			userfont = NULL;
		}
		if (info) printf("Error: %d [%s]\r\n", err, err_msg);
	}
	return err;
}


// -----------------------------------------------------------------------------------------
// Individual Proportional Font Character Format:
// -----------------------------------------------------------------------------------------
// Character Code
// yOffset				(start Y of visible pixels)
// Width				(width of the visible pixels)
// Height				(height of the visible pixels)
// xOffset				(start X of visible pixels)
// xDelta				(the distance to move the cursor. Effective width of the character.)
// Data[n]
// -----------------------------------------------------------------------------------------

//---------------------------------------------------------------------------------------------
// Character drawing rectangle is (0, 0) (xDelta-1, cfont.y_size-1)
// Character visible pixels rectangle is (xOffset, yOffset) (xOffset+Width-1, yOffset+Height-1)
//---------------------------------------------------------------------------------------------

//----------------------------------

// Set max width & height of the proportional font
//-----------------------------
static void getMaxWidthHeight()
{
	uint16_t tempPtr = 4; // point at first char data
	uint8_t cc, cw, ch, cd, cy;

	cfont.numchars = 0;
	cfont.max_x_size = 0;

    cc = cfont.font[tempPtr++];
    while (cc != 0xFF)  {
    	cfont.numchars++;
        cy = cfont.font[tempPtr++];
        cw = cfont.font[tempPtr++];
        ch = cfont.font[tempPtr++];
        tempPtr++;
        cd = cfont.font[tempPtr++];
        cy += ch;
		if (cw > cfont.max_x_size) cfont.max_x_size = cw;
		if (cd > cfont.max_x_size) cfont.max_x_size = cd;
		if (ch > cfont.y_size) cfont.y_size = ch;
		if (cy > cfont.y_size) cfont.y_size = cy;
		if (cw != 0) {
			// packed bits
			tempPtr += (((cw * ch)-1) / 8) + 1;
		}
	    cc = cfont.font[tempPtr++];
	}
    cfont.size = tempPtr;
}

// Return the Glyph data for an individual character in the proportional font
//------------------------------------
static uint8_t getCharPtr(uint8_t c) {
  uint16_t tempPtr = 4; // point at first char data

  do {
	fontChar.charCode = cfont.font[tempPtr++];
    if (fontChar.charCode == 0xFF) return 0;

    fontChar.adjYOffset = cfont.font[tempPtr++];
    fontChar.width = cfont.font[tempPtr++];
    fontChar.height = cfont.font[tempPtr++];
    fontChar.xOffset = cfont.font[tempPtr++];
    fontChar.xOffset = fontChar.xOffset < 0x80 ? fontChar.xOffset : -(0xFF - fontChar.xOffset);
    fontChar.xDelta = cfont.font[tempPtr++];

    if (c != fontChar.charCode && fontChar.charCode != 0xFF) {
      if (fontChar.width != 0) {
        // packed bits
        tempPtr += (((fontChar.width * fontChar.height)-1) / 8) + 1;
      }
    }
  } while ((c != fontChar.charCode) && (fontChar.charCode != 0xFF));

  fontChar.dataPtr = tempPtr;
  if (c == fontChar.charCode) {
    if (font_forceFixed > 0) {
      // fix width & offset for forced fixed width
      fontChar.xDelta = cfont.max_x_size;
      fontChar.xOffset = (fontChar.xDelta - fontChar.width) / 2;
    }
  }
  else return 0;

  return 1;
}


//===================================================
void TFT_setFont(uint8_t font, const char *font_file)
{
    cfont.font = NULL;

    if (font == FONT_7SEG) {
	cfont.bitmap = 2;
	cfont.x_size = 24;
	cfont.y_size = 6;
	cfont.offset = 0;
	cfont.color  = _fg;
    } else {
	if (font == USER_FONT) {
	    if (load_file_font(font_file, 0) != 0)
		cfont.font = tft_DefaultFont;
	    else
		cfont.font = userfont;
	} else if (font == DEJAVU18_FONT)
	    cfont.font = tft_Dejavu18;
	else if (font == DEJAVU24_FONT) 
	    cfont.font = tft_Dejavu24;
	else if (font == UBUNTU16_FONT) 
	    cfont.font = tft_Ubuntu16;
	else if (font == COMIC24_FONT)
	    cfont.font = tft_Comic24;
	else if (font == MINYA24_FONT)
	    cfont.font = tft_minya24;
	else if (font == TOONEY32_FONT)
	    cfont.font = tft_tooney32;
	else if (font == SMALL_FONT)
	    cfont.font = tft_SmallFont;
	else if (font == DEF_SMALL_FONT)
	    cfont.font = tft_def_small;
	else
	    cfont.font = tft_DefaultFont;

	cfont.bitmap = 1;
	cfont.x_size = cfont.font[0];
	cfont.y_size = cfont.font[1];
	if (cfont.x_size > 0) {
	    cfont.offset = cfont.font[2];
	    cfont.numchars = cfont.font[3];
	    cfont.size = cfont.x_size * cfont.y_size * cfont.numchars;
	} else {
	    cfont.offset = 4;
	    getMaxWidthHeight();
	}
	//_testFont();
    }
}


// -----------------------------------------------------------------------------------------
// Individual Proportional Font Character Format:
// -----------------------------------------------------------------------------------------
// Character Code
// yOffset				(start Y of visible pixels)
// Width				(width of the visible pixels)
// Height				(height of the visible pixels)
// xOffset				(start X of visible pixels)
// xDelta				(the distance to move the cursor. Effective width of the character.)
// Data[n]
// -----------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------
// Character drawing rectangle is (0, 0) (xDelta-1, cfont.y_size-1)
// Character visible pixels rectangle is (xOffset, yOffset) (xOffset+Width-1, yOffset+Height-1)
//---------------------------------------------------------------------------------------------

// print non-rotated proportional character
// character is already in fontChar
//----------------------------------------------
static int printProportionalChar(int x, int y) {
    uint8_t	ch = 0;
    int		i, j, char_width;

    char_width = ((fontChar.width > fontChar.xDelta) ? fontChar.width : fontChar.xDelta);

    if ((font_buffered_char) && (!font_transparent)) {
	int len, bufPos;

	// === buffer Glyph data for faster sending ===
	len = char_width * cfont.y_size;
	color_t *color_line = heap_caps_malloc(len*3, MALLOC_CAP_DMA);
	if (color_line) {
	    // fill with background color
	    for (int n = 0; n < len; n++) {
		color_line[n] = _bg;
	    }
	    // set character pixels to foreground color
	    uint8_t mask = 0x80;
	    for (j = 0; j < fontChar.height; j++) {
		for (i = 0; i < fontChar.width; i++) {
		    if (((i + (j*fontChar.width)) % 8) == 0) {
			mask = 0x80;
			ch = cfont.font[fontChar.dataPtr++];
		    }
		    if ((ch & mask) != 0) {
			// visible pixel
			bufPos = ((j + fontChar.adjYOffset) * char_width) + (fontChar.xOffset + i);  // bufY + bufX
			color_line[bufPos] = _fg;
		    }
		    mask >>= 1;
		}
	    }
	    // send to display in one transaction
	    disp_select();
	    send_data(x, y, x+char_width-1, y+cfont.y_size-1, len, color_line);
	    disp_deselect();
	    free(color_line);

	    return char_width;
	}
    }

    int cx, cy;

    if (!font_transparent)
	_fillRect(x, y, char_width+1, cfont.y_size, _bg);

    // draw Glyph
    uint8_t mask = 0x80;
    disp_select();
    for (j=0; j < fontChar.height; j++) {
		for (i=0; i < fontChar.width; i++) {
			if (((i + (j*fontChar.width)) % 8) == 0) {
				mask = 0x80;
				ch = cfont.font[fontChar.dataPtr++];
			}

			if ((ch & mask) !=0) {
				cx = (uint16_t)(x+fontChar.xOffset+i);
				cy = (uint16_t)(y+j+fontChar.adjYOffset);
				_drawPixel(cx, cy, _fg, 0);
			}
			mask >>= 1;
		}
    }
    disp_deselect();

    return char_width;
}


// non-rotated fixed width character
//----------------------------------------------
static void printChar(uint8_t c, int x, int y) {
    uint8_t	i, j, ch, fz, mask;
    uint16_t	k, temp, cx, cy, len;

    // fz = bytes per char row
    fz = cfont.x_size/8;
    if (cfont.x_size % 8)
	fz++;

    // get character position in buffer
    temp = ((c-cfont.offset)*((fz)*cfont.y_size))+4;

    if ((font_buffered_char) && (!font_transparent)) {
	// === buffer Glyph data for faster sending ===
	len = cfont.x_size * cfont.y_size;
	color_t *color_line = heap_caps_malloc(len*3, MALLOC_CAP_DMA);
	if (color_line) {
	    // fill with background color
	    for (int n = 0; n < len; n++) {
		color_line[n] = _bg;
	    }
	    // set character pixels to foreground color
	    for (j = 0; j < cfont.y_size; j++) {
		for (k = 0; k < fz; k++) {
		    ch = cfont.font[temp + k];
		    mask = 0x80;
		    for (i = 0; i < 8; i++) {
			if ((ch & mask) !=0) {
			    color_line[(j*cfont.x_size) + (i+(k*8))] = _fg;
			}
			mask >>= 1;
		    }
		}
		temp += (fz);
	    }
	    // send to display in one transaction
	    disp_select();
	    send_data(x, y, x+cfont.x_size-1, y+cfont.y_size-1, len, color_line);
	    disp_deselect();
	    free(color_line);

	    return;
	}
    }

    if (!font_transparent)
	_fillRect(x, y, cfont.x_size, cfont.y_size, _bg);

    disp_select();
    for (j = 0; j < cfont.y_size; j++) {
	for (k = 0; k < fz; k++) {
	    ch = cfont.font[temp + k];
	    mask = 0x80;
	    for (i = 0; i < 8; i++) {
		if ((ch & mask) !=0) {
		    cx = (uint16_t)(x+i+(k*8));
		    cy = (uint16_t)(y+j);
		    _drawPixel(cx, cy, _fg, 0);
		}
		mask >>= 1;
	    }
	}
	temp += (fz);
    }
    disp_deselect();
}


// print rotated proportional character
// character is already in fontChar
//---------------------------------------------------
static int rotatePropChar(int x, int y, int offset) {
  uint8_t ch = 0;
  double radian = font_rotate * DEG_TO_RAD;
  float cos_radian = cos(radian);
  float sin_radian = sin(radian);

  uint8_t mask = 0x80;
  disp_select();
  for (int j=0; j < fontChar.height; j++) {
    for (int i=0; i < fontChar.width; i++) {
      if (((i + (j*fontChar.width)) % 8) == 0) {
        mask = 0x80;
        ch = cfont.font[fontChar.dataPtr++];
      }

      int newX = (int)(x + (((offset + i) * cos_radian) - ((j+fontChar.adjYOffset)*sin_radian)));
      int newY = (int)(y + (((j+fontChar.adjYOffset) * cos_radian) + ((offset + i) * sin_radian)));

      if ((ch & mask) != 0) _drawPixel(newX,newY,_fg, 0);
      else if (!font_transparent) _drawPixel(newX,newY,_bg, 0);

      mask >>= 1;
    }
  }
  disp_deselect();

  return fontChar.xDelta+1;
}

// rotated fixed width character
//--------------------------------------------------------
static void rotateChar(uint8_t c, int x, int y, int pos) {
  uint8_t i,j,ch,fz,mask;
  uint16_t temp;
  int newx,newy;
  double radian = font_rotate*0.0175;
  float cos_radian = cos(radian);
  float sin_radian = sin(radian);
  int zz;

  if( cfont.x_size < 8 ) fz = cfont.x_size;
  else fz = cfont.x_size/8;
  temp=((c-cfont.offset)*((fz)*cfont.y_size))+4;

  disp_select();
  for (j=0; j<cfont.y_size; j++) {
    for (zz=0; zz<(fz); zz++) {
      ch = cfont.font[temp+zz];
      mask = 0x80;
      for (i=0; i<8; i++) {
        newx=(int)(x+(((i+(zz*8)+(pos*cfont.x_size))*cos_radian)-((j)*sin_radian)));
        newy=(int)(y+(((j)*cos_radian)+((i+(zz*8)+(pos*cfont.x_size))*sin_radian)));

        if ((ch & mask) != 0) _drawPixel(newx,newy,_fg, 0);
        else if (!font_transparent) _drawPixel(newx,newy,_bg, 0);
        mask >>= 1;
      }
    }
    temp+=(fz);
  }
  disp_deselect();
  // calculate x,y for the next char
  TFT_X = (int)(x + ((pos+1) * cfont.x_size * cos_radian));
  TFT_Y = (int)(y + ((pos+1) * cfont.x_size * sin_radian));
}

//----------------------
static int _7seg_width()
{
	return (2 * (2 * cfont.y_size + 1)) + cfont.x_size;
}

//-----------------------
static int _7seg_height()
{
	return (3 * (2 * cfont.y_size + 1)) + (2 * cfont.x_size);
}

// Returns the string width in pixels.
// Useful for positions strings on the screen.
//===============================
int TFT_getStringWidth(char* str)
{
    int strWidth = 0;

	if (cfont.bitmap == 2) strWidth = ((_7seg_width()+2) * strlen(str)) - 2;	// 7-segment font
	else if (cfont.x_size != 0) strWidth = strlen(str) * cfont.x_size;			// fixed width font
	else {
		// calculate the width of the string of proportional characters
		char* tempStrptr = str;
		while (*tempStrptr != 0) {
			if (getCharPtr(*tempStrptr++)) {
				strWidth += (((fontChar.width > fontChar.xDelta) ? fontChar.width : fontChar.xDelta) + 1);
			}
		}
		strWidth--;
	}
	return strWidth;
}

//===============================================
void TFT_clearStringRect(int x, int y, char *str)
{
	int w = TFT_getStringWidth(str);
	int h = TFT_getfontheight();
	TFT_fillRect(x+dispWin.x1, y+dispWin.y1, w, h, _bg);
}

//==============================================================================
/**
 * bit-encoded bar position of all digits' bcd segments
 *
 *           6
 * 		  +-----+
 * 		3 |  .	| 2
 * 		  +--5--+
 * 		1 |  .	| 0
 * 		  +--.--+
 * 		     4
 */
static const uint16_t font_bcd[] = {
  0x200, // 0010 0000 0000  // -
  0x080, // 0000 1000 0000  // .
  0x06C, // 0100 0110 1100  // /, degree
  0x05f, // 0000 0101 1111, // 0
  0x005, // 0000 0000 0101, // 1
  0x076, // 0000 0111 0110, // 2
  0x075, // 0000 0111 0101, // 3
  0x02d, // 0000 0010 1101, // 4
  0x079, // 0000 0111 1001, // 5
  0x07b, // 0000 0111 1011, // 6
  0x045, // 0000 0100 0101, // 7
  0x07f, // 0000 0111 1111, // 8
  0x07d, // 0000 0111 1101  // 9
  0x900  // 1001 0000 0000  // :
};

//-----------------------------------------------------------------------------------------------
static void barVert(int16_t x, int16_t y, int16_t w, int16_t l, color_t color, color_t outline) {
  _fillTriangle(x+1, y+2*w, x+w, y+w+1, x+2*w-1, y+2*w, color);
  _fillTriangle(x+1, y+2*w+l+1, x+w, y+3*w+l, x+2*w-1, y+2*w+l+1, color);
  _fillRect(x, y+2*w+1, 2*w+1, l, color);
  if (cfont.offset) {
    _drawTriangle(x+1, y+2*w, x+w, y+w+1, x+2*w-1, y+2*w, outline);
    _drawTriangle(x+1, y+2*w+l+1, x+w, y+3*w+l, x+2*w-1, y+2*w+l+1, outline);
    _drawRect(x, y+2*w+1, 2*w+1, l, outline);
  }
}

//----------------------------------------------------------------------------------------------
static void barHor(int16_t x, int16_t y, int16_t w, int16_t l, color_t color, color_t outline) {
  _fillTriangle(x+2*w, y+2*w-1, x+w+1, y+w, x+2*w, y+1, color);
  _fillTriangle(x+2*w+l+1, y+2*w-1, x+3*w+l, y+w, x+2*w+l+1, y+1, color);
  _fillRect(x+2*w+1, y, l, 2*w+1, color);
  if (cfont.offset) {
    _drawTriangle(x+2*w, y+2*w-1, x+w+1, y+w, x+2*w, y+1, outline);
    _drawTriangle(x+2*w+l+1, y+2*w-1, x+3*w+l, y+w, x+2*w+l+1, y+1, outline);
    _drawRect(x+2*w+1, y, l, 2*w+1, outline);
  }
}

//--------------------------------------------------------------------------------------------
static void _draw7seg(int16_t x, int16_t y, int8_t num, int16_t w, int16_t l, color_t color) {
  /* TODO: clipping */
  if (num < 0x2D || num > 0x3A) return;

  int16_t c = font_bcd[num-0x2D];
  int16_t d = 2*w+l+1;

  // === Clear unused segments ===
  if (!(c & 0x001)) barVert(x+d, y+d, w, l, _bg, _bg);
  if (!(c & 0x002)) barVert(x,   y+d, w, l, _bg, _bg);
  if (!(c & 0x004)) barVert(x+d, y, w, l, _bg, _bg);
  if (!(c & 0x008)) barVert(x,   y, w, l, _bg, _bg);
  if (!(c & 0x010)) barHor(x, y+2*d, w, l, _bg, _bg);
  if (!(c & 0x020)) barHor(x, y+d, w, l, _bg, _bg);
  if (!(c & 0x040)) barHor(x, y, w, l, _bg, _bg);

  if (!(c & 0x080)) {
    // low point
    _fillRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, _bg);
    if (cfont.offset) _drawRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, _bg);
  }
  if (!(c & 0x100)) {
    // down middle point
    _fillRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, _bg);
    if (cfont.offset) _drawRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, _bg);
  }
  if (!(c & 0x800)) {
	// up middle point
    _fillRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, _bg);
    if (cfont.offset) _drawRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, _bg);
  }
  if (!(c & 0x200)) {
    // middle, minus
    _fillRect(x+2*w+1, y+d, l, 2*w+1, _bg);
    if (cfont.offset) _drawRect(x+2*w+1, y+d, l, 2*w+1, _bg);
  }

  // === Draw used segments ===
  if (c & 0x001) barVert(x+d, y+d, w, l, color, cfont.color);	// down right
  if (c & 0x002) barVert(x,   y+d, w, l, color, cfont.color);	// down left
  if (c & 0x004) barVert(x+d, y, w, l, color, cfont.color);		// up right
  if (c & 0x008) barVert(x,   y, w, l, color, cfont.color);		// up left
  if (c & 0x010) barHor(x, y+2*d, w, l, color, cfont.color);	// down
  if (c & 0x020) barHor(x, y+d, w, l, color, cfont.color);		// middle
  if (c & 0x040) barHor(x, y, w, l, color, cfont.color);		// up

  if (c & 0x080) {
    // low point
    _fillRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, color);
    if (cfont.offset) _drawRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, cfont.color);
  }
  if (c & 0x100) {
    // down middle point
    _fillRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, color);
    if (cfont.offset) _drawRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, cfont.color);
  }
  if (c & 0x800) {
	// up middle point
    _fillRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, color);
    if (cfont.offset) _drawRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, cfont.color);
  }
  if (c & 0x200) {
    // middle, minus
    _fillRect(x+2*w+1, y+d, l, 2*w+1, color);
    if (cfont.offset) _drawRect(x+2*w+1, y+d, l, 2*w+1, cfont.color);
  }
}
//==============================================================================

//======================================
void TFT_print(char *st, int x, int y) {
	int stl, i, tmpw, tmph, fh;
	uint8_t ch;

	if (cfont.bitmap == 0) return; // wrong font selected

	// ** Rotated strings cannot be aligned
	if ((font_rotate != 0) && ((x <= CENTER) || (y <= CENTER))) return;

	if ((x < LASTX) || (font_rotate == 0)) TFT_OFFSET = 0;

	if ((x >= LASTX) && (x < LASTY)) x = TFT_X + (x-LASTX);
	else if (x > CENTER) x += dispWin.x1;

	if (y >= LASTY) y = TFT_Y + (y-LASTY);
	else if (y > CENTER) y += dispWin.y1;

	// ** Get number of characters in string to print
	stl = strlen(st);

	// ** Calculate CENTER, RIGHT or BOTTOM position
	tmpw = TFT_getStringWidth(st);	// string width in pixels
	fh = cfont.y_size;			// font height
	if ((cfont.x_size != 0) && (cfont.bitmap == 2)) {
		// 7-segment font
		fh = (3 * (2 * cfont.y_size + 1)) + (2 * cfont.x_size);  // 7-seg character height
	}

	if (x == RIGHT) x = dispWin.x2 - tmpw + dispWin.x1;
	else if (x == CENTER) x = (((dispWin.x2 - dispWin.x1 + 1) - tmpw) / 2) + dispWin.x1;

	if (y == BOTTOM) y = dispWin.y2 - fh + dispWin.y1;
	else if (y==CENTER) y = (((dispWin.y2 - dispWin.y1 + 1) - (fh/2)) / 2) + dispWin.y1;

	if (x < dispWin.x1) x = dispWin.x1;
	if (y < dispWin.y1) y = dispWin.y1;
	if ((x > dispWin.x2) || (y > dispWin.y2)) return;

	TFT_X = x;
	TFT_Y = y;

	// ** Adjust y position
	tmph = cfont.y_size; // font height
	// for non-proportional fonts, char width is the same for all chars
	tmpw = cfont.x_size;
	if (cfont.x_size != 0) {
		if (cfont.bitmap == 2) {	// 7-segment font
			tmpw = _7seg_width();	// character width
			tmph = _7seg_height();	// character height
		}
	}
	else TFT_OFFSET = 0;	// fixed font; offset not needed

	if ((TFT_Y + tmph - 1) > dispWin.y2) return;

	int offset = TFT_OFFSET;

	for (i=0; i<stl; i++) {
		ch = st[i]; // get string character

		if (ch == 0x0D) { // === '\r', erase to eol ====
			if ((!font_transparent) && (font_rotate==0)) _fillRect(TFT_X, TFT_Y,  dispWin.x2+1-TFT_X, tmph, _bg);
		}

		else if (ch == 0x0A) { // ==== '\n', new line ====
			if (cfont.bitmap == 1) {
				TFT_Y += tmph + font_line_space;
				if (TFT_Y > (dispWin.y2-tmph)) break;
				TFT_X = dispWin.x1;
			}
		}

		else { // ==== other characters ====
			if (cfont.x_size == 0) {
				// for proportional font get character data to 'fontChar'
				if (getCharPtr(ch)) tmpw = fontChar.xDelta;
				else continue;
			}

			// check if character can be displayed in the current line
			if ((TFT_X+tmpw) > (dispWin.x2)) {
				if (text_wrap == 0) break;
				TFT_Y += tmph + font_line_space;
				if (TFT_Y > (dispWin.y2-tmph)) break;
				TFT_X = dispWin.x1;
			}

			// Let's print the character
			if (cfont.x_size == 0) {
				// == proportional font
				if (font_rotate == 0) TFT_X += printProportionalChar(TFT_X, TFT_Y) + 1;
				else {
					// rotated proportional font
					offset += rotatePropChar(x, y, offset);
					TFT_OFFSET = offset;
				}
			}
			else {
				if (cfont.bitmap == 1) {
					// == fixed font
					if ((ch < cfont.offset) || ((ch-cfont.offset) > cfont.numchars)) ch = cfont.offset;
					if (font_rotate == 0) {
						printChar(ch, TFT_X, TFT_Y);
						TFT_X += tmpw;
					}
					else rotateChar(ch, x, y, i);
				}
				else if (cfont.bitmap == 2) {
					// == 7-segment font ==
					_draw7seg(TFT_X, TFT_Y, ch, cfont.y_size, cfont.x_size, _fg);
					TFT_X += (tmpw + 2);
				}
			}
		}
	}
}


// ================ Service functions ==========================================

// Change the screen rotation.
// Input: m new rotation value (0 to 3)
//=================================
void TFT_setRotation(uint8_t rot) {
    if (rot > 3) {
        uint8_t madctl = (rot & 0xF8); // for testing, manually set MADCTL register
		if (disp_select() == ESP_OK) {
			disp_spi_transfer_cmd_data(TFT_MADCTL, &madctl, 1);
			disp_deselect();
		}
    }
	else {
		orientation = rot;
        _tft_setRotation(rot);
	}

	dispWin.x1 = 0;
	dispWin.y1 = 0;
	dispWin.x2 = _width-1;
	dispWin.y2 = _height-1;

	TFT_fillScreen(_bg);
}


// Select gamma curve
// Input: gamma = 0~3
//==================================
void TFT_setGammaCurve(uint8_t gm) {
  uint8_t gamma_curve = 1 << (gm & 0x03);
  disp_spi_transfer_cmd_data(TFT_CMD_GAMMASET, &gamma_curve, 1);
}


//=====================================================================
void TFT_setclipwin(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
	dispWin.x1 = x1;
	dispWin.y1 = y1;
	dispWin.x2 = x2;
	dispWin.y2 = y2;

	if (dispWin.x2 >= _width) dispWin.x2 = _width-1;
	if (dispWin.y2 >= _height) dispWin.y2 = _height-1;
	if (dispWin.x1 > dispWin.x2) dispWin.x1 = dispWin.x2;
	if (dispWin.y1 > dispWin.y2) dispWin.y1 = dispWin.y2;
}

//=====================
void TFT_resetclipwin()
{
	dispWin.x2 = _width-1;
	dispWin.y2 = _height-1;
	dispWin.x1 = 0;
	dispWin.y1 = 0;
}

//==========================================================================
void set_7seg_font_atrib(uint8_t l, uint8_t w, int outline, color_t color) {
	if (cfont.bitmap != 2) return;

	if (l < 6) l = 6;
    if (l > 40) l = 40;
    if (w < 1) w = 1;
    if (w > (l/2)) w = l/2;
    if (w > 12) w = 12;

    cfont.x_size = l;
	cfont.y_size = w;
	cfont.offset = outline;
	cfont.color  = color;
}

//==========================================
int TFT_getfontsize(int *width, int* height)
{
  if (cfont.bitmap == 1) {
    if (cfont.x_size != 0) *width = cfont.x_size;	// fixed width font
    else *width = cfont.max_x_size;					// proportional font
    *height = cfont.y_size;
  }
  else if (cfont.bitmap == 2) {
	// 7-segment font
    *width = _7seg_width();
    *height = _7seg_height();
  }
  else {
    *width = 0;
    *height = 0;
    return 0;
  }
  return 1;
}

//=====================
int TFT_getfontheight()
{
  if (cfont.bitmap == 1) return cfont.y_size;			// Bitmap font
  else if (cfont.bitmap == 2) return _7seg_height();	// 7-segment font
  return 0;
}

//====================
void TFT_saveClipWin()
{
	dispWinTemp.x1 = dispWin.x1;
	dispWinTemp.y1 = dispWin.y1;
	dispWinTemp.x2 = dispWin.x2;
	dispWinTemp.y2 = dispWin.y2;
}

//=======================
void TFT_restoreClipWin()
{
	dispWin.x1 = dispWinTemp.x1;
	dispWin.y1 = dispWinTemp.y1;
	dispWin.x2 = dispWinTemp.x2;
	dispWin.y2 = dispWinTemp.y2;
}


// ============= Touch panel functions =========================================

#if USE_TOUCH == TOUCH_TYPE_XPT2046
//-------------------------------------------------------
static int tp_get_data_xpt2046(uint8_t type, int samples)
{
	if (ts_spi == NULL) return 0;

	int n, result, val = 0;
	uint32_t i = 0;
	uint32_t vbuf[18];
	uint32_t minval, maxval, dif;

    if (samples < 3) samples = 1;
    if (samples > 18) samples = 18;

    // one dummy read
    result = touch_get_data(type);

    // read data
	while (i < 10) {
    	minval = 5000;
    	maxval = 0;
		// get values
		for (n=0;n<samples;n++) {
		    result = touch_get_data(type);
			if (result < 0) break;

			vbuf[n] = result;
			if (result < minval) minval = result;
			if (result > maxval) maxval = result;
		}
		if (result < 0) break;
		dif = maxval - minval;
		if (dif < 40) break;
		i++;
    }
	if (result < 0) return -1;

	if (samples > 2) {
		// remove one min value
		for (n = 0; n < samples; n++) {
			if (vbuf[n] == minval) {
				vbuf[n] = 5000;
				break;
			}
		}
		// remove one max value
		for (n = 0; n < samples; n++) {
			if (vbuf[n] == maxval) {
				vbuf[n] = 5000;
				break;
			}
		}
		for (n = 0; n < samples; n++) {
			if (vbuf[n] < 5000) val += vbuf[n];
		}
		val /= (samples-2);
	}
	else val = vbuf[0];

    return val;
}

//-----------------------------------------------
static int TFT_read_touch_xpt2046(int *x, int* y)
{
    int res = 0, result = -1;
	
    if (spi_lobo_device_select(ts_spi, 0) != ESP_OK)
	return 0;

    result = tp_get_data_xpt2046(0xB0, 3);  	// Z; pressure; touch detect
    if (result <= 15) goto exit;		// Z was 50, but near the origin it's just above 10.
   						// 26-6-2018 from 10 to 15.

	// touch panel pressed
	result = tp_get_data_xpt2046(0xD0, 6);
	if (result < 0) goto exit;
	*x = result;

	result = tp_get_data_xpt2046(0x90, 6);
	if (result < 0) goto exit;
	*y = result;
	res = 1;

exit:
	spi_lobo_device_deselect(ts_spi);
	return res;
}
#endif

//=============================================
int TFT_read_touch(int *x, int* y, uint8_t raw)
{
    *x = 0;
    *y = 0;
	
    if (ts_spi == NULL)
	return 0;
#if USE_TOUCH == TOUCH_TYPE_NONE
    return 0;
#else
    int result = -1;
    int X=0, Y=0;

#if USE_TOUCH == TOUCH_TYPE_XPT2046
    result = TFT_read_touch_xpt2046(&X, &Y);
    if (result == 0)
	return 0;
#elif USE_TOUCH == TOUCH_TYPE_STMPE610
    uint32_t tp_calx = TP_CALX_STMPE610;
    uint32_t tp_caly = TP_CALY_STMPE610;
    uint16_t Xx, Yy, Z=0;
    result = stmpe610_get_touch(&Xx, &Yy, &Z);
    if (result == 0) return 0;
    X = Xx;
    Y = Yy;
#else
    return 0;
#endif

    if (raw) {
    	*x = X;
    	*y = Y;
    	return 1;
    }

    // Calibrate the result
    int tmp;
    int xleft   = tp_xleft;   //(tp_calx >> 16) & 0x3FFF;
    int xright  = tp_xright;  //tp_calx & 0x3FFF;
    int ytop    = tp_ytop;    //(tp_caly >> 16) & 0x3FFF;
    int ybottom = tp_ybottom; //tp_caly & 0x3FFF;

   if (((xright - xleft) <= 0) || ((ytop - ybottom) <= 0))
	return 0;

#if USE_TOUCH == TOUCH_TYPE_XPT2046
//  printf("Raw %dx%d ", X, Y);
    // Received coordinates are always in portrait, origin left bottom.
    int width = DEFAULT_TFT_DISPLAY_WIDTH;
    int height = DEFAULT_TFT_DISPLAY_HEIGHT;
    X = ((X - xleft) * width)  / (xright - xleft);
    Y = ((Y - ybottom) * height) / (ytop - ybottom);

    if (X < 0)
	X = 0;
    if (X > width-1)
	X = width-1;
    if (Y < 0)
	Y = 0;
    if (Y > height-1)
	Y = height-1;

    switch (orientation) {
	case PORTRAIT:
		Y = height - Y - 1;
		break;
	case LANDSCAPE:
		tmp = X;
		X = height - Y - 1;
		Y = width - tmp - 1;
		break;
	case PORTRAIT_FLIP:
		X = width - X - 1;
		break;
	case LANDSCAPE_FLIP:
		tmp = X;
		X = Y;
		Y = tmp;
		break;
    }
//  printf("Cal %dx%d %dx%d  XxY %dx%d\n", xleft, ybottom, xright, ytop, X, Y);
#elif USE_TOUCH == TOUCH_TYPE_STMPE610
    int width = _width;
    int height = _height;
    if (_width > _height) {
	width = _height;
	height = _width;
    }
    X = ((X - xleft) * width) / (xright - xleft);
    Y = ((Y - ytop) * height) / (ybottom - ytop);

    if (X < 0) X = 0;
    if (X > width-1) X = width-1;
    if (Y < 0) Y = 0;
    if (Y > height-1) Y = height-1;

    switch (orientation) {
	case PORTRAIT_FLIP:
		X = width - X - 1;
		Y = height - Y - 1;
		break;
	case LANDSCAPE:
		tmp = X;
		X = Y;
		Y = width - tmp -1;
		break;
	case LANDSCAPE_FLIP:
		tmp = X;
		X = height - Y -1;
		Y = tmp;
		break;
    }
#endif
    *x = X;
    *y = Y;
    return 1;
#endif
}