lilybook/components/epaper/EPD.c

/* EPD library
 *
 *  Author: LoBo (loboris@gmail.com, loboris.github)
 *
 *  Module supporting SPI ePaper displays
*/

#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 "time.h"
#include <math.h>
#include "rom/tjpgd.h"
#include "esp_heap_alloc_caps.h"
#include "EPD.h"
#include "EPDspi.h"
#include "rom/tjpgd.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[];

// ==============================================================
// ==== Set default values of global variables ==================

uint8_t orientation = LANDSCAPE_0;	// 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 = EPD_BLACK;
color_t _bg = EPD_WHITE;

float _angleOffset = DEFAULT_ANGLE_OFFSET;

int	EPD_X = 0;
int	EPD_Y = 0;

dispWin_t dispWin = {
  .x1 = 0,
  .y1 = 0,
  .x2 = EPD_DISPLAY_WIDTH-1,
  .y2 = EPD_DISPLAY_HEIGHT-1,
};

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

uint8_t font_line_space = 0;
uint8_t image_debug = 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 EPD_OFFSET = 0;
static propFont	fontChar;
static float _arcAngleMax = DEFAULT_ARC_ANGLE_MAX;



// ==============================================================================================================
//----------------------------------------------
static void drawPixel(int x, int y, uint8_t val)
{
	if (orientation == LANDSCAPE_180) {
		x = _width - x - 1;
		y = _height - y - 1;
	}
	if (_gs) {
		val &= 0x0F;
		//if (gs_drawBuff[(y * _width) + x] != val) {
			gs_drawBuff[(y * _width) + x] = val;
			gs_used_shades |= (1<<val);
		//}
	}
	else {
		val &= 0x01;
		uint8_t buf_val = drawBuff[(x * (_height >> 3)) + (y>>3)];
		uint8_t new_val = buf_val;
		if (val) new_val &= (0x80 >> (y % 8)) ^ 0xFF;
		else new_val |= (0x80 >> (y % 8));
		//if (new_val != buf_val) drawBuff[(x * (_height>>3)) + (y>>3)] = new_val;
		drawBuff[(x * (_height>>3)) + (y>>3)] = new_val;
	}
}

//-------------------------------------------------------------------------
static void EPD_pushColorRep(int x1, int y1, int x2, int y2, color_t color)
{
	if (_gs == 0) color &= 0x01;
	else color &= 0x0F;
	for (int y=y1; y<=y2; y++) {
		for (int x = x1; x<=x2; x++){
			drawPixel(x, y, color);
		}
	}
}

/*
//---------------------------------------------------------------------------------------
static void copyBuff(int x1, int y1, int x2, int y2, uint8_t *src_buf, uint8_t *dest_buf)
{
	if
	uint8_t buf_val1 = (src_buf[(x1 * (EPD_DISPLAY_HEIGHT>>3)) + (y1>>3)]) ;
	uint8_t val = 0x80 >> (y1 % 8);
	if (val) buf_val &= (0x80 >> (y % 8)) ^ 0xFF;
	else buf_val |= (0x80 >> (y % 8));

	drawBuff[(x * (EPD_DISPLAY_HEIGHT>>3)) + (y>>3)] = buf_val;
}
*/

// ==============================================================================================================

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


// Compare two colors; return 0 if equal
//============================================
int EPD_compare_colors(color_t c1, color_t c2)
{
	if (c1 != c2) return 1;

	return 0;
}

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

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

//=======================================================
void EPD_drawPixel(int16_t x, int16_t y, color_t color) {

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

//--------------------------------------------------------------------------
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;
	EPD_pushColorRep(x, y, x, y+h-1, color);
}

//--------------------------------------------------------------------------
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;

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

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

//======================================================================
void EPD_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);
        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);
        else _drawFastHLine(xs, y0, dlen, color);
        dlen = 0; y0 += ystep; xs = x0 + 1;
      }
    }
    if (dlen) _drawFastHLine(xs, y0, dlen, color);
  }
}

//==============================================================================
void EPD_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;
	EPD_pushColorRep(x, y, x+w-1, y+h-1, color);
}

//============================================================================
void EPD_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 EPD_fillScreen(color_t color) {
	color &= 0x0F;
	memset(disp_buffer, ((color&1) ? 0 : 0xFF), _width * (_height/8));
	memset(gs_disp_buffer, color, _width * _height);
	gs_used_shades = 0;
}

//==================================
void EPD_fillWindow(color_t color) {
	EPD_pushColorRep(dispWin.x1, dispWin.y1, dispWin.x2, dispWin.y2, 0xFF);
}

// ^^^============= 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 EPD_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;

	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);
			_drawPixel(x0 + y, y0 + x, color);
		}
		if (cornername & 0x2) {
			_drawPixel(x0 + x, y0 - y, color);
			_drawPixel(x0 + y, y0 - x, color);
		}
		if (cornername & 0x8) {
			_drawPixel(x0 - y, y0 + x, color);
			_drawPixel(x0 - x, y0 + y, color);
		}
		if (cornername & 0x1) {
			_drawPixel(x0 - y, y0 - x, color);
			_drawPixel(x0 - x, y0 - y, color);
		}
	}
}

// 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 EPD_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 EPD_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);
}




//-----------------------------------------------------------------------------------------------
static void _drawLineByAngle(int16_t x, int16_t y, int16_t angle, uint16_t length, color_t color)
{
	_drawLine(
		x,
		y,
		x + length * cos((angle + _angleOffset) * DEG_TO_RAD),
		y + length * sin((angle + _angleOffset) * DEG_TO_RAD), color);
}

//---------------------------------------------------------------------------------------------------------------
static void _DrawLineByAngle(int16_t x, int16_t y, int16_t angle, uint16_t start, uint16_t length, color_t color)
{
	_drawLine(
		x + start * cos((angle + _angleOffset) * DEG_TO_RAD),
		y + start * sin((angle + _angleOffset) * DEG_TO_RAD),
		x + (start + length) * cos((angle + _angleOffset) * DEG_TO_RAD),
		y + (start + length) * sin((angle + _angleOffset) * DEG_TO_RAD), color);
}

//===========================================================================================================
void EPD_drawLineByAngle(uint16_t x, uint16_t y, uint16_t start, uint16_t len, uint16_t angle, color_t color)
{
	x += dispWin.x1;
	y += dispWin.y1;

	if (start == 0) _drawLineByAngle(x, y, angle, len, color);
	else _DrawLineByAngle(x, y, angle, start, len, 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);
}

//================================================================================================================
void EPD_drawTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
{
	x0 += dispWin.x1;
	y0 += dispWin.y1;
	x1 += dispWin.x1;
	y1 += dispWin.y1;
	x2 += dispWin.x1;
	y2 += dispWin.y1;

	_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 EPD_fillTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
{
	_fillTriangle(
			x0 + dispWin.x1, y0 + dispWin.y1,
			x1 + dispWin.x1, y1 + dispWin.y1,
			x2 + dispWin.x1, y2 + dispWin.y1,
			color);
}

//====================================================================
void EPD_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;

	_drawPixel(x, y + radius, color);
	_drawPixel(x, y - radius, color);
	_drawPixel(x + radius, y, color);
	_drawPixel(x - radius, y, color);
	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);
		_drawPixel(x - x1, y + y1, color);
		_drawPixel(x + x1, y - y1, color);
		_drawPixel(x - x1, y - y1, color);
		_drawPixel(x + y1, y + x1, color);
		_drawPixel(x - y1, y + x1, color);
		_drawPixel(x + y1, y - x1, color);
		_drawPixel(x - y1, y - x1, color);
	}
}

//====================================================================
void EPD_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);
}

//----------------------------------------------------------------------------------------------------------------
static void _draw_ellipse_section(uint16_t x, uint16_t y, uint16_t x0, uint16_t y0, color_t color, uint8_t option)
{
    // upper right
    if ( option & EPD_ELLIPSE_UPPER_RIGHT ) _drawPixel(x0 + x, y0 - y, color);
    // upper left
    if ( option & EPD_ELLIPSE_UPPER_LEFT ) _drawPixel(x0 - x, y0 - y, color);
    // lower right
    if ( option & EPD_ELLIPSE_LOWER_RIGHT ) _drawPixel(x0 + x, y0 + y, color);
    // lower left
    if ( option & EPD_ELLIPSE_LOWER_LEFT ) _drawPixel(x0 - x, y0 + y, color);
}

//=====================================================================================================
void EPD_drawEllipse(uint16_t x0, uint16_t y0, uint16_t rx, uint16_t ry, color_t color, uint8_t option)
{
	x0 += dispWin.x1;
	y0 += dispWin.y1;

	uint16_t x, y;
	int32_t xchg, ychg;
	int32_t err;
	int32_t rxrx2;
	int32_t ryry2;
	int32_t stopx, stopy;

	rxrx2 = rx;
	rxrx2 *= rx;
	rxrx2 *= 2;

	ryry2 = ry;
	ryry2 *= ry;
	ryry2 *= 2;

	x = rx;
	y = 0;

	xchg = 1;
	xchg -= rx;
	xchg -= rx;
	xchg *= ry;
	xchg *= ry;

	ychg = rx;
	ychg *= rx;

	err = 0;

	stopx = ryry2;
	stopx *= rx;
	stopy = 0;

	while( stopx >= stopy ) {
		_draw_ellipse_section(x, y, x0, y0, color, option);
		y++;
		stopy += rxrx2;
		err += ychg;
		ychg += rxrx2;
		if ( 2*err+xchg > 0 ) {
			x--;
			stopx -= ryry2;
			err += xchg;
			xchg += ryry2;
		}
	}

	x = 0;
	y = ry;

	xchg = ry;
	xchg *= ry;

	ychg = 1;
	ychg -= ry;
	ychg -= ry;
	ychg *= rx;
	ychg *= rx;

	err = 0;

	stopx = 0;

	stopy = rxrx2;
	stopy *= ry;

	while( stopx <= stopy ) {
		_draw_ellipse_section(x, y, x0, y0, color, option);
		x++;
		stopx += ryry2;
		err += xchg;
		xchg += ryry2;
		if ( 2*err+ychg > 0 ) {
			y--;
			stopy -= rxrx2;
			err += ychg;
			ychg += rxrx2;
		}
	}
}

//-----------------------------------------------------------------------------------------------------------------------
static void _draw_filled_ellipse_section(uint16_t x, uint16_t y, uint16_t x0, uint16_t y0, color_t color, uint8_t option)
{
    // upper right
    if ( option & EPD_ELLIPSE_UPPER_RIGHT ) _drawFastVLine(x0+x, y0-y, y+1, color);
    // upper left
    if ( option & EPD_ELLIPSE_UPPER_LEFT ) _drawFastVLine(x0-x, y0-y, y+1, color);
    // lower right
    if ( option & EPD_ELLIPSE_LOWER_RIGHT ) _drawFastVLine(x0+x, y0, y+1, color);
    // lower left
    if ( option & EPD_ELLIPSE_LOWER_LEFT ) _drawFastVLine(x0-x, y0, y+1, color);
}

//=====================================================================================================
void EPD_fillEllipse(uint16_t x0, uint16_t y0, uint16_t rx, uint16_t ry, color_t color, uint8_t option)
{
	x0 += dispWin.x1;
	y0 += dispWin.y1;

	uint16_t x, y;
	int32_t xchg, ychg;
	int32_t err;
	int32_t rxrx2;
	int32_t ryry2;
	int32_t stopx, stopy;

	rxrx2 = rx;
	rxrx2 *= rx;
	rxrx2 *= 2;

	ryry2 = ry;
	ryry2 *= ry;
	ryry2 *= 2;

	x = rx;
	y = 0;

	xchg = 1;
	xchg -= rx;
	xchg -= rx;
	xchg *= ry;
	xchg *= ry;

	ychg = rx;
	ychg *= rx;

	err = 0;

	stopx = ryry2;
	stopx *= rx;
	stopy = 0;

	while( stopx >= stopy ) {
		_draw_filled_ellipse_section(x, y, x0, y0, color, option);
		y++;
		stopy += rxrx2;
		err += ychg;
		ychg += rxrx2;
		if ( 2*err+xchg > 0 ) {
			x--;
			stopx -= ryry2;
			err += xchg;
			xchg += ryry2;
		}
	}

	x = 0;
	y = ry;

	xchg = ry;
	xchg *= ry;

	ychg = 1;
	ychg -= ry;
	ychg -= ry;
	ychg *= rx;
	ychg *= rx;

	err = 0;

	stopx = 0;

	stopy = rxrx2;
	stopy *= ry;

	while( stopx <= stopy ) {
		_draw_filled_ellipse_section(x, y, x0, y0, color, option);
		x++;
		stopx += ryry2;
		err += xchg;
		xchg += ryry2;
		if ( 2*err+ychg > 0 ) {
			y--;
			stopy -= rxrx2;
			err += ychg;
			ychg += rxrx2;
		}
	}
}


// ==== ARC DRAWING ===================================================================

//---------------------------------------------------------------------------------------------------------------------------------
static void _fillArcOffsetted(uint16_t cx, uint16_t cy, uint16_t radius, uint16_t thickness, float start, float end, color_t color)
{
	//float sslope = (float)cos_lookup(start) / (float)sin_lookup(start);
	//float eslope = (float)cos_lookup(end) / (float)sin_lookup(end);
	float sslope = (cos(start/_arcAngleMax * 2 * PI) * _arcAngleMax) / (sin(start/_arcAngleMax * 2 * PI) * _arcAngleMax) ;
	float eslope = (cos(end/_arcAngleMax * 2 * PI) * _arcAngleMax) / (sin(end/_arcAngleMax * 2 * PI) * _arcAngleMax);

	if (end == 360) eslope = -1000000;

	int ir2 = (radius - thickness) * (radius - thickness);
	int or2 = radius * radius;

	for (int x = -radius; x <= radius; x++) {
		for (int y = -radius; y <= radius; y++) {
			int x2 = x * x;
			int y2 = y * y;

			if (
				(x2 + y2 < or2 && x2 + y2 >= ir2) &&
				(
				(y > 0 && start < 180 && x <= y * sslope) ||
				(y < 0 && start > 180 && x >= y * sslope) ||
				(y < 0 && start <= 180) ||
				(y == 0 && start <= 180 && x < 0) ||
				(y == 0 && start == 0 && x > 0)
				) &&
				(
				(y > 0 && end < 180 && x >= y * eslope) ||
				(y < 0 && end > 180 && x <= y * eslope) ||
				(y > 0 && end >= 180) ||
				(y == 0 && end >= 180 && x < 0) ||
				(y == 0 && start == 0 && x > 0)
				)
				)
				_drawPixel(cx+x, cy+y, color);
		}
	}
}


//===========================================================================================================================
void EPD_drawArc(uint16_t cx, uint16_t cy, uint16_t r, uint16_t th, float start, float end, color_t color, color_t fillcolor)
{
	cx += dispWin.x1;
	cy += dispWin.y1;

	if (th < 1) th = 1;
	if (th > r) th = r;

	int f = EPD_compare_colors(fillcolor, color);

	float astart = fmodf(start, _arcAngleMax);
	float aend = fmodf(end, _arcAngleMax);

	astart += _angleOffset;
	aend += _angleOffset;

	if (astart < 0) astart += (float)360;
	if (aend < 0) aend += (float)360;

	if (aend == 0) aend = (float)360;

	if (astart > aend) {
		_fillArcOffsetted(cx, cy, r, th, astart, _arcAngleMax, fillcolor);
		_fillArcOffsetted(cx, cy, r, th, 0, aend, fillcolor);
		if (f) {
			_fillArcOffsetted(cx, cy, r, 1, astart, _arcAngleMax, color);
			_fillArcOffsetted(cx, cy, r, 1, 0, aend, color);
			_fillArcOffsetted(cx, cy, r-th, 1, astart, _arcAngleMax, color);
			_fillArcOffsetted(cx, cy, r-th, 1, 0, aend, color);
		}
	}
	else {
		_fillArcOffsetted(cx, cy, r, th, astart, aend, fillcolor);
		if (f) {
			_fillArcOffsetted(cx, cy, r, 1, astart, aend, color);
			_fillArcOffsetted(cx, cy, r-th, 1, astart, aend, color);
		}
	}
	if (f) {
		_drawLine(cx + (r-th) * cos(astart * DEG_TO_RAD), cy + (r-th) * sin(astart * DEG_TO_RAD),
			cx + (r-1) * cos(astart * DEG_TO_RAD), cy + (r-1) * sin(astart * DEG_TO_RAD), color);
		_drawLine(cx + (r-th) * cos(aend * DEG_TO_RAD), cy + (r-th) * sin(aend * DEG_TO_RAD),
			cx + (r-1) * cos(aend * DEG_TO_RAD), cy + (r-1) * sin(aend * DEG_TO_RAD), color);
	}
}

//=============================================================================================================
void EPD_drawPolygon(int cx, int cy, int sides, int diameter, color_t color, color_t fill, int rot, uint8_t th)
{
	cx += dispWin.x1;
	cy += dispWin.y1;

	int deg = rot - _angleOffset;
	int f = EPD_compare_colors(fill, color);

	if (sides < MIN_POLIGON_SIDES) sides = MIN_POLIGON_SIDES;	// This ensures the minimum side number
	if (sides > MAX_POLIGON_SIDES) sides = MAX_POLIGON_SIDES;	// This ensures the maximum side number

	int Xpoints[sides], Ypoints[sides];							// Set the arrays based on the number of sides entered
	int rads = 360 / sides;										// This equally spaces the points.

	for (int idx = 0; idx < sides; idx++) {
		Xpoints[idx] = cx + sin((float)(idx*rads + deg) * deg_to_rad) * diameter;
		Ypoints[idx] = cy + cos((float)(idx*rads + deg) * deg_to_rad) * diameter;
	}

	// Draw the polygon on the screen.
	if (f) {
		for(int idx = 0; idx < sides; idx++) {
			if((idx+1) < sides) _fillTriangle(cx,cy,Xpoints[idx],Ypoints[idx],Xpoints[idx+1],Ypoints[idx+1], fill);
			else _fillTriangle(cx,cy,Xpoints[idx],Ypoints[idx],Xpoints[0],Ypoints[0], fill);
		}
	}

	if (th) {
		for (int n=0; n<th; n++) {
			if (n > 0) {
				for (int idx = 0; idx < sides; idx++) {
					Xpoints[idx] = cx + sin((float)(idx*rads + deg) * deg_to_rad) * (diameter-n);
					Ypoints[idx] = cy + cos((float)(idx*rads + deg) * deg_to_rad) * (diameter-n);
				}
			}
			for(int idx = 0; idx < sides; idx++) {
				if( (idx+1) < sides)
					_drawLine(Xpoints[idx],Ypoints[idx],Xpoints[idx+1],Ypoints[idx+1], color); // draw the lines
				else
					_drawLine(Xpoints[idx],Ypoints[idx],Xpoints[0],Ypoints[0], color); // finishes the last line to close up the polygon.
			}
		}
	}
}

/*
// Similar to the Polygon function.
//=====================================================================================
void EPD_drawStar(int cx, int cy, int diameter, color_t color, bool fill, float factor)
{
	cx += dispWin.x1;
	cy += dispWin.y1;

	factor = constrain(factor, 1.0, 4.0);
	uint8_t sides = 5;
	uint8_t rads = 360 / sides;

	int Xpoints_O[sides], Ypoints_O[sides], Xpoints_I[sides], Ypoints_I[sides];//Xpoints_T[5], Ypoints_T[5];

	for(int idx = 0; idx < sides; idx++) {
		// makes the outer points
		Xpoints_O[idx] = cx + sin((float)(idx*rads + 72) * deg_to_rad) * diameter;
		Ypoints_O[idx] = cy + cos((float)(idx*rads + 72) * deg_to_rad) * diameter;
		// makes the inner points
		Xpoints_I[idx] = cx + sin((float)(idx*rads + 36) * deg_to_rad) * ((float)(diameter)/factor);
		// 36 is half of 72, and this will allow the inner and outer points to line up like a triangle.
		Ypoints_I[idx] = cy + cos((float)(idx*rads + 36) * deg_to_rad) * ((float)(diameter)/factor);
	}

	for(int idx = 0; idx < sides; idx++) {
		if((idx+1) < sides) {
			if(fill) {// this part below should be self explanatory. It fills in the star.
				_fillTriangle(cx,cy,Xpoints_I[idx],Ypoints_I[idx],Xpoints_O[idx],Ypoints_O[idx], color);
				_fillTriangle(cx,cy,Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx+1],Ypoints_I[idx+1], color);
			}
			else {
				_drawLine(Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx+1],Ypoints_I[idx+1], color);
				_drawLine(Xpoints_I[idx],Ypoints_I[idx],Xpoints_O[idx],Ypoints_O[idx], color);
			}
		}
		else {
			if(fill) {
				_fillTriangle(cx,cy,Xpoints_I[0],Ypoints_I[0],Xpoints_O[idx],Ypoints_O[idx], color);
				_fillTriangle(cx,cy,Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx],Ypoints_I[idx], color);
			}
			else {
				_drawLine(Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx],Ypoints_I[idx], color);
				_drawLine(Xpoints_I[0],Ypoints_I[0],Xpoints_O[idx],Ypoints_O[idx], 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 opening font file '%s'", fontfile);
		err = 1;
		goto exit;
    }

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

	userfont = malloc(fsize+4);
	if (userfont == NULL) {
		sprintf(err_msg, "Font memory allocation 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)",
					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;
}

//------------------------------------------------
int compile_font_file(char *fontfile, uint8_t dbg)
{
	int err = 0;
	char err_msg[128] = {'\0'};
	char outfile[128] = {'\0'};
	size_t len;
    struct stat sb;
    FILE *ffd = NULL;
    FILE *ffd_out = NULL;
    char *sourcebuf = NULL;

    len = strlen(fontfile);

	// check here that filename end with ".c".
	if ((len < 3) || (len > 125) || (strcmp(fontfile + len - 2, ".c") != 0)) {
		sprintf(err_msg, "not a .c file");
		err = 1;
		goto exit;
	}

	sprintf(outfile, "%s", fontfile);
	sprintf(outfile+strlen(outfile)-1, "fon");

	// Open the source file
    if (stat(fontfile, &sb) != 0) {
    	sprintf(err_msg, "Error opening source file '%s'", fontfile);
    	err = 2;
		goto exit;
    }
    // Open the file
    ffd = fopen(fontfile, "rb");
    if (!ffd) {
    	sprintf(err_msg, "Error opening source file '%s'", fontfile);
    	err = 3;
		goto exit;
    }

	// Open the font file
    ffd_out= fopen(outfile, "wb");
	if (!ffd_out) {
		sprintf(err_msg, "error opening destination file");
		err = 4;
		goto exit;
	}

	// Get file size
	int fsize = sb.st_size;
	if (fsize <= 0) {
		sprintf(err_msg, "source file size error");
		err = 5;
		goto exit;
	}

	sourcebuf = malloc(fsize+4);
	if (sourcebuf == NULL) {
		sprintf(err_msg, "memory allocation error");
		err = 6;
		goto exit;
	}
	char *fbuf = sourcebuf;

	int rdsize = fread(fbuf, 1, fsize, ffd);
	fclose(ffd);
	ffd = NULL;

	if (rdsize != fsize) {
		sprintf(err_msg, "error reading from source file");
		err = 7;
		goto exit;
	}

	*(fbuf+rdsize) = '\0';

	fbuf = strchr(fbuf, '{');			// beginning of font data
	char *fend = strstr(fbuf, "};");	// end of font data

	if ((fbuf == NULL) || (fend == NULL) || ((fend-fbuf) < 22)) {
		sprintf(err_msg, "wrong source file format");
		err = 8;
		goto exit;
	}

	fbuf++;
	*fend = '\0';
	char hexstr[5] = {'\0'};
	int lastline = 0;

	fbuf = strstr(fbuf, "0x");
	int size = 0;
	char *nextline;
	char *numptr;

	int bptr = 0;

	while ((fbuf != NULL) && (fbuf < fend) && (lastline == 0)) {
		nextline = strchr(fbuf, '\n'); // beginning of the next line
		if (nextline == NULL) {
			nextline = fend-1;
			lastline++;
		}
		else nextline++;

		while (fbuf < nextline) {
			numptr = strstr(fbuf, "0x");
			if ((numptr == NULL) || ((fbuf+4) > nextline)) numptr = strstr(fbuf, "0X");
			if ((numptr != NULL) && ((numptr+4) <= nextline)) {
				fbuf = numptr;
				if (bptr >= 128) {
					// buffer full, write to file
                    if (fwrite(outfile, 1, 128, ffd_out) != 128) goto error;
					bptr = 0;
					size += 128;
				}
				memcpy(hexstr, fbuf, 4);
				hexstr[4] = 0;
				outfile[bptr++] = (uint8_t)strtol(hexstr, NULL, 0);
				fbuf += 4;
			}
			else fbuf = nextline;
		}
		fbuf = nextline;
	}

	if (bptr > 0) {
		size += bptr;
        if (fwrite(outfile, 1, bptr, ffd_out) != bptr) goto error;
	}

	// write font ID
	sprintf(outfile, "RPH_font");
    if (fwrite(outfile, 1, 8, ffd_out) != 8) goto error;

	// === Test compiled font ===
	sprintf(outfile, "%s", fontfile);
	sprintf(outfile+strlen(outfile)-1, "fon");

	uint8_t *uf = userfont; // save userfont pointer
	userfont = NULL;
	if (load_file_font(outfile, 1) == 0) {
		sprintf(err_msg, "Error compiling file!");
	}
	else {
		free(userfont);
		sprintf(err_msg, "File compiled successfully.");
	}
	userfont = uf; // restore userfont

	goto exit;

error:
	sprintf(err_msg, "error writing to destination file");
	err = 9;

exit:
	if (sourcebuf) free(sourcebuf);
	if (ffd) fclose(ffd);
	if (ffd_out) fclose(ffd_out);

	if (dbg) printf("%s\r\n", 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)
//---------------------------------------------------------------------------------------------

//----------------------------------
void getFontCharacters(uint8_t *buf)
{
    if (cfont.bitmap == 2) {
    	//For 7 segment font only characters 0,1,2,3,4,5,6,7,8,9, . , - , : , / are available.
		for (uint8_t n=0; n < 11; n++) {
			buf[n] = n + 0x30;
		}
		buf[11] = '.';
		buf[12] = '-';
		buf[13] = '/';
		buf[14] = '\0';
    	return;
    }

    if (cfont.x_size > 0) {
		for (uint8_t n=0; n < cfont.numchars; n++) {
			buf[n] = cfont.offset + n;
		}
		buf[cfont.numchars] = '\0';
		return;
	}

	uint16_t tempPtr = 4; // point at first char data
	uint8_t cc, cw, ch, n;

	n = 0;
    cc = cfont.font[tempPtr++];
    while (cc != 0xFF)  {
    	cfont.numchars++;
        tempPtr++;
        cw = cfont.font[tempPtr++];
        ch = cfont.font[tempPtr++];
        tempPtr++;
        tempPtr++;
		if (cw != 0) {
			// packed bits
			tempPtr += (((cw * ch)-1) / 8) + 1;
		}
		buf[n++] = cc;
	    cc = cfont.font[tempPtr++];
	}
	buf[n] = '\0';
}

// 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;
}

/*
//-----------------------
static void _testFont() {
  if (cfont.x_size) {
	  printf("FONT TEST: fixed font\r\n");
	  return;
  }
  uint16_t tempPtr = 4; // point at first char data
  uint8_t c = 0x20;
  for (c=0x20; c <0xFF; c++) {
	fontChar.charCode = cfont.font[tempPtr++];
    if (fontChar.charCode == 0xFF) break;
    if (fontChar.charCode != c) {
    	printf("FONT TEST: last sequential char: %d, expected %d\r\n", fontChar.charCode, c);
    	break;
    }
    c = fontChar.charCode;
    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 (fontChar.charCode != 0xFF) {
      if (fontChar.width != 0) {
        // packed bits
        tempPtr += (((fontChar.width * fontChar.height)-1) / 8) + 1;
      }
    }
  }
  printf("FONT TEST: W=%d  H=%d last char: %d [%c]; length: %d\r\n", cfont.max_x_size, cfont.y_size, c, c, tempPtr);
}
*/

//===================================================
void EPD_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 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);
	int cx, cy;

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

	// draw Glyph
	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) {
				cx = (uint16_t)(x+fontChar.xOffset+i);
				cy = (uint16_t)(y+j+fontChar.adjYOffset);
				_drawPixel(cx, cy, _fg);
			}
			mask >>= 1;
		}
	}

	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;

	// 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_transparent) _fillRect(x, y, cfont.x_size, cfont.y_size, _bg);

	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);
				}
				mask >>= 1;
			}
		}
		temp += (fz);
	}
}

// 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;
  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);
      else if (!font_transparent) _drawPixel(newX,newY,_bg);

      mask >>= 1;
    }
  }

  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;

  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);
        else if (!font_transparent) _drawPixel(newx,newy,_bg);
        mask >>= 1;
      }
    }
    temp+=(fz);
  }
  // calculate x,y for the next char
  EPD_X = (int)(x + ((pos+1) * cfont.x_size * cos_radian));
  EPD_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 EPD_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 EPD_clearStringRect(int x, int y, char *str)
{
	int w = EPD_getStringWidth(str);
	int h = EPD_getfontheight();
	EPD_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);
  }
  */
  barVert(x+d, y+d, w, l, _bg, _bg);
  barVert(x,   y+d, w, l, _bg, _bg);
  barVert(x+d, y, w, l, _bg, _bg);
  barVert(x,   y, w, l, _bg, _bg);
  barHor(x, y+2*d, w, l, _bg, _bg);
  barHor(x, y+d, w, l, _bg, _bg);
  barHor(x, y, w, l, _bg, _bg);

  _fillRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, _bg);
  _drawRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, _bg);
  _fillRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, _bg);
  _drawRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, _bg);
  _fillRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, _bg);
  _drawRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, _bg);
  _fillRect(x+2*w+1, y+d, l, 2*w+1, _bg);
  _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 EPD_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)) EPD_OFFSET = 0;

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

	if (y >= LASTY) y = EPD_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 = EPD_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;

	 EPD_X = x;
	 EPD_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 EPD_OFFSET = 0;	// fixed font; offset not needed

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

	int offset = EPD_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( EPD_X, EPD_Y,  dispWin.x2+1- EPD_X, tmph, _bg);
		}

		else if (ch == 0x0A) { // ==== '\n', new line ====
			if (cfont.bitmap == 1) {
				 EPD_Y += tmph + font_line_space;
				if ( EPD_Y > (dispWin.y2-tmph)) break;
				 EPD_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 (( EPD_X+tmpw) > (dispWin.x2)) {
				if (text_wrap == 0) break;
				 EPD_Y += tmph + font_line_space;
				if ( EPD_Y > (dispWin.y2-tmph)) break;
				 EPD_X = dispWin.x1;
			}

			// Let's print the character
			if (cfont.x_size == 0) {
				// == proportional font
				if (font_rotate == 0) EPD_X += printProportionalChar( EPD_X, EPD_Y) + 1;
				else {
					// rotated proportional font
					offset += rotatePropChar(x, y, offset);
					EPD_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, EPD_X, EPD_Y);
						 EPD_X += tmpw;
					}
					else rotateChar(ch, x, y, i);
				}
				else if (cfont.bitmap == 2) {
					// == 7-segment font ==
					_draw7seg( EPD_X, EPD_Y, ch, cfont.y_size, cfont.x_size, _fg);
					 EPD_X += (tmpw + 2);
				}
			}
		}
	}
}


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

//=====================================================================
void EPD_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 >= EPD_DISPLAY_WIDTH) dispWin.x2 = EPD_DISPLAY_WIDTH-1;
	if (dispWin.y2 >= EPD_DISPLAY_HEIGHT) dispWin.y2 = EPD_DISPLAY_HEIGHT-1;
	if (dispWin.x1 > dispWin.x2) dispWin.x1 = dispWin.x2;
	if (dispWin.y1 > dispWin.y2) dispWin.y1 = dispWin.y2;
}

//=====================
void EPD_resetclipwin()
{
	dispWin.x2 = EPD_DISPLAY_WIDTH-1;
	dispWin.y2 = EPD_DISPLAY_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 EPD_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 EPD_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 EPD_saveClipWin()
{
	dispWinTemp.x1 = dispWin.x1;
	dispWinTemp.y1 = dispWin.y1;
	dispWinTemp.x2 = dispWin.x2;
	dispWinTemp.y2 = dispWin.y2;
}

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


// ================ JPG SUPPORT ================================================

// RGB to GRAYSCALE constants
// 0.2989  0.5870  0.1140
#define GS_FACT_R 0.2989
#define GS_FACT_G 0.4870
#define GS_FACT_B 0.2140

// User defined device identifier
typedef struct {
	FILE		*fhndl;			// File handler for input function
    int			x;				// image top left point X position
    int			y;				// image top left point Y position
    uint8_t		*membuff;		// memory buffer containing the image
    uint32_t	bufsize;		// size of the memory buffer
    uint32_t	bufptr;			// memory buffer current position
} JPGIODEV;


// User defined call-back function to input JPEG data from file
//---------------------
static UINT tjd_input (
	JDEC* jd,		// Decompression object
	BYTE* buff,		// Pointer to the read buffer (NULL:skip)
	UINT nd			// Number of bytes to read/skip from input stream
)
{
	int rb = 0;
	// Device identifier for the session (5th argument of jd_prepare function)
	JPGIODEV *dev = (JPGIODEV*)jd->device;

	if (buff) {	// Read nd bytes from the input strem
		rb = fread(buff, 1, nd, dev->fhndl);
		return rb;	// Returns actual number of bytes read
	}
	else {	// Remove nd bytes from the input stream
		if (fseek(dev->fhndl, nd, SEEK_CUR) >= 0) return nd;
		else return 0;
	}
}

// User defined call-back function to input JPEG data from memory buffer
//-------------------------
static UINT tjd_buf_input (
	JDEC* jd,		// Decompression object
	BYTE* buff,		// Pointer to the read buffer (NULL:skip)
	UINT nd			// Number of bytes to read/skip from input stream
)
{
	// Device identifier for the session (5th argument of jd_prepare function)
	JPGIODEV *dev = (JPGIODEV*)jd->device;
	if (!dev->membuff) return 0;
	if (dev->bufptr >= (dev->bufsize + 2)) return 0; // end of stream

	if ((dev->bufptr + nd) > (dev->bufsize + 2)) nd = (dev->bufsize + 2) - dev->bufptr;

	if (buff) {	// Read nd bytes from the input strem
		memcpy(buff, dev->membuff + dev->bufptr, nd);
		dev->bufptr += nd;
		return nd;	// Returns number of bytes read
	}
	else {	// Remove nd bytes from the input stream
		dev->bufptr += nd;
		return nd;
	}
}

// User defined call-back function to output RGB bitmap to display device
//----------------------
static UINT tjd_output (
	JDEC* jd,		// Decompression object of current session
	void* bitmap,	// Bitmap data to be output
	JRECT* rect		// Rectangular region to output
)
{
	// Device identifier for the session (5th argument of jd_prepare function)
	JPGIODEV *dev = (JPGIODEV*)jd->device;

	// ** Put the rectangular into the display device **
	int x;
	int y;
	int dleft, dtop, dright, dbottom;
	BYTE *src = (BYTE*)bitmap;

	int left = rect->left + dev->x;
	int top = rect->top + dev->y;
	int right = rect->right + dev->x;
	int bottom = rect->bottom + dev->y;

	if ((left > dispWin.x2) || (top > dispWin.y2)) return 1;	// out of screen area, return
	if ((right < dispWin.x1) || (bottom < dispWin.y1)) return 1;// out of screen area, return

	if (left < dispWin.x1) dleft = dispWin.x1;
	else dleft = left;
	if (top < dispWin.y1) dtop = dispWin.y1;
	else dtop = top;
	if (right > dispWin.x2) dright = dispWin.x2;
	else dright = right;
	if (bottom > dispWin.y2) dbottom = dispWin.y2;
	else dbottom = bottom;

	if ((dleft > dispWin.x2) || (dtop > dispWin.y2)) return 1;		// out of screen area, return
	if ((dright < dispWin.x1) || (dbottom < dispWin.y1)) return 1;	// out of screen area, return

	uint32_t len = ((dright-dleft+1) * (dbottom-dtop+1));	// calculate length of data

	float gs_clr = 0;
	uint8_t rgb_color[3];
	uint8_t last_lvl, i;
	uint8_t pix;
	if ((len > 0) && (len <= JPG_IMAGE_LINE_BUF_SIZE)) {
		for (y = top; y <= bottom; y++) {
			for (x = left; x <= right; x++) {
				// Clip to display area
				if ((x >= dleft) && (y >= dtop) && (x <= dright) && (y <= dbottom)) {
					// Directly convert color to 4-bit gray scale
					pix = 0;
					pix |= ((*src++) >> 4) & 0x08;
					pix |= ((*src++) >> 5) & 0x06;
					pix |= ((*src++) >> 7);
					pix ^= 0x0F;


					/* Convert rgb color to gray scale
					memcpy(rgb_color, src, 3);
					src += 3;
				    gs_clr = (GS_FACT_R * rgb_color[0]) + (GS_FACT_G * rgb_color[1]) + (GS_FACT_B * rgb_color[2]);
				    if (gs_clr > 255) gs_clr = 255;
				    // Use only 4 bits & invert
				    //pix = ((uint8_t)gs_clr >> 4) ^ 0x0F;
				    pix = (uint8_t)gs_clr;

				    // Using gray scale lookup table
				    last_lvl = 0;
				    i = 0;
				    for (i=0; i<16; i++) {
				    	if ((pix > last_lvl) && (pix <= lvl_buf_jpg[i])) {
				    		pix = 15 - i;
					        last_lvl = lvl_buf[i];
					        break;
				        }
				        last_lvl = lvl_buf[i];
				    }
					*/
				    gs_disp_buffer[(y * EPD_DISPLAY_WIDTH) + x] = pix;
		    		gs_used_shades |= (1 << pix);
				}
				else src += 3; // skip
			}
		}
	}
	else {
		printf("Data size error: %d jpg: (%d,%d,%d,%d) disp: (%d,%d,%d,%d)\r\n", len, left,top,right,bottom, dleft,dtop,dright,dbottom);
		return 0;  // stop decompression
	}

	return 1;	// Continue to decompression
}

// X & Y can be < 0 !
//=================================================================================
int EPD_jpg_image(int x, int y, uint8_t scale, char *fname, uint8_t *buf, int size)
{
	JPGIODEV dev;
    struct stat sb;
	char *work = NULL;		// Pointer to the working buffer (must be 4-byte aligned)
	UINT sz_work = 3800;	// Size of the working buffer (must be power of 2)
	JDEC jd;				// Decompression object (70 bytes)
	JRESULT rc;
	int res = -10;

    if (fname == NULL) {
    	// image from buffer
    	dev.fhndl = NULL;
        dev.membuff = buf;
        dev.bufsize = size;
        dev.bufptr = 0;
    }
    else {
    	// image from file
        dev.membuff = NULL;
        dev.bufsize = 0;
        dev.bufptr = 0;

        if (stat(fname, &sb) != 0) {
        	if (image_debug) printf("File error: %ss\r\n", strerror(errno));
        	res = -11;
            goto exit;
        }

        dev.fhndl = fopen(fname, "r");
        if (!dev.fhndl) {
        	if (image_debug) printf("Error opening file: %s\r\n", strerror(errno));
        	res = -12;
            goto exit;
        }
    }

	if (scale > 3) scale = 3;

	work = malloc(sz_work);
	if (work) {
		if (dev.membuff) rc = jd_prepare(&jd, tjd_buf_input, (void *)work, sz_work, &dev);
		else rc = jd_prepare(&jd, tjd_input, (void *)work, sz_work, &dev);
		if (rc == JDR_OK) {
			if (x == CENTER) x = ((dispWin.x2 - dispWin.x1 + 1 - (int)(jd.width >> scale)) / 2) + dispWin.x1;
			else if (x == RIGHT) x = dispWin.x2 + 1 - (int)(jd.width >> scale);

			if (y == CENTER) y = ((dispWin.y2 - dispWin.y1 + 1 - (int)(jd.height >> scale)) / 2) + dispWin.y1;
			else if (y == BOTTOM) y = dispWin.y2 + 1 - (int)(jd.height >> scale);

			if (x < ((dispWin.x2-1) * -1)) x = (dispWin.x2-1) * -1;
			if (y < ((dispWin.y2-1)) * -1) y = (dispWin.y2-1) * -1;
			if (x > (dispWin.x2-1)) x = dispWin.x2 - 1;
			if (y > (dispWin.y2-1)) y = dispWin.y2-1;

			dev.x = x;
			dev.y = y;

			// Start to decode the JPEG file
			rc = jd_decomp(&jd, tjd_output, scale);

			if (rc != JDR_OK) {
				if (image_debug) printf("jpg decompression error %d\r\n", rc);
				res = rc * -1;
			}
			res = 0;
			if (image_debug) printf("Jpg size: %dx%d, position; %d,%d, scale: %d, bytes used: %d\r\n", jd.width, jd.height, x, y, scale, jd.sz_pool);
		}
		else {
			if (image_debug) printf("jpg prepare error %d\r\n", rc);
			res = rc * -1;
		}
	}
	else {
		if (image_debug) printf("work buffer allocation error\r\n");
		res = -13;
	}

exit:
	if (work) free(work);  // free work buffer
    if (dev.fhndl) fclose(dev.fhndl);  // close input file
    return res;
}