examples/common/m5stack-tft/repo/components/tft/tft.c

   1 /* TFT module
   2  *
   3  *  Author: LoBo (loboris@gmail.com, loboris.github)
   4  *
   5  *  Module supporting SPI TFT displays based on ILI9341 & ILI9488 controllers
   6 */
   7
   8 #include <stdio.h>
   9 #include <errno.h>
  10 #include <sys/stat.h>
  11 #include <string.h>
  12 #include "freertos/FreeRTOS.h"
  13 #include "tft.h"
  14 #include <math.h>
  15 #include "esp32/rom/tjpgd.h"
  16
  17
  18 #define DEG_TO_RAD 0.01745329252
  19 #define deg_to_rad 0.01745329252 + 3.14159265359
  20 #define swap(a, b) { int16_t t = a; a = b; b = t; }
  21
  22 #if !defined(max)
  23 #define max(A,B) ( (A) > (B) ? (A):(B))
  24 #endif
  25 #if !defined(min)
  26 #define min(A,B) ( (A) < (B) ? (A):(B))
  27 #endif
  28
  29 // Embedded fonts
  30 extern uint8_t tft_SmallFont[];
  31 extern uint8_t tft_DefaultFont[];
  32 extern uint8_t tft_Dejavu18[];
  33 extern uint8_t tft_Dejavu24[];
  34 extern uint8_t tft_Ubuntu16[];
  35 extern uint8_t tft_Comic24[];
  36 extern uint8_t tft_minya24[];
  37 extern uint8_t tft_tooney32[];
  38 extern uint8_t tft_def_small[];
  39
  40 // ==== Color definitions constants ==============
  41 const color_t TFT_BLACK       = {   0,   0,   0 };
  42 const color_t TFT_NAVY        = {   0,   0, 128 };
  43 const color_t TFT_DARKGREEN   = {   0, 128,   0 };
  44 const color_t TFT_DARKCYAN    = {   0, 128, 128 };
  45 const color_t TFT_MAROON      = { 128,   0,   0 };
  46 const color_t TFT_PURPLE      = { 128,   0, 128 };
  47 const color_t TFT_OLIVE       = { 128, 128,   0 };
  48 const color_t TFT_LIGHTGREY   = { 192, 192, 192 };
  49 const color_t TFT_DARKGREY    = { 128, 128, 128 };
  50 const color_t TFT_BLUE        = {   0,   0, 255 };
  51 const color_t TFT_GREEN       = {   0, 255,   0 };
  52 const color_t TFT_CYAN        = {   0, 255, 255 };
  53 const color_t TFT_RED         = { 252,   0,   0 };
  54 const color_t TFT_MAGENTA     = { 252,   0, 255 };
  55 const color_t TFT_YELLOW      = { 252, 252,   0 };
  56 const color_t TFT_WHITE       = { 252, 252, 252 };
  57 const color_t TFT_ORANGE      = { 252, 164,   0 };
  58 const color_t TFT_GREENYELLOW = { 172, 252,  44 };
  59 const color_t TFT_PINK        = { 252, 192, 202 };
  60 // ===============================================
  61
  62 // ==============================================================
  63 // ==== Set default values of global variables ==================
  64 uint8_t tft_orientation = LANDSCAPE;// screen tft_orientation
  65 uint16_t tft_font_rotate = 0;           // font rotation
  66 uint8_t tft_font_transparent = 0;
  67 uint8_t tft_font_forceFixed = 0;
  68 uint8_t tft_text_wrap = 0;                      // character wrapping to new line
  69 color_t tft_fg = {  0, 255,   0};
  70 color_t tft_bg = {  0,   0,   0};
  71 uint8_t tft_image_debug = 0;
  72
  73 float tft_angleOffset = DEFAULT_ANGLE_OFFSET;
  74
  75 int     tft_x = 0;
  76 int     tft_y = 0;
  77
  78 uint32_t tft_tp_calx = 7472920;
  79 uint32_t tft_tp_caly = 122224794;
  80
  81 dispWin_t tft_dispWin = {
  82   .x1 = TFT_STATIC_WIDTH_OFFSET,
  83   .y1 = TFT_STATIC_HEIGHT_OFFSET,
  84   .x2 = DEFAULT_TFT_DISPLAY_WIDTH + TFT_STATIC_WIDTH_OFFSET,
  85   .y2 = DEFAULT_TFT_DISPLAY_HEIGHT + TFT_STATIC_HEIGHT_OFFSET,
  86 };
  87
  88 Font tft_cfont = {
  89         .font = tft_DefaultFont,
  90         .x_size = 0,
  91         .y_size = 0x0B,
  92         .offset = 0,
  93         .numchars = 95,
  94         .bitmap = 1,
  95 };
  96
  97 uint8_t tft_font_buffered_char = 1;
  98 uint8_t tft_font_line_space = 0;
  99 // ==============================================================
 100
 101
 102 typedef struct {
 103       uint8_t charCode;
 104       int adjYOffset;
 105       int width;
 106       int height;
 107       int xOffset;
 108       int xDelta;
 109       uint16_t dataPtr;
 110 } propFont;
 111
 112 static dispWin_t dispWinTemp;
 113
 114 static uint8_t *userfont = NULL;
 115 static int TFT_OFFSET = 0;
 116 static propFont fontChar;
 117 static float _arcAngleMax = DEFAULT_ARC_ANGLE_MAX;
 118
 119
 120 // =========================================================================
 121 // ** All drawings are clipped to 'tft_dispWin' **
 122 // ** All x,y coordinates in public functions are relative to clip window **
 123 // =========== : Public functions
 124 // ----------- : Local functions
 125 // =========================================================================
 126
 127
 128 // Compare two colors; return 0 if equal
 129 //============================================
 130 int TFT_compare_colors(color_t c1, color_t c2)
 131 {
 132         if ((c1.r & 0xFC) != (c2.r & 0xFC)) return 1;
 133         if ((c1.g & 0xFC) != (c2.g & 0xFC)) return 1;
 134         if ((c1.b & 0xFC) != (c2.b & 0xFC)) return 1;
 135
 136         return 0;
 137 }
 138
 139 // draw color pixel on screen
 140 //------------------------------------------------------------------------
 141 static void _drawPixel(int16_t x, int16_t y, color_t color, uint8_t sel) {
 142
 143         if ((x < tft_dispWin.x1) || (y < tft_dispWin.y1) || (x > tft_dispWin.x2) || (y > tft_dispWin.y2)) return;
 144         drawPixel(x, y, color, sel);
 145 }
 146
 147 //====================================================================
 148 void TFT_drawPixel(int16_t x, int16_t y, color_t color, uint8_t sel) {
 149
 150         _drawPixel(x+tft_dispWin.x1, y+tft_dispWin.y1, color, sel);
 151 }
 152
 153 //===========================================
 154 color_t TFT_readPixel(int16_t x, int16_t y) {
 155
 156   if ((x < tft_dispWin.x1) || (y < tft_dispWin.y1) || (x > tft_dispWin.x2) || (y > tft_dispWin.y2)) return TFT_BLACK;
 157
 158   return readPixel(x, y);
 159 }
 160
 161 //--------------------------------------------------------------------------
 162 static void _drawFastVLine(int16_t x, int16_t y, int16_t h, color_t color) {
 163         // clipping
 164         if ((x < tft_dispWin.x1) || (x > tft_dispWin.x2) || (y > tft_dispWin.y2)) return;
 165         if (y < tft_dispWin.y1) {
 166                 h -= (tft_dispWin.y1 - y);
 167                 y = tft_dispWin.y1;
 168         }
 169         if (h < 0) h = 0;
 170         if ((y + h) > (tft_dispWin.y2+1)) h = tft_dispWin.y2 - y + 1;
 171         if (h == 0) h = 1;
 172         TFT_pushColorRep(x, y, x, y+h-1, color, (uint32_t)h);
 173 }
 174
 175 //--------------------------------------------------------------------------
 176 static void _drawFastHLine(int16_t x, int16_t y, int16_t w, color_t color) {
 177         // clipping
 178         if ((y < tft_dispWin.y1) || (x > tft_dispWin.x2) || (y > tft_dispWin.y2)) return;
 179         if (x < tft_dispWin.x1) {
 180                 w -= (tft_dispWin.x1 - x);
 181                 x = tft_dispWin.x1;
 182         }
 183         if (w < 0) w = 0;
 184         if ((x + w) > (tft_dispWin.x2+1)) w = tft_dispWin.x2 - x + 1;
 185         if (w == 0) w = 1;
 186
 187         TFT_pushColorRep(x, y, x+w-1, y, color, (uint32_t)w);
 188 }
 189
 190 //======================================================================
 191 void TFT_drawFastVLine(int16_t x, int16_t y, int16_t h, color_t color) {
 192         _drawFastVLine(x+tft_dispWin.x1, y+tft_dispWin.y1, h, color);
 193 }
 194
 195 //======================================================================
 196 void TFT_drawFastHLine(int16_t x, int16_t y, int16_t w, color_t color) {
 197         _drawFastHLine(x+tft_dispWin.x1, y+tft_dispWin.y1, w, color);
 198 }
 199
 200 // Bresenham's algorithm - thx wikipedia - speed enhanced by Bodmer this uses
 201 // the eficient FastH/V Line draw routine for segments of 2 pixels or more
 202 //----------------------------------------------------------------------------------
 203 static void _drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, color_t color)
 204 {
 205   if (x0 == x1) {
 206           if (y0 <= y1) _drawFastVLine(x0, y0, y1-y0, color);
 207           else _drawFastVLine(x0, y1, y0-y1, color);
 208           return;
 209   }
 210   if (y0 == y1) {
 211           if (x0 <= x1) _drawFastHLine(x0, y0, x1-x0, color);
 212           else _drawFastHLine(x1, y0, x0-x1, color);
 213           return;
 214   }
 215
 216   int steep = 0;
 217   if (abs(y1 - y0) > abs(x1 - x0)) steep = 1;
 218   if (steep) {
 219     swap(x0, y0);
 220     swap(x1, y1);
 221   }
 222   if (x0 > x1) {
 223     swap(x0, x1);
 224     swap(y0, y1);
 225   }
 226
 227   int16_t dx = x1 - x0, dy = abs(y1 - y0);
 228   int16_t err = dx >> 1, ystep = -1, xs = x0, dlen = 0;
 229
 230   if (y0 < y1) ystep = 1;
 231
 232   // Split into steep and not steep for FastH/V separation
 233   if (steep) {
 234     for (; x0 <= x1; x0++) {
 235       dlen++;
 236       err -= dy;
 237       if (err < 0) {
 238         err += dx;
 239         if (dlen == 1) _drawPixel(y0, xs, color, 1);
 240         else _drawFastVLine(y0, xs, dlen, color);
 241         dlen = 0; y0 += ystep; xs = x0 + 1;
 242       }
 243     }
 244     if (dlen) _drawFastVLine(y0, xs, dlen, color);
 245   }
 246   else
 247   {
 248     for (; x0 <= x1; x0++) {
 249       dlen++;
 250       err -= dy;
 251       if (err < 0) {
 252         err += dx;
 253         if (dlen == 1) _drawPixel(xs, y0, color, 1);
 254         else _drawFastHLine(xs, y0, dlen, color);
 255         dlen = 0; y0 += ystep; xs = x0 + 1;
 256       }
 257     }
 258     if (dlen) _drawFastHLine(xs, y0, dlen, color);
 259   }
 260 }
 261
 262 //==============================================================================
 263 void TFT_drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, color_t color)
 264 {
 265         _drawLine(x0+tft_dispWin.x1, y0+tft_dispWin.y1, x1+tft_dispWin.x1, y1+tft_dispWin.y1, color);
 266 }
 267
 268 // fill a rectangle
 269 //--------------------------------------------------------------------------------
 270 static void _fillRect(int16_t x, int16_t y, int16_t w, int16_t h, color_t color) {
 271         // clipping
 272         if ((x >= tft_dispWin.x2) || (y > tft_dispWin.y2)) return;
 273
 274         if (x < tft_dispWin.x1) {
 275                 w -= (tft_dispWin.x1 - x);
 276                 x = tft_dispWin.x1;
 277         }
 278         if (y < tft_dispWin.y1) {
 279                 h -= (tft_dispWin.y1 - y);
 280                 y = tft_dispWin.y1;
 281         }
 282         if (w < 0) w = 0;
 283         if (h < 0) h = 0;
 284
 285         if ((x + w) > (tft_dispWin.x2+1)) w = tft_dispWin.x2 - x + 1;
 286         if ((y + h) > (tft_dispWin.y2+1)) h = tft_dispWin.y2 - y + 1;
 287         if (w == 0) w = 1;
 288         if (h == 0) h = 1;
 289         TFT_pushColorRep(x, y, x+w-1, y+h-1, color, (uint32_t)(h*w));
 290 }
 291
 292 //============================================================================
 293 void TFT_fillRect(int16_t x, int16_t y, int16_t w, int16_t h, color_t color) {
 294         _fillRect(x+tft_dispWin.x1, y+tft_dispWin.y1, w, h, color);
 295 }
 296
 297 //==================================
 298 void TFT_fillScreen(color_t color) {
 299         TFT_pushColorRep(TFT_STATIC_X_OFFSET, TFT_STATIC_Y_OFFSET, tft_width + TFT_STATIC_X_OFFSET -1, tft_height + TFT_STATIC_Y_OFFSET -1, color, (uint32_t)(tft_height*tft_width));
 300 }
 301
 302 //==================================
 303 void TFT_fillWindow(color_t color) {
 304         TFT_pushColorRep(tft_dispWin.x1, tft_dispWin.y1, tft_dispWin.x2, tft_dispWin.y2,
 305                         color, (uint32_t)((tft_dispWin.x2-tft_dispWin.x1+1) * (tft_dispWin.y2-tft_dispWin.y1+1)));
 306 }
 307
 308 // ^^^============= Basics drawing functions ================================^^^
 309
 310
 311 // ================ Graphics drawing functions ==================================
 312
 313 //-----------------------------------------------------------------------------------
 314 static void _drawRect(uint16_t x1,uint16_t y1,uint16_t w,uint16_t h, color_t color) {
 315   _drawFastHLine(x1,y1,w, color);
 316   _drawFastVLine(x1+w-1,y1,h, color);
 317   _drawFastHLine(x1,y1+h-1,w, color);
 318   _drawFastVLine(x1,y1,h, color);
 319 }
 320
 321 //===============================================================================
 322 void TFT_drawRect(uint16_t x1,uint16_t y1,uint16_t w,uint16_t h, color_t color) {
 323         _drawRect(x1+tft_dispWin.x1, y1+tft_dispWin.y1, w, h, color);
 324 }
 325
 326 //-------------------------------------------------------------------------------------------------
 327 static void drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, color_t color)
 328 {
 329         int16_t f = 1 - r;
 330         int16_t ddF_x = 1;
 331         int16_t ddF_y = -2 * r;
 332         int16_t x = 0;
 333         int16_t y = r;
 334
 335         disp_select();
 336         while (x < y) {
 337                 if (f >= 0) {
 338                         y--;
 339                         ddF_y += 2;
 340                         f += ddF_y;
 341                 }
 342                 x++;
 343                 ddF_x += 2;
 344                 f += ddF_x;
 345                 if (cornername & 0x4) {
 346                         _drawPixel(x0 + x, y0 + y, color, 0);
 347                         _drawPixel(x0 + y, y0 + x, color, 0);
 348                 }
 349                 if (cornername & 0x2) {
 350                         _drawPixel(x0 + x, y0 - y, color, 0);
 351                         _drawPixel(x0 + y, y0 - x, color, 0);
 352                 }
 353                 if (cornername & 0x8) {
 354                         _drawPixel(x0 - y, y0 + x, color, 0);
 355                         _drawPixel(x0 - x, y0 + y, color, 0);
 356                 }
 357                 if (cornername & 0x1) {
 358                         _drawPixel(x0 - y, y0 - x, color, 0);
 359                         _drawPixel(x0 - x, y0 - y, color, 0);
 360                 }
 361         }
 362         disp_deselect();
 363 }
 364
 365 // Used to do circles and roundrects
 366 //----------------------------------------------------------------------------------------------------------------
 367 static void fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, int16_t delta, color_t color)
 368 {
 369         int16_t f = 1 - r;
 370         int16_t ddF_x = 1;
 371         int16_t ddF_y = -2 * r;
 372         int16_t x = 0;
 373         int16_t y = r;
 374         int16_t ylm = x0 - r;
 375
 376         while (x < y) {
 377                 if (f >= 0) {
 378                         if (cornername & 0x1) _drawFastVLine(x0 + y, y0 - x, 2 * x + 1 + delta, color);
 379                         if (cornername & 0x2) _drawFastVLine(x0 - y, y0 - x, 2 * x + 1 + delta, color);
 380                         ylm = x0 - y;
 381                         y--;
 382                         ddF_y += 2;
 383                         f += ddF_y;
 384                 }
 385                 x++;
 386                 ddF_x += 2;
 387                 f += ddF_x;
 388
 389                 if ((x0 - x) > ylm) {
 390                         if (cornername & 0x1) _drawFastVLine(x0 + x, y0 - y, 2 * y + 1 + delta, color);
 391                         if (cornername & 0x2) _drawFastVLine(x0 - x, y0 - y, 2 * y + 1 + delta, color);
 392                 }
 393         }
 394 }
 395
 396 // Draw a rounded rectangle
 397 //=============================================================================================
 398 void TFT_drawRoundRect(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t r, color_t color)
 399 {
 400         x += tft_dispWin.x1;
 401         y += tft_dispWin.y1;
 402
 403         // smarter version
 404         _drawFastHLine(x + r, y, w - 2 * r, color);                     // Top
 405         _drawFastHLine(x + r, y + h - 1, w - 2 * r, color);     // Bottom
 406         _drawFastVLine(x, y + r, h - 2 * r, color);                     // Left
 407         _drawFastVLine(x + w - 1, y + r, h - 2 * r, color);     // Right
 408
 409         // draw four corners
 410         drawCircleHelper(x + r, y + r, r, 1, color);
 411         drawCircleHelper(x + w - r - 1, y + r, r, 2, color);
 412         drawCircleHelper(x + w - r - 1, y + h - r - 1, r, 4, color);
 413         drawCircleHelper(x + r, y + h - r - 1, r, 8, color);
 414 }
 415
 416 // Fill a rounded rectangle
 417 //=============================================================================================
 418 void TFT_fillRoundRect(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t r, color_t color)
 419 {
 420         x += tft_dispWin.x1;
 421         y += tft_dispWin.y1;
 422
 423         // smarter version
 424         _fillRect(x + r, y, w - 2 * r, h, color);
 425
 426         // draw four corners
 427         fillCircleHelper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);
 428         fillCircleHelper(x + r, y + r, r, 2, h - 2 * r - 1, color);
 429 }
 430
 431
 432
 433
 434 //-----------------------------------------------------------------------------------------------
 435 static void _drawLineByAngle(int16_t x, int16_t y, int16_t angle, uint16_t length, color_t color)
 436 {
 437         _drawLine(
 438                 x,
 439                 y,
 440                 x + length * cos((angle + tft_angleOffset) * DEG_TO_RAD),
 441                 y + length * sin((angle + tft_angleOffset) * DEG_TO_RAD), color);
 442 }
 443
 444 //---------------------------------------------------------------------------------------------------------------
 445 static void _DrawLineByAngle(int16_t x, int16_t y, int16_t angle, uint16_t start, uint16_t length, color_t color)
 446 {
 447         _drawLine(
 448                 x + start * cos((angle + tft_angleOffset) * DEG_TO_RAD),
 449                 y + start * sin((angle + tft_angleOffset) * DEG_TO_RAD),
 450                 x + (start + length) * cos((angle + tft_angleOffset) * DEG_TO_RAD),
 451                 y + (start + length) * sin((angle + tft_angleOffset) * DEG_TO_RAD), color);
 452 }
 453
 454 //===========================================================================================================
 455 void TFT_drawLineByAngle(uint16_t x, uint16_t y, uint16_t start, uint16_t len, uint16_t angle, color_t color)
 456 {
 457         x += tft_dispWin.x1;
 458         y += tft_dispWin.y1;
 459
 460         if (start == 0) _drawLineByAngle(x, y, angle, len, color);
 461         else _DrawLineByAngle(x, y, angle, start, len, color);
 462 }
 463
 464
 465 // Draw a triangle
 466 //--------------------------------------------------------------------------------------------------------------------
 467 static void _drawTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
 468 {
 469         _drawLine(x0, y0, x1, y1, color);
 470         _drawLine(x1, y1, x2, y2, color);
 471         _drawLine(x2, y2, x0, y0, color);
 472 }
 473
 474 //================================================================================================================
 475 void TFT_drawTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
 476 {
 477         x0 += tft_dispWin.x1;
 478         y0 += tft_dispWin.y1;
 479         x1 += tft_dispWin.x1;
 480         y1 += tft_dispWin.y1;
 481         x2 += tft_dispWin.x1;
 482         y2 += tft_dispWin.y1;
 483
 484         _drawLine(x0, y0, x1, y1, color);
 485         _drawLine(x1, y1, x2, y2, color);
 486         _drawLine(x2, y2, x0, y0, color);
 487 }
 488
 489 // Fill a triangle
 490 //--------------------------------------------------------------------------------------------------------------------
 491 static void _fillTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
 492 {
 493   int16_t a, b, y, last;
 494
 495   // Sort coordinates by Y order (y2 >= y1 >= y0)
 496   if (y0 > y1) {
 497     swap(y0, y1); swap(x0, x1);
 498   }
 499   if (y1 > y2) {
 500     swap(y2, y1); swap(x2, x1);
 501   }
 502   if (y0 > y1) {
 503     swap(y0, y1); swap(x0, x1);
 504   }
 505
 506   if(y0 == y2) { // Handle awkward all-on-same-line case as its own thing
 507     a = b = x0;
 508     if(x1 < a)      a = x1;
 509     else if(x1 > b) b = x1;
 510     if(x2 < a)      a = x2;
 511     else if(x2 > b) b = x2;
 512     _drawFastHLine(a, y0, b-a+1, color);
 513     return;
 514   }
 515
 516   int16_t
 517     dx01 = x1 - x0,
 518     dy01 = y1 - y0,
 519     dx02 = x2 - x0,
 520     dy02 = y2 - y0,
 521     dx12 = x2 - x1,
 522     dy12 = y2 - y1;
 523   int32_t
 524     sa   = 0,
 525     sb   = 0;
 526
 527   // For upper part of triangle, find scanline crossings for segments
 528   // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
 529   // is included here (and second loop will be skipped, avoiding a /0
 530   // error there), otherwise scanline y1 is skipped here and handled
 531   // in the second loop...which also avoids a /0 error here if y0=y1
 532   // (flat-topped triangle).
 533   if(y1 == y2) last = y1;   // Include y1 scanline
 534   else         last = y1-1; // Skip it
 535
 536   for(y=y0; y<=last; y++) {
 537     a   = x0 + sa / dy01;
 538     b   = x0 + sb / dy02;
 539     sa += dx01;
 540     sb += dx02;
 541     /* longhand:
 542     a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
 543     b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
 544     */
 545     if(a > b) swap(a,b);
 546     _drawFastHLine(a, y, b-a+1, color);
 547   }
 548
 549   // For lower part of triangle, find scanline crossings for segments
 550   // 0-2 and 1-2.  This loop is skipped if y1=y2.
 551   sa = dx12 * (y - y1);
 552   sb = dx02 * (y - y0);
 553   for(; y<=y2; y++) {
 554     a   = x1 + sa / dy12;
 555     b   = x0 + sb / dy02;
 556     sa += dx12;
 557     sb += dx02;
 558     /* longhand:
 559     a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
 560     b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
 561     */
 562     if(a > b) swap(a,b);
 563     _drawFastHLine(a, y, b-a+1, color);
 564   }
 565 }
 566
 567 //================================================================================================================
 568 void TFT_fillTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, color_t color)
 569 {
 570         _fillTriangle(
 571                         x0 + tft_dispWin.x1, y0 + tft_dispWin.y1,
 572                         x1 + tft_dispWin.x1, y1 + tft_dispWin.y1,
 573                         x2 + tft_dispWin.x1, y2 + tft_dispWin.y1,
 574                         color);
 575 }
 576
 577 //====================================================================
 578 void TFT_drawCircle(int16_t x, int16_t y, int radius, color_t color) {
 579         x += tft_dispWin.x1;
 580         y += tft_dispWin.y1;
 581         int f = 1 - radius;
 582         int ddF_x = 1;
 583         int ddF_y = -2 * radius;
 584         int x1 = 0;
 585         int y1 = radius;
 586
 587         disp_select();
 588         _drawPixel(x, y + radius, color, 0);
 589         _drawPixel(x, y - radius, color, 0);
 590         _drawPixel(x + radius, y, color, 0);
 591         _drawPixel(x - radius, y, color, 0);
 592         while(x1 < y1) {
 593                 if (f >= 0) {
 594                         y1--;
 595                         ddF_y += 2;
 596                         f += ddF_y;
 597                 }
 598                 x1++;
 599                 ddF_x += 2;
 600                 f += ddF_x;
 601                 _drawPixel(x + x1, y + y1, color, 0);
 602                 _drawPixel(x - x1, y + y1, color, 0);
 603                 _drawPixel(x + x1, y - y1, color, 0);
 604                 _drawPixel(x - x1, y - y1, color, 0);
 605                 _drawPixel(x + y1, y + x1, color, 0);
 606                 _drawPixel(x - y1, y + x1, color, 0);
 607                 _drawPixel(x + y1, y - x1, color, 0);
 608                 _drawPixel(x - y1, y - x1, color, 0);
 609         }
 610   disp_deselect();
 611 }
 612
 613 //====================================================================
 614 void TFT_fillCircle(int16_t x, int16_t y, int radius, color_t color) {
 615         x += tft_dispWin.x1;
 616         y += tft_dispWin.y1;
 617
 618         _drawFastVLine(x, y-radius, 2*radius+1, color);
 619         fillCircleHelper(x, y, radius, 3, 0, color);
 620 }
 621
 622 //----------------------------------------------------------------------------------------------------------------
 623 static void _draw_ellipse_section(uint16_t x, uint16_t y, uint16_t x0, uint16_t y0, color_t color, uint8_t option)
 624 {
 625         disp_select();
 626     // upper right
 627     if ( option & TFT_ELLIPSE_UPPER_RIGHT ) _drawPixel(x0 + x, y0 - y, color, 0);
 628     // upper left
 629     if ( option & TFT_ELLIPSE_UPPER_LEFT ) _drawPixel(x0 - x, y0 - y, color, 0);
 630     // lower right
 631     if ( option & TFT_ELLIPSE_LOWER_RIGHT ) _drawPixel(x0 + x, y0 + y, color, 0);
 632     // lower left
 633     if ( option & TFT_ELLIPSE_LOWER_LEFT ) _drawPixel(x0 - x, y0 + y, color, 0);
 634         disp_deselect();
 635 }
 636
 637 //=====================================================================================================
 638 void TFT_drawEllipse(uint16_t x0, uint16_t y0, uint16_t rx, uint16_t ry, color_t color, uint8_t option)
 639 {
 640         x0 += tft_dispWin.x1;
 641         y0 += tft_dispWin.y1;
 642
 643         uint16_t x, y;
 644         int32_t xchg, ychg;
 645         int32_t err;
 646         int32_t rxrx2;
 647         int32_t ryry2;
 648         int32_t stopx, stopy;
 649
 650         rxrx2 = rx;
 651         rxrx2 *= rx;
 652         rxrx2 *= 2;
 653
 654         ryry2 = ry;
 655         ryry2 *= ry;
 656         ryry2 *= 2;
 657
 658         x = rx;
 659         y = 0;
 660
 661         xchg = 1;
 662         xchg -= rx;
 663         xchg -= rx;
 664         xchg *= ry;
 665         xchg *= ry;
 666
 667         ychg = rx;
 668         ychg *= rx;
 669
 670         err = 0;
 671
 672         stopx = ryry2;
 673         stopx *= rx;
 674         stopy = 0;
 675
 676         while( stopx >= stopy ) {
 677                 _draw_ellipse_section(x, y, x0, y0, color, option);
 678                 y++;
 679                 stopy += rxrx2;
 680                 err += ychg;
 681                 ychg += rxrx2;
 682                 if ( 2*err+xchg > 0 ) {
 683                         x--;
 684                         stopx -= ryry2;
 685                         err += xchg;
 686                         xchg += ryry2;
 687                 }
 688         }
 689
 690         x = 0;
 691         y = ry;
 692
 693         xchg = ry;
 694         xchg *= ry;
 695
 696         ychg = 1;
 697         ychg -= ry;
 698         ychg -= ry;
 699         ychg *= rx;
 700         ychg *= rx;
 701
 702         err = 0;
 703
 704         stopx = 0;
 705
 706         stopy = rxrx2;
 707         stopy *= ry;
 708
 709         while( stopx <= stopy ) {
 710                 _draw_ellipse_section(x, y, x0, y0, color, option);
 711                 x++;
 712                 stopx += ryry2;
 713                 err += xchg;
 714                 xchg += ryry2;
 715                 if ( 2*err+ychg > 0 ) {
 716                         y--;
 717                         stopy -= rxrx2;
 718                         err += ychg;
 719                         ychg += rxrx2;
 720                 }
 721         }
 722 }
 723
 724 //-----------------------------------------------------------------------------------------------------------------------
 725 static void _draw_filled_ellipse_section(uint16_t x, uint16_t y, uint16_t x0, uint16_t y0, color_t color, uint8_t option)
 726 {
 727     // upper right
 728     if ( option & TFT_ELLIPSE_UPPER_RIGHT ) _drawFastVLine(x0+x, y0-y, y+1, color);
 729     // upper left
 730     if ( option & TFT_ELLIPSE_UPPER_LEFT ) _drawFastVLine(x0-x, y0-y, y+1, color);
 731     // lower right
 732     if ( option & TFT_ELLIPSE_LOWER_RIGHT ) _drawFastVLine(x0+x, y0, y+1, color);
 733     // lower left
 734     if ( option & TFT_ELLIPSE_LOWER_LEFT ) _drawFastVLine(x0-x, y0, y+1, color);
 735 }
 736
 737 //=====================================================================================================
 738 void TFT_fillEllipse(uint16_t x0, uint16_t y0, uint16_t rx, uint16_t ry, color_t color, uint8_t option)
 739 {
 740         x0 += tft_dispWin.x1;
 741         y0 += tft_dispWin.y1;
 742
 743         uint16_t x, y;
 744         int32_t xchg, ychg;
 745         int32_t err;
 746         int32_t rxrx2;
 747         int32_t ryry2;
 748         int32_t stopx, stopy;
 749
 750         rxrx2 = rx;
 751         rxrx2 *= rx;
 752         rxrx2 *= 2;
 753
 754         ryry2 = ry;
 755         ryry2 *= ry;
 756         ryry2 *= 2;
 757
 758         x = rx;
 759         y = 0;
 760
 761         xchg = 1;
 762         xchg -= rx;
 763         xchg -= rx;
 764         xchg *= ry;
 765         xchg *= ry;
 766
 767         ychg = rx;
 768         ychg *= rx;
 769
 770         err = 0;
 771
 772         stopx = ryry2;
 773         stopx *= rx;
 774         stopy = 0;
 775
 776         while( stopx >= stopy ) {
 777                 _draw_filled_ellipse_section(x, y, x0, y0, color, option);
 778                 y++;
 779                 stopy += rxrx2;
 780                 err += ychg;
 781                 ychg += rxrx2;
 782                 if ( 2*err+xchg > 0 ) {
 783                         x--;
 784                         stopx -= ryry2;
 785                         err += xchg;
 786                         xchg += ryry2;
 787                 }
 788         }
 789
 790         x = 0;
 791         y = ry;
 792
 793         xchg = ry;
 794         xchg *= ry;
 795
 796         ychg = 1;
 797         ychg -= ry;
 798         ychg -= ry;
 799         ychg *= rx;
 800         ychg *= rx;
 801
 802         err = 0;
 803
 804         stopx = 0;
 805
 806         stopy = rxrx2;
 807         stopy *= ry;
 808
 809         while( stopx <= stopy ) {
 810                 _draw_filled_ellipse_section(x, y, x0, y0, color, option);
 811                 x++;
 812                 stopx += ryry2;
 813                 err += xchg;
 814                 xchg += ryry2;
 815                 if ( 2*err+ychg > 0 ) {
 816                         y--;
 817                         stopy -= rxrx2;
 818                         err += ychg;
 819                         ychg += rxrx2;
 820                 }
 821         }
 822 }
 823
 824
 825 // ==== ARC DRAWING ===================================================================
 826
 827 //---------------------------------------------------------------------------------------------------------------------------------
 828 static void _fillArcOffsetted(uint16_t cx, uint16_t cy, uint16_t radius, uint16_t thickness, float start, float end, color_t color)
 829 {
 830         //float sslope = (float)cos_lookup(start) / (float)sin_lookup(start);
 831         //float eslope = (float)cos_lookup(end) / (float)sin_lookup(end);
 832         float sslope = (cos(start/_arcAngleMax * 2 * PI) * _arcAngleMax) / (sin(start/_arcAngleMax * 2 * PI) * _arcAngleMax) ;
 833         float eslope = (cos(end/_arcAngleMax * 2 * PI) * _arcAngleMax) / (sin(end/_arcAngleMax * 2 * PI) * _arcAngleMax);
 834
 835         if (end == 360) eslope = -1000000;
 836
 837         int ir2 = (radius - thickness) * (radius - thickness);
 838         int or2 = radius * radius;
 839
 840         disp_select();
 841         for (int x = -radius; x <= radius; x++) {
 842                 for (int y = -radius; y <= radius; y++) {
 843                         int x2 = x * x;
 844                         int y2 = y * y;
 845
 846                         if (
 847                                 (x2 + y2 < or2 && x2 + y2 >= ir2) &&
 848                                 (
 849                                 (y > 0 && start < 180 && x <= y * sslope) ||
 850                                 (y < 0 && start > 180 && x >= y * sslope) ||
 851                                 (y < 0 && start <= 180) ||
 852                                 (y == 0 && start <= 180 && x < 0) ||
 853                                 (y == 0 && start == 0 && x > 0)
 854                                 ) &&
 855                                 (
 856                                 (y > 0 && end < 180 && x >= y * eslope) ||
 857                                 (y < 0 && end > 180 && x <= y * eslope) ||
 858                                 (y > 0 && end >= 180) ||
 859                                 (y == 0 && end >= 180 && x < 0) ||
 860                                 (y == 0 && start == 0 && x > 0)
 861                                 )
 862                                 )
 863                                 _drawPixel(cx+x, cy+y, color, 0);
 864                 }
 865         }
 866         disp_deselect();
 867 }
 868
 869
 870 //===========================================================================================================================
 871 void TFT_drawArc(uint16_t cx, uint16_t cy, uint16_t r, uint16_t th, float start, float end, color_t color, color_t fillcolor)
 872 {
 873         cx += tft_dispWin.x1;
 874         cy += tft_dispWin.y1;
 875
 876         if (th < 1) th = 1;
 877         if (th > r) th = r;
 878
 879         int f = TFT_compare_colors(fillcolor, color);
 880
 881         float astart = fmodf(start, _arcAngleMax);
 882         float aend = fmodf(end, _arcAngleMax);
 883
 884         astart += tft_angleOffset;
 885         aend += tft_angleOffset;
 886
 887         if (astart < 0) astart += (float)360;
 888         if (aend < 0) aend += (float)360;
 889
 890         if (aend == 0) aend = (float)360;
 891
 892         if (astart > aend) {
 893                 _fillArcOffsetted(cx, cy, r, th, astart, _arcAngleMax, fillcolor);
 894                 _fillArcOffsetted(cx, cy, r, th, 0, aend, fillcolor);
 895                 if (f) {
 896                         _fillArcOffsetted(cx, cy, r, 1, astart, _arcAngleMax, color);
 897                         _fillArcOffsetted(cx, cy, r, 1, 0, aend, color);
 898                         _fillArcOffsetted(cx, cy, r-th, 1, astart, _arcAngleMax, color);
 899                         _fillArcOffsetted(cx, cy, r-th, 1, 0, aend, color);
 900                 }
 901         }
 902         else {
 903                 _fillArcOffsetted(cx, cy, r, th, astart, aend, fillcolor);
 904                 if (f) {
 905                         _fillArcOffsetted(cx, cy, r, 1, astart, aend, color);
 906                         _fillArcOffsetted(cx, cy, r-th, 1, astart, aend, color);
 907                 }
 908         }
 909         if (f) {
 910                 _drawLine(cx + (r-th) * cos(astart * DEG_TO_RAD), cy + (r-th) * sin(astart * DEG_TO_RAD),
 911                         cx + (r-1) * cos(astart * DEG_TO_RAD), cy + (r-1) * sin(astart * DEG_TO_RAD), color);
 912                 _drawLine(cx + (r-th) * cos(aend * DEG_TO_RAD), cy + (r-th) * sin(aend * DEG_TO_RAD),
 913                         cx + (r-1) * cos(aend * DEG_TO_RAD), cy + (r-1) * sin(aend * DEG_TO_RAD), color);
 914         }
 915 }
 916
 917 //=============================================================================================================
 918 void TFT_drawPolygon(int cx, int cy, int sides, int diameter, color_t color, color_t fill, int rot, uint8_t th)
 919 {
 920         cx += tft_dispWin.x1;
 921         cy += tft_dispWin.y1;
 922
 923         int deg = rot - tft_angleOffset;
 924         int f = TFT_compare_colors(fill, color);
 925
 926         if (sides < MIN_POLIGON_SIDES) sides = MIN_POLIGON_SIDES;       // This ensures the minimum side number
 927         if (sides > MAX_POLIGON_SIDES) sides = MAX_POLIGON_SIDES;       // This ensures the maximum side number
 928
 929         int Xpoints[sides], Ypoints[sides];                                                     // Set the arrays based on the number of sides entered
 930         int rads = 360 / sides;                                                                         // This equally spaces the points.
 931
 932         for (int idx = 0; idx < sides; idx++) {
 933                 Xpoints[idx] = cx + sin((float)(idx*rads + deg) * deg_to_rad) * diameter;
 934                 Ypoints[idx] = cy + cos((float)(idx*rads + deg) * deg_to_rad) * diameter;
 935         }
 936
 937         // Draw the polygon on the screen.
 938         if (f) {
 939                 for(int idx = 0; idx < sides; idx++) {
 940                         if((idx+1) < sides) _fillTriangle(cx,cy,Xpoints[idx],Ypoints[idx],Xpoints[idx+1],Ypoints[idx+1], fill);
 941                         else _fillTriangle(cx,cy,Xpoints[idx],Ypoints[idx],Xpoints[0],Ypoints[0], fill);
 942                 }
 943         }
 944
 945         if (th) {
 946                 for (int n=0; n<th; n++) {
 947                         if (n > 0) {
 948                                 for (int idx = 0; idx < sides; idx++) {
 949                                         Xpoints[idx] = cx + sin((float)(idx*rads + deg) * deg_to_rad) * (diameter-n);
 950                                         Ypoints[idx] = cy + cos((float)(idx*rads + deg) * deg_to_rad) * (diameter-n);
 951                                 }
 952                         }
 953                         for(int idx = 0; idx < sides; idx++) {
 954                                 if( (idx+1) < sides)
 955                                         _drawLine(Xpoints[idx],Ypoints[idx],Xpoints[idx+1],Ypoints[idx+1], color); // draw the lines
 956                                 else
 957                                         _drawLine(Xpoints[idx],Ypoints[idx],Xpoints[0],Ypoints[0], color); // finishes the last line to close up the polygon.
 958                         }
 959                 }
 960         }
 961 }
 962
 963 /*
 964 // Similar to the Polygon function.
 965 //=====================================================================================
 966 void TFT_drawStar(int cx, int cy, int diameter, color_t color, bool fill, float factor)
 967 {
 968         cx += tft_dispWin.x1;
 969         cy += tft_dispWin.y1;
 970
 971         factor = constrain(factor, 1.0, 4.0);
 972         uint8_t sides = 5;
 973         uint8_t rads = 360 / sides;
 974
 975         int Xpoints_O[sides], Ypoints_O[sides], Xpoints_I[sides], Ypoints_I[sides];//Xpoints_T[5], Ypoints_T[5];
 976
 977         for(int idx = 0; idx < sides; idx++) {
 978                 // makes the outer points
 979                 Xpoints_O[idx] = cx + sin((float)(idx*rads + 72) * deg_to_rad) * diameter;
 980                 Ypoints_O[idx] = cy + cos((float)(idx*rads + 72) * deg_to_rad) * diameter;
 981                 // makes the inner points
 982                 Xpoints_I[idx] = cx + sin((float)(idx*rads + 36) * deg_to_rad) * ((float)(diameter)/factor);
 983                 // 36 is half of 72, and this will allow the inner and outer points to line up like a triangle.
 984                 Ypoints_I[idx] = cy + cos((float)(idx*rads + 36) * deg_to_rad) * ((float)(diameter)/factor);
 985         }
 986
 987         for(int idx = 0; idx < sides; idx++) {
 988                 if((idx+1) < sides) {
 989                         if(fill) {// this part below should be self explanatory. It fills in the star.
 990                                 _fillTriangle(cx,cy,Xpoints_I[idx],Ypoints_I[idx],Xpoints_O[idx],Ypoints_O[idx], color);
 991                                 _fillTriangle(cx,cy,Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx+1],Ypoints_I[idx+1], color);
 992                         }
 993                         else {
 994                                 _drawLine(Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx+1],Ypoints_I[idx+1], color);
 995                                 _drawLine(Xpoints_I[idx],Ypoints_I[idx],Xpoints_O[idx],Ypoints_O[idx], color);
 996                         }
 997                 }
 998                 else {
 999                         if(fill) {
1000                                 _fillTriangle(cx,cy,Xpoints_I[0],Ypoints_I[0],Xpoints_O[idx],Ypoints_O[idx], color);
1001                                 _fillTriangle(cx,cy,Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx],Ypoints_I[idx], color);
1002                         }
1003                         else {
1004                                 _drawLine(Xpoints_O[idx],Ypoints_O[idx],Xpoints_I[idx],Ypoints_I[idx], color);
1005                                 _drawLine(Xpoints_I[0],Ypoints_I[0],Xpoints_O[idx],Ypoints_O[idx], color);
1006                         }
1007                 }
1008         }
1009 }
1010 */
1011
1012 // ================ Font and string functions ==================================
1013
1014 //-------------------------------------------------------
1015 static int load_file_font(const char *fontfile, int info)
1016 {
1017         int err = 0;
1018         char err_msg[256] = {'\0'};
1019
1020         if (userfont != NULL) {
1021                 free(userfont);
1022                 userfont = NULL;
1023         }
1024
1025     struct stat sb;
1026
1027     // Open the file
1028     FILE *fhndl = fopen(fontfile, "r");
1029     if (!fhndl) {
1030         sprintf(err_msg, "Error opening font file '%s'", fontfile);
1031                 err = 1;
1032                 goto exit;
1033     }
1034
1035         // Get file size
1036     if (stat(fontfile, &sb) != 0) {
1037         sprintf(err_msg, "Error getting font file size");
1038                 err = 2;
1039                 goto exit;
1040     }
1041         int fsize = sb.st_size;
1042         if (fsize < 30) {
1043                 sprintf(err_msg, "Error getting font file size");
1044                 err = 3;
1045                 goto exit;
1046         }
1047
1048         userfont = malloc(fsize+4);
1049         if (userfont == NULL) {
1050                 sprintf(err_msg, "Font memory allocation error");
1051                 fclose(fhndl);
1052                 err = 4;
1053                 goto exit;
1054         }
1055
1056         int read = fread(userfont, 1, fsize, fhndl);
1057
1058         fclose(fhndl);
1059
1060         if (read != fsize) {
1061                 sprintf(err_msg, "Font read error");
1062                 err = 5;
1063                 goto exit;
1064         }
1065
1066         userfont[read] = 0;
1067         if (strstr((char *)(userfont+read-8), "RPH_font") == NULL) {
1068                 sprintf(err_msg, "Font ID not found");
1069                 err = 6;
1070                 goto exit;
1071         }
1072
1073         // Check size
1074         int size = 0;
1075         int numchar = 0;
1076         int width = userfont[0];
1077         int height = userfont[1];
1078         uint8_t first = 255;
1079         uint8_t last = 0;
1080         //int offst = 0;
1081         int pminwidth = 255;
1082         int pmaxwidth = 0;
1083
1084         if (width != 0) {
1085                 // Fixed font
1086                 numchar = userfont[3];
1087                 first = userfont[2];
1088                 last = first + numchar - 1;
1089                 size = ((width * height * numchar) / 8) + 4;
1090         }
1091         else {
1092                 // Proportional font
1093                 size = 4; // point at first char data
1094                 uint8_t charCode;
1095                 int charwidth;
1096
1097                 do {
1098                     charCode = userfont[size];
1099                     charwidth = userfont[size+2];
1100
1101                     if (charCode != 0xFF) {
1102                         numchar++;
1103                         if (charwidth != 0) size += ((((charwidth * userfont[size+3])-1) / 8) + 7);
1104                         else size += 6;
1105
1106                         if (info) {
1107                                 if (charwidth > pmaxwidth) pmaxwidth = charwidth;
1108                                 if (charwidth < pminwidth) pminwidth = charwidth;
1109                                 if (charCode < first) first = charCode;
1110                                 if (charCode > last) last = charCode;
1111                         }
1112                     }
1113                     else size++;
1114                   } while ((size < (read-8)) && (charCode != 0xFF));
1115         }
1116
1117         if (size != (read-8)) {
1118                 sprintf(err_msg, "Font size error: found %d expected %d)", size, (read-8));
1119                 err = 7;
1120                 goto exit;
1121         }
1122
1123         if (info) {
1124                 if (width != 0) {
1125                         printf("Fixed width font:\r\n  size: %d  width: %d  height: %d  characters: %d (%d~%d)\n",
1126                                         size, width, height, numchar, first, last);
1127                 }
1128                 else {
1129                         printf("Proportional font:\r\n  size: %d  width: %d~%d  height: %d  characters: %d (%d~%d)\n",
1130                                         size, pminwidth, pmaxwidth, height, numchar, first, last);
1131                 }
1132         }
1133
1134 exit:
1135         if (err) {
1136                 if (userfont) {
1137                         free(userfont);
1138                         userfont = NULL;
1139                 }
1140                 if (info) printf("Error: %d [%s]\r\n", err, err_msg);
1141         }
1142         return err;
1143 }
1144
1145 //------------------------------------------------------
1146 int compile_font_file(const char *fontfile, uint8_t dbg)
1147 {
1148         int err = 0;
1149         char err_msg[128] = {'\0'};
1150         char outfile[128] = {'\0'};
1151         size_t len;
1152     struct stat sb;
1153     FILE *ffd = NULL;
1154     FILE *ffd_out = NULL;
1155     char *sourcebuf = NULL;
1156
1157     len = strlen(fontfile);
1158
1159         // check here that filename end with ".c".
1160         if ((len < 3) || (len > 125) || (strcmp(fontfile + len - 2, ".c") != 0)) {
1161                 sprintf(err_msg, "not a .c file");
1162                 err = 1;
1163                 goto exit;
1164         }
1165
1166         sprintf(outfile, "%s", fontfile);
1167         sprintf(outfile+strlen(outfile)-1, "fon");
1168
1169         // Open the source file
1170     if (stat(fontfile, &sb) != 0) {
1171         sprintf(err_msg, "Error opening source file '%s'", fontfile);
1172         err = 2;
1173                 goto exit;
1174     }
1175     // Open the file
1176     ffd = fopen(fontfile, "rb");
1177     if (!ffd) {
1178         sprintf(err_msg, "Error opening source file '%s'", fontfile);
1179         err = 3;
1180                 goto exit;
1181     }
1182
1183         // Open the font file
1184     ffd_out= fopen(outfile, "wb");
1185         if (!ffd_out) {
1186                 sprintf(err_msg, "error opening destination file");
1187                 err = 4;
1188                 goto exit;
1189         }
1190
1191         // Get file size
1192         int fsize = sb.st_size;
1193         if (fsize <= 0) {
1194                 sprintf(err_msg, "source file size error");
1195                 err = 5;
1196                 goto exit;
1197         }
1198
1199         sourcebuf = malloc(fsize+4);
1200         if (sourcebuf == NULL) {
1201                 sprintf(err_msg, "memory allocation error");
1202                 err = 6;
1203                 goto exit;
1204         }
1205         char *fbuf = sourcebuf;
1206
1207         int rdsize = fread(fbuf, 1, fsize, ffd);
1208         fclose(ffd);
1209         ffd = NULL;
1210
1211         if (rdsize != fsize) {
1212                 sprintf(err_msg, "error reading from source file");
1213                 err = 7;
1214                 goto exit;
1215         }
1216
1217         *(fbuf+rdsize) = '\0';
1218
1219         fbuf = strchr(fbuf, '{');                       // beginning of font data
1220         char *fend = strstr(fbuf, "};");        // end of font data
1221
1222         if ((fbuf == NULL) || (fend == NULL) || ((fend-fbuf) < 22)) {
1223                 sprintf(err_msg, "wrong source file format");
1224                 err = 8;
1225                 goto exit;
1226         }
1227
1228         fbuf++;
1229         *fend = '\0';
1230         char hexstr[5] = {'\0'};
1231         int lastline = 0;
1232
1233         fbuf = strstr(fbuf, "0x");
1234         int size = 0;
1235         char *nextline;
1236         char *numptr;
1237
1238         int bptr = 0;
1239
1240         while ((fbuf != NULL) && (fbuf < fend) && (lastline == 0)) {
1241                 nextline = strchr(fbuf, '\n'); // beginning of the next line
1242                 if (nextline == NULL) {
1243                         nextline = fend-1;
1244                         lastline++;
1245                 }
1246                 else nextline++;
1247
1248                 while (fbuf < nextline) {
1249                         numptr = strstr(fbuf, "0x");
1250                         if ((numptr == NULL) || ((fbuf+4) > nextline)) numptr = strstr(fbuf, "0X");
1251                         if ((numptr != NULL) && ((numptr+4) <= nextline)) {
1252                                 fbuf = numptr;
1253                                 if (bptr >= 128) {
1254                                         // buffer full, write to file
1255                     if (fwrite(outfile, 1, 128, ffd_out) != 128) goto error;
1256                                         bptr = 0;
1257                                         size += 128;
1258                                 }
1259                                 memcpy(hexstr, fbuf, 4);
1260                                 hexstr[4] = 0;
1261                                 outfile[bptr++] = (uint8_t)strtol(hexstr, NULL, 0);
1262                                 fbuf += 4;
1263                         }
1264                         else fbuf = nextline;
1265                 }
1266                 fbuf = nextline;
1267         }
1268
1269         if (bptr > 0) {
1270                 size += bptr;
1271         if (fwrite(outfile, 1, bptr, ffd_out) != bptr) goto error;
1272         }
1273
1274         // write font ID
1275         sprintf(outfile, "RPH_font");
1276     if (fwrite(outfile, 1, 8, ffd_out) != 8) goto error;
1277
1278     fclose(ffd_out);
1279     ffd_out = NULL;
1280
1281         // === Test compiled font ===
1282         sprintf(outfile, "%s", fontfile);
1283         sprintf(outfile+strlen(outfile)-1, "fon");
1284
1285         uint8_t *uf = userfont; // save userfont pointer
1286         userfont = NULL;
1287         if (load_file_font(outfile, 1) != 0) {
1288                 sprintf(err_msg, "Error compiling file!");
1289                 err = 10;
1290         }
1291         else {
1292                 free(userfont);
1293                 sprintf(err_msg, "File compiled successfully.");
1294         }
1295         userfont = uf; // restore userfont
1296
1297         goto exit;
1298
1299 error:
1300         sprintf(err_msg, "error writing to destination file");
1301         err = 9;
1302
1303 exit:
1304         if (sourcebuf) free(sourcebuf);
1305         if (ffd) fclose(ffd);
1306         if (ffd_out) fclose(ffd_out);
1307
1308         if (dbg) printf("%s\r\n", err_msg);
1309
1310         return err;
1311 }
1312
1313
1314 // -----------------------------------------------------------------------------------------
1315 // Individual Proportional Font Character Format:
1316 // -----------------------------------------------------------------------------------------
1317 // Character Code
1318 // yOffset                              (start Y of visible pixels)
1319 // Width                                (width of the visible pixels)
1320 // Height                               (height of the visible pixels)
1321 // xOffset                              (start X of visible pixels)
1322 // xDelta                               (the distance to move the cursor. Effective width of the character.)
1323 // Data[n]
1324 // -----------------------------------------------------------------------------------------
1325
1326 //---------------------------------------------------------------------------------------------
1327 // Character drawing rectangle is (0, 0) (xDelta-1, tft_cfont.y_size-1)
1328 // Character visible pixels rectangle is (xOffset, yOffset) (xOffset+Width-1, yOffset+Height-1)
1329 //---------------------------------------------------------------------------------------------
1330
1331 //----------------------------------
1332 void getFontCharacters(uint8_t *buf)
1333 {
1334     if (tft_cfont.bitmap == 2) {
1335         //For 7 segment font only characters 0,1,2,3,4,5,6,7,8,9, . , - , : , / are available.
1336                 for (uint8_t n=0; n < 11; n++) {
1337                         buf[n] = n + 0x30;
1338                 }
1339                 buf[11] = '.';
1340                 buf[12] = '-';
1341                 buf[13] = '/';
1342                 buf[14] = '\0';
1343         return;
1344     }
1345
1346     if (tft_cfont.x_size > 0) {
1347                 for (uint8_t n=0; n < tft_cfont.numchars; n++) {
1348                         buf[n] = tft_cfont.offset + n;
1349                 }
1350                 buf[tft_cfont.numchars] = '\0';
1351                 return;
1352         }
1353
1354         uint16_t tempPtr = 4; // point at first char data
1355         uint8_t cc, cw, ch, n;
1356
1357         n = 0;
1358     cc = tft_cfont.font[tempPtr++];
1359     while (cc != 0xFF)  {
1360         tft_cfont.numchars++;
1361         tempPtr++;
1362         cw = tft_cfont.font[tempPtr++];
1363         ch = tft_cfont.font[tempPtr++];
1364         tempPtr++;
1365         tempPtr++;
1366                 if (cw != 0) {
1367                         // packed bits
1368                         tempPtr += (((cw * ch)-1) / 8) + 1;
1369                 }
1370                 buf[n++] = cc;
1371             cc = tft_cfont.font[tempPtr++];
1372         }
1373         buf[n] = '\0';
1374 }
1375
1376 // Set max width & height of the proportional font
1377 //-----------------------------
1378 static void getMaxWidthHeight()
1379 {
1380         uint16_t tempPtr = 4; // point at first char data
1381         uint8_t cc, cw, ch, cd, cy;
1382
1383         tft_cfont.numchars = 0;
1384         tft_cfont.max_x_size = 0;
1385
1386     cc = tft_cfont.font[tempPtr++];
1387     while (cc != 0xFF)  {
1388         tft_cfont.numchars++;
1389         cy = tft_cfont.font[tempPtr++];
1390         cw = tft_cfont.font[tempPtr++];
1391         ch = tft_cfont.font[tempPtr++];
1392         tempPtr++;
1393         cd = tft_cfont.font[tempPtr++];
1394         cy += ch;
1395                 if (cw > tft_cfont.max_x_size) tft_cfont.max_x_size = cw;
1396                 if (cd > tft_cfont.max_x_size) tft_cfont.max_x_size = cd;
1397                 if (ch > tft_cfont.y_size) tft_cfont.y_size = ch;
1398                 if (cy > tft_cfont.y_size) tft_cfont.y_size = cy;
1399                 if (cw != 0) {
1400                         // packed bits
1401                         tempPtr += (((cw * ch)-1) / 8) + 1;
1402                 }
1403             cc = tft_cfont.font[tempPtr++];
1404         }
1405     tft_cfont.size = tempPtr;
1406 }
1407
1408 // Return the Glyph data for an individual character in the proportional font
1409 //------------------------------------
1410 static uint8_t getCharPtr(uint8_t c) {
1411   uint16_t tempPtr = 4; // point at first char data
1412
1413   do {
1414         fontChar.charCode = tft_cfont.font[tempPtr++];
1415     if (fontChar.charCode == 0xFF) return 0;
1416
1417     fontChar.adjYOffset = tft_cfont.font[tempPtr++];
1418     fontChar.width = tft_cfont.font[tempPtr++];
1419     fontChar.height = tft_cfont.font[tempPtr++];
1420     fontChar.xOffset = tft_cfont.font[tempPtr++];
1421     fontChar.xOffset = fontChar.xOffset < 0x80 ? fontChar.xOffset : -(0xFF - fontChar.xOffset);
1422     fontChar.xDelta = tft_cfont.font[tempPtr++];
1423
1424     if (c != fontChar.charCode && fontChar.charCode != 0xFF) {
1425       if (fontChar.width != 0) {
1426         // packed bits
1427         tempPtr += (((fontChar.width * fontChar.height)-1) / 8) + 1;
1428       }
1429     }
1430   } while ((c != fontChar.charCode) && (fontChar.charCode != 0xFF));
1431
1432   fontChar.dataPtr = tempPtr;
1433   if (c == fontChar.charCode) {
1434     if (tft_font_forceFixed > 0) {
1435       // fix width & offset for forced fixed width
1436       fontChar.xDelta = tft_cfont.max_x_size;
1437       fontChar.xOffset = (fontChar.xDelta - fontChar.width) / 2;
1438     }
1439   }
1440   else return 0;
1441
1442   return 1;
1443 }
1444
1445 /*
1446 //-----------------------
1447 static void _testFont() {
1448   if (tft_cfont.x_size) {
1449           printf("FONT TEST: fixed font\r\n");
1450           return;
1451   }
1452   uint16_t tempPtr = 4; // point at first char data
1453   uint8_t c = 0x20;
1454   for (c=0x20; c <0xFF; c++) {
1455         fontChar.charCode = tft_cfont.font[tempPtr++];
1456     if (fontChar.charCode == 0xFF) break;
1457     if (fontChar.charCode != c) {
1458         printf("FONT TEST: last sequential char: %d, expected %d\r\n", fontChar.charCode, c);
1459         break;
1460     }
1461     c = fontChar.charCode;
1462     fontChar.adjYOffset = tft_cfont.font[tempPtr++];
1463     fontChar.width = tft_cfont.font[tempPtr++];
1464     fontChar.height = tft_cfont.font[tempPtr++];
1465     fontChar.xOffset = tft_cfont.font[tempPtr++];
1466     fontChar.xOffset = fontChar.xOffset < 0x80 ? fontChar.xOffset : -(0xFF - fontChar.xOffset);
1467     fontChar.xDelta = tft_cfont.font[tempPtr++];
1468
1469     if (fontChar.charCode != 0xFF) {
1470       if (fontChar.width != 0) {
1471         // packed bits
1472         tempPtr += (((fontChar.width * fontChar.height)-1) / 8) + 1;
1473       }
1474     }
1475   }
1476   printf("FONT TEST: W=%d  H=%d last char: %d [%c]; length: %d\r\n", tft_cfont.max_x_size, tft_cfont.y_size, c, c, tempPtr);
1477 }
1478 */
1479
1480 //===================================================
1481 void TFT_setFont(uint8_t font, const char *font_file)
1482 {
1483   tft_cfont.font = NULL;
1484
1485   if (font == FONT_7SEG) {
1486     tft_cfont.bitmap = 2;
1487     tft_cfont.x_size = 24;
1488     tft_cfont.y_size = 6;
1489     tft_cfont.offset = 0;
1490     tft_cfont.color  = tft_fg;
1491   }
1492   else {
1493           if (font == USER_FONT) {
1494                   if (load_file_font(font_file, 0) != 0) tft_cfont.font = tft_DefaultFont;
1495                   else tft_cfont.font = userfont;
1496           }
1497           else if (font == DEJAVU18_FONT) tft_cfont.font = tft_Dejavu18;
1498           else if (font == DEJAVU24_FONT) tft_cfont.font = tft_Dejavu24;
1499           else if (font == UBUNTU16_FONT) tft_cfont.font = tft_Ubuntu16;
1500           else if (font == COMIC24_FONT) tft_cfont.font = tft_Comic24;
1501           else if (font == MINYA24_FONT) tft_cfont.font = tft_minya24;
1502           else if (font == TOONEY32_FONT) tft_cfont.font = tft_tooney32;
1503           else if (font == SMALL_FONT) tft_cfont.font = tft_SmallFont;
1504           else if (font == DEF_SMALL_FONT) tft_cfont.font = tft_def_small;
1505           else tft_cfont.font = tft_DefaultFont;
1506
1507           tft_cfont.bitmap = 1;
1508           tft_cfont.x_size = tft_cfont.font[0];
1509           tft_cfont.y_size = tft_cfont.font[1];
1510           if (tft_cfont.x_size > 0) {
1511                   tft_cfont.offset = tft_cfont.font[2];
1512                   tft_cfont.numchars = tft_cfont.font[3];
1513                   tft_cfont.size = tft_cfont.x_size * tft_cfont.y_size * tft_cfont.numchars;
1514           }
1515           else {
1516                   tft_cfont.offset = 4;
1517                   getMaxWidthHeight();
1518           }
1519           //_testFont();
1520   }
1521 }
1522
1523 // -----------------------------------------------------------------------------------------
1524 // Individual Proportional Font Character Format:
1525 // -----------------------------------------------------------------------------------------
1526 // Character Code
1527 // yOffset                              (start Y of visible pixels)
1528 // Width                                (width of the visible pixels)
1529 // Height                               (height of the visible pixels)
1530 // xOffset                              (start X of visible pixels)
1531 // xDelta                               (the distance to move the cursor. Effective width of the character.)
1532 // Data[n]
1533 // -----------------------------------------------------------------------------------------
1534 //---------------------------------------------------------------------------------------------
1535 // Character drawing rectangle is (0, 0) (xDelta-1, tft_cfont.y_size-1)
1536 // Character visible pixels rectangle is (xOffset, yOffset) (xOffset+Width-1, yOffset+Height-1)
1537 //---------------------------------------------------------------------------------------------
1538
1539 // print non-rotated proportional character
1540 // character is already in fontChar
1541 //----------------------------------------------
1542 static int printProportionalChar(int x, int y) {
1543         uint8_t ch = 0;
1544         int i, j, char_width;
1545
1546         char_width = ((fontChar.width > fontChar.xDelta) ? fontChar.width : fontChar.xDelta);
1547
1548         if ((tft_font_buffered_char) && (!tft_font_transparent)) {
1549                 int len, bufPos;
1550
1551                 // === buffer Glyph data for faster sending ===
1552                 len = char_width * tft_cfont.y_size;
1553                 color_t *color_line = heap_caps_malloc(len*3, MALLOC_CAP_DMA);
1554                 if (color_line) {
1555                         // fill with background color
1556                         for (int n = 0; n < len; n++) {
1557                                 color_line[n] = tft_bg;
1558                         }
1559                         // set character pixels to foreground color
1560                         uint8_t mask = 0x80;
1561                         for (j=0; j < fontChar.height; j++) {
1562                                 for (i=0; i < fontChar.width; i++) {
1563                                         if (((i + (j*fontChar.width)) % 8) == 0) {
1564                                                 mask = 0x80;
1565                                                 ch = tft_cfont.font[fontChar.dataPtr++];
1566                                         }
1567                                         if ((ch & mask) != 0) {
1568                                                 // visible pixel
1569                                                 bufPos = ((j + fontChar.adjYOffset) * char_width) + (fontChar.xOffset + i);  // bufY + bufX
1570                                                 color_line[bufPos] = tft_fg;
1571                                                 /*
1572                                                 bufY = (j + fontChar.adjYOffset) * char_width;
1573                                                 bufX = fontChar.xOffset + i;
1574                                                 if ((bufX < 0) || (bufX > char_width)) {
1575                                                         printf("[%c] X ERR: %d\r\n", fontChar.charCode, bufX);
1576                                                 }
1577                                                 bufPos = bufY + bufX;
1578                                                 if ((bufPos < len) && (bufPos > 0)) color_line[bufPos] = tft_fg;
1579                                                 else printf("[%c] ERR: %d > %d  W=%d  H=%d  bufX=%d  bufY=%d  X=%d  Y=%d\r\n",
1580                                                                 fontChar.charCode, bufPos, len, char_width, tft_cfont.y_size, bufX, bufY, fontChar.xOffset + i, j + fontChar.adjYOffset);
1581                                                 */
1582                                         }
1583                                         mask >>= 1;
1584                                 }
1585                         }
1586                         // send to display in one transaction
1587                         disp_select();
1588                         send_data(x, y, x+char_width-1, y+tft_cfont.y_size-1, len, color_line);
1589                         disp_deselect();
1590                         free(color_line);
1591
1592                         return char_width;
1593                 }
1594         }
1595
1596         int cx, cy;
1597
1598         if (!tft_font_transparent) _fillRect(x, y, char_width+1, tft_cfont.y_size, tft_bg);
1599
1600         // draw Glyph
1601         uint8_t mask = 0x80;
1602         disp_select();
1603         for (j=0; j < fontChar.height; j++) {
1604                 for (i=0; i < fontChar.width; i++) {
1605                         if (((i + (j*fontChar.width)) % 8) == 0) {
1606                                 mask = 0x80;
1607                                 ch = tft_cfont.font[fontChar.dataPtr++];
1608                         }
1609
1610                         if ((ch & mask) !=0) {
1611                                 cx = (uint16_t)(x+fontChar.xOffset+i);
1612                                 cy = (uint16_t)(y+j+fontChar.adjYOffset);
1613                                 _drawPixel(cx, cy, tft_fg, 0);
1614                         }
1615                         mask >>= 1;
1616                 }
1617         }
1618         disp_deselect();
1619
1620         return char_width;
1621 }
1622
1623 // non-rotated fixed width character
1624 //----------------------------------------------
1625 static void printChar(uint8_t c, int x, int y) {
1626         uint8_t i, j, ch, fz, mask;
1627         uint16_t k, temp, cx, cy, len;
1628
1629         // fz = bytes per char row
1630         fz = tft_cfont.x_size/8;
1631         if (tft_cfont.x_size % 8) fz++;
1632
1633         // get character position in buffer
1634         temp = ((c-tft_cfont.offset)*((fz)*tft_cfont.y_size))+4;
1635
1636         if ((tft_font_buffered_char) && (!tft_font_transparent)) {
1637                 // === buffer Glyph data for faster sending ===
1638                 len = tft_cfont.x_size * tft_cfont.y_size;
1639                 color_t *color_line = heap_caps_malloc(len*3, MALLOC_CAP_DMA);
1640                 if (color_line) {
1641                         // fill with background color
1642                         for (int n = 0; n < len; n++) {
1643                                 color_line[n] = tft_bg;
1644                         }
1645                         // set character pixels to foreground color
1646                         for (j=0; j<tft_cfont.y_size; j++) {
1647                                 for (k=0; k < fz; k++) {
1648                                         ch = tft_cfont.font[temp+k];
1649                                         mask=0x80;
1650                                         for (i=0; i<8; i++) {
1651                                                 if ((ch & mask) !=0) color_line[(j*tft_cfont.x_size) + (i+(k*8))] = tft_fg;
1652                                                 mask >>= 1;
1653                                         }
1654                                 }
1655                                 temp += (fz);
1656                         }
1657                         // send to display in one transaction
1658                         disp_select();
1659                         send_data(x, y, x+tft_cfont.x_size-1, y+tft_cfont.y_size-1, len, color_line);
1660                         disp_deselect();
1661                         free(color_line);
1662
1663                         return;
1664                 }
1665         }
1666
1667         if (!tft_font_transparent) _fillRect(x, y, tft_cfont.x_size, tft_cfont.y_size, tft_bg);
1668
1669         disp_select();
1670         for (j=0; j<tft_cfont.y_size; j++) {
1671                 for (k=0; k < fz; k++) {
1672                         ch = tft_cfont.font[temp+k];
1673                         mask=0x80;
1674                         for (i=0; i<8; i++) {
1675                                 if ((ch & mask) !=0) {
1676                                         cx = (uint16_t)(x+i+(k*8));
1677                                         cy = (uint16_t)(y+j);
1678                                         _drawPixel(cx, cy, tft_fg, 0);
1679                                 }
1680                                 mask >>= 1;
1681                         }
1682                 }
1683                 temp += (fz);
1684         }
1685         disp_deselect();
1686 }
1687
1688 // print rotated proportional character
1689 // character is already in fontChar
1690 //---------------------------------------------------
1691 static int rotatePropChar(int x, int y, int offset) {
1692   uint8_t ch = 0;
1693   double radian = tft_font_rotate * DEG_TO_RAD;
1694   float cos_radian = cos(radian);
1695   float sin_radian = sin(radian);
1696
1697   uint8_t mask = 0x80;
1698   disp_select();
1699   for (int j=0; j < fontChar.height; j++) {
1700     for (int i=0; i < fontChar.width; i++) {
1701       if (((i + (j*fontChar.width)) % 8) == 0) {
1702         mask = 0x80;
1703         ch = tft_cfont.font[fontChar.dataPtr++];
1704       }
1705
1706       int newX = (int)(x + (((offset + i) * cos_radian) - ((j+fontChar.adjYOffset)*sin_radian)));
1707       int newY = (int)(y + (((j+fontChar.adjYOffset) * cos_radian) + ((offset + i) * sin_radian)));
1708
1709       if ((ch & mask) != 0) _drawPixel(newX,newY,tft_fg, 0);
1710       else if (!tft_font_transparent) _drawPixel(newX,newY,tft_bg, 0);
1711
1712       mask >>= 1;
1713     }
1714   }
1715   disp_deselect();
1716
1717   return fontChar.xDelta+1;
1718 }
1719
1720 // rotated fixed width character
1721 //--------------------------------------------------------
1722 static void rotateChar(uint8_t c, int x, int y, int pos) {
1723   uint8_t i,j,ch,fz,mask;
1724   uint16_t temp;
1725   int newx,newy;
1726   double radian = tft_font_rotate*0.0175;
1727   float cos_radian = cos(radian);
1728   float sin_radian = sin(radian);
1729   int zz;
1730
1731   if( tft_cfont.x_size < 8 ) fz = tft_cfont.x_size;
1732   else fz = tft_cfont.x_size/8;
1733   temp=((c-tft_cfont.offset)*((fz)*tft_cfont.y_size))+4;
1734
1735   disp_select();
1736   for (j=0; j<tft_cfont.y_size; j++) {
1737     for (zz=0; zz<(fz); zz++) {
1738       ch = tft_cfont.font[temp+zz];
1739       mask = 0x80;
1740       for (i=0; i<8; i++) {
1741         newx=(int)(x+(((i+(zz*8)+(pos*tft_cfont.x_size))*cos_radian)-((j)*sin_radian)));
1742         newy=(int)(y+(((j)*cos_radian)+((i+(zz*8)+(pos*tft_cfont.x_size))*sin_radian)));
1743
1744         if ((ch & mask) != 0) _drawPixel(newx,newy,tft_fg, 0);
1745         else if (!tft_font_transparent) _drawPixel(newx,newy,tft_bg, 0);
1746         mask >>= 1;
1747       }
1748     }
1749     temp+=(fz);
1750   }
1751   disp_deselect();
1752   // calculate x,y for the next char
1753   tft_x = (int)(x + ((pos+1) * tft_cfont.x_size * cos_radian));
1754   tft_y = (int)(y + ((pos+1) * tft_cfont.x_size * sin_radian));
1755 }
1756
1757 //----------------------
1758 static int _7seg_width()
1759 {
1760         return (2 * (2 * tft_cfont.y_size + 1)) + tft_cfont.x_size;
1761 }
1762
1763 //-----------------------
1764 static int _7seg_height()
1765 {
1766         return (3 * (2 * tft_cfont.y_size + 1)) + (2 * tft_cfont.x_size);
1767 }
1768
1769 // Returns the string width in pixels.
1770 // Useful for positions strings on the screen.
1771 //=====================================
1772 int TFT_getStringWidth(const char *str)
1773 {
1774     int strWidth = 0;
1775
1776         if (tft_cfont.bitmap == 2) strWidth = ((_7seg_width()+2) * strlen(str)) - 2;    // 7-segment font
1777         else if (tft_cfont.x_size != 0) strWidth = strlen(str) * tft_cfont.x_size;                      // fixed width font
1778         else {
1779                 // calculate the width of the string of proportional characters
1780                 const char *tempStrptr = str;
1781                 while (*tempStrptr != 0) {
1782                         if (getCharPtr(*tempStrptr++)) {
1783                                 strWidth += (((fontChar.width > fontChar.xDelta) ? fontChar.width : fontChar.xDelta) + 1);
1784                         }
1785                 }
1786                 strWidth--;
1787         }
1788         return strWidth;
1789 }
1790
1791 //=====================================================
1792 void TFT_clearStringRect(int x, int y, const char *str)
1793 {
1794         int w = TFT_getStringWidth(str);
1795         int h = TFT_getfontheight();
1796         TFT_fillRect(x+tft_dispWin.x1, y+tft_dispWin.y1, w, h, tft_bg);
1797 }
1798
1799 //==============================================================================
1800 /**
1801  * bit-encoded bar position of all digits' bcd segments
1802  *
1803  *           6
1804  *                +-----+
1805  *              3 |  .  | 2
1806  *                +--5--+
1807  *              1 |  .  | 0
1808  *                +--.--+
1809  *                   4
1810  */
1811 static const uint16_t font_bcd[] = {
1812   0x200, // 0010 0000 0000  // -
1813   0x080, // 0000 1000 0000  // .
1814   0x06C, // 0100 0110 1100  // /, degree
1815   0x05f, // 0000 0101 1111, // 0
1816   0x005, // 0000 0000 0101, // 1
1817   0x076, // 0000 0111 0110, // 2
1818   0x075, // 0000 0111 0101, // 3
1819   0x02d, // 0000 0010 1101, // 4
1820   0x079, // 0000 0111 1001, // 5
1821   0x07b, // 0000 0111 1011, // 6
1822   0x045, // 0000 0100 0101, // 7
1823   0x07f, // 0000 0111 1111, // 8
1824   0x07d, // 0000 0111 1101  // 9
1825   0x900  // 1001 0000 0000  // :
1826 };
1827
1828 //-----------------------------------------------------------------------------------------------
1829 static void barVert(int16_t x, int16_t y, int16_t w, int16_t l, color_t color, color_t outline) {
1830   _fillTriangle(x+1, y+2*w, x+w, y+w+1, x+2*w-1, y+2*w, color);
1831   _fillTriangle(x+1, y+2*w+l+1, x+w, y+3*w+l, x+2*w-1, y+2*w+l+1, color);
1832   _fillRect(x, y+2*w+1, 2*w+1, l, color);
1833   if (tft_cfont.offset) {
1834     _drawTriangle(x+1, y+2*w, x+w, y+w+1, x+2*w-1, y+2*w, outline);
1835     _drawTriangle(x+1, y+2*w+l+1, x+w, y+3*w+l, x+2*w-1, y+2*w+l+1, outline);
1836     _drawRect(x, y+2*w+1, 2*w+1, l, outline);
1837   }
1838 }
1839
1840 //----------------------------------------------------------------------------------------------
1841 static void barHor(int16_t x, int16_t y, int16_t w, int16_t l, color_t color, color_t outline) {
1842   _fillTriangle(x+2*w, y+2*w-1, x+w+1, y+w, x+2*w, y+1, color);
1843   _fillTriangle(x+2*w+l+1, y+2*w-1, x+3*w+l, y+w, x+2*w+l+1, y+1, color);
1844   _fillRect(x+2*w+1, y, l, 2*w+1, color);
1845   if (tft_cfont.offset) {
1846     _drawTriangle(x+2*w, y+2*w-1, x+w+1, y+w, x+2*w, y+1, outline);
1847     _drawTriangle(x+2*w+l+1, y+2*w-1, x+3*w+l, y+w, x+2*w+l+1, y+1, outline);
1848     _drawRect(x+2*w+1, y, l, 2*w+1, outline);
1849   }
1850 }
1851
1852 //--------------------------------------------------------------------------------------------
1853 static void _draw7seg(int16_t x, int16_t y, int8_t num, int16_t w, int16_t l, color_t color) {
1854   /* TODO: clipping */
1855   if (num < 0x2D || num > 0x3A) return;
1856
1857   int16_t c = font_bcd[num-0x2D];
1858   int16_t d = 2*w+l+1;
1859
1860   // === Clear unused segments ===
1861   if (!(c & 0x001)) barVert(x+d, y+d, w, l, tft_bg, tft_bg);
1862   if (!(c & 0x002)) barVert(x,   y+d, w, l, tft_bg, tft_bg);
1863   if (!(c & 0x004)) barVert(x+d, y, w, l, tft_bg, tft_bg);
1864   if (!(c & 0x008)) barVert(x,   y, w, l, tft_bg, tft_bg);
1865   if (!(c & 0x010)) barHor(x, y+2*d, w, l, tft_bg, tft_bg);
1866   if (!(c & 0x020)) barHor(x, y+d, w, l, tft_bg, tft_bg);
1867   if (!(c & 0x040)) barHor(x, y, w, l, tft_bg, tft_bg);
1868
1869   if (!(c & 0x080)) {
1870     // low point
1871     _fillRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, tft_bg);
1872     if (tft_cfont.offset) _drawRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, tft_bg);
1873   }
1874   if (!(c & 0x100)) {
1875     // down middle point
1876     _fillRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, tft_bg);
1877     if (tft_cfont.offset) _drawRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, tft_bg);
1878   }
1879   if (!(c & 0x800)) {
1880         // up middle point
1881     _fillRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, tft_bg);
1882     if (tft_cfont.offset) _drawRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, tft_bg);
1883   }
1884   if (!(c & 0x200)) {
1885     // middle, minus
1886     _fillRect(x+2*w+1, y+d, l, 2*w+1, tft_bg);
1887     if (tft_cfont.offset) _drawRect(x+2*w+1, y+d, l, 2*w+1, tft_bg);
1888   }
1889
1890   // === Draw used segments ===
1891   if (c & 0x001) barVert(x+d, y+d, w, l, color, tft_cfont.color);       // down right
1892   if (c & 0x002) barVert(x,   y+d, w, l, color, tft_cfont.color);       // down left
1893   if (c & 0x004) barVert(x+d, y, w, l, color, tft_cfont.color);         // up right
1894   if (c & 0x008) barVert(x,   y, w, l, color, tft_cfont.color);         // up left
1895   if (c & 0x010) barHor(x, y+2*d, w, l, color, tft_cfont.color);        // down
1896   if (c & 0x020) barHor(x, y+d, w, l, color, tft_cfont.color);          // middle
1897   if (c & 0x040) barHor(x, y, w, l, color, tft_cfont.color);            // up
1898
1899   if (c & 0x080) {
1900     // low point
1901     _fillRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, color);
1902     if (tft_cfont.offset) _drawRect(x+(d/2), y+2*d, 2*w+1, 2*w+1, tft_cfont.color);
1903   }
1904   if (c & 0x100) {
1905     // down middle point
1906     _fillRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, color);
1907     if (tft_cfont.offset) _drawRect(x+(d/2), y+d+2*w+1, 2*w+1, l/2, tft_cfont.color);
1908   }
1909   if (c & 0x800) {
1910         // up middle point
1911     _fillRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, color);
1912     if (tft_cfont.offset) _drawRect(x+(d/2), y+(2*w)+1+(l/2), 2*w+1, l/2, tft_cfont.color);
1913   }
1914   if (c & 0x200) {
1915     // middle, minus
1916     _fillRect(x+2*w+1, y+d, l, 2*w+1, color);
1917     if (tft_cfont.offset) _drawRect(x+2*w+1, y+d, l, 2*w+1, tft_cfont.color);
1918   }
1919 }
1920 //==============================================================================
1921
1922 //============================================
1923 void TFT_print(const char *st, int x, int y) {
1924         int stl, i, tmpw, tmph, fh;
1925         uint8_t ch;
1926
1927         if (tft_cfont.bitmap == 0) return; // wrong font selected
1928
1929         // ** Rotated strings cannot be aligned
1930         if ((tft_font_rotate != 0) && ((x <= CENTER) || (y <= CENTER))) return;
1931
1932         if ((x < LASTX) || (tft_font_rotate == 0)) TFT_OFFSET = 0;
1933
1934         if ((x >= LASTX) && (x < LASTY)) x = tft_x + (x-LASTX);
1935         else if (x > CENTER) x += tft_dispWin.x1;
1936
1937         if (y >= LASTY) y = tft_y + (y-LASTY);
1938         else if (y > CENTER) y += tft_dispWin.y1;
1939
1940         // ** Get number of characters in string to print
1941         stl = strlen(st);
1942
1943         // ** Calculate CENTER, RIGHT or BOTTOM position
1944         tmpw = TFT_getStringWidth(st);  // string width in pixels
1945         fh = tft_cfont.y_size;                  // font height
1946         if ((tft_cfont.x_size != 0) && (tft_cfont.bitmap == 2)) {
1947                 // 7-segment font
1948                 fh = (3 * (2 * tft_cfont.y_size + 1)) + (2 * tft_cfont.x_size);  // 7-seg character height
1949         }
1950
1951         if (x == RIGHT) x = tft_dispWin.x2 - tmpw + tft_dispWin.x1;
1952         else if (x == CENTER) x = (((tft_dispWin.x2 - tft_dispWin.x1 + 1) - tmpw) / 2) + tft_dispWin.x1;
1953
1954         if (y == BOTTOM) y = tft_dispWin.y2 - fh + tft_dispWin.y1;
1955         else if (y==CENTER) y = (((tft_dispWin.y2 - tft_dispWin.y1 + 1) - (fh/2)) / 2) + tft_dispWin.y1;
1956
1957         if (x < tft_dispWin.x1) x = tft_dispWin.x1;
1958         if (y < tft_dispWin.y1) y = tft_dispWin.y1;
1959         if ((x > tft_dispWin.x2) || (y > tft_dispWin.y2)) return;
1960
1961         tft_x = x;
1962         tft_y = y;
1963
1964         // ** Adjust y position
1965         tmph = tft_cfont.y_size; // font height
1966         // for non-proportional fonts, char width is the same for all chars
1967         tmpw = tft_cfont.x_size;
1968         if (tft_cfont.x_size != 0) {
1969                 if (tft_cfont.bitmap == 2) {    // 7-segment font
1970                         tmpw = _7seg_width();   // character width
1971                         tmph = _7seg_height();  // character height
1972                 }
1973         }
1974         else TFT_OFFSET = 0;    // fixed font; offset not needed
1975
1976         if ((tft_y + tmph - 1) > tft_dispWin.y2) return;
1977
1978         int offset = TFT_OFFSET;
1979
1980         for (i=0; i<stl; i++) {
1981                 ch = st[i]; // get string character
1982
1983                 if (ch == 0x0D) { // === '\r', erase to eol ====
1984                         if ((!tft_font_transparent) && (tft_font_rotate==0)) _fillRect(tft_x, tft_y,  tft_dispWin.x2+1-tft_x, tmph, tft_bg);
1985                 }
1986
1987                 else if (ch == 0x0A) { // ==== '\n', new line ====
1988                         if (tft_cfont.bitmap == 1) {
1989                                 tft_y += tmph + tft_font_line_space;
1990                                 if (tft_y > (tft_dispWin.y2-tmph)) break;
1991                                 tft_x = tft_dispWin.x1;
1992                         }
1993                 }
1994
1995                 else { // ==== other characters ====
1996                         if (tft_cfont.x_size == 0) {
1997                                 // for proportional font get character data to 'fontChar'
1998                                 if (getCharPtr(ch)) tmpw = fontChar.xDelta;
1999                                 else continue;
2000                         }
2001
2002                         // check if character can be displayed in the current line
2003                         if ((tft_x+tmpw) > (tft_dispWin.x2)) {
2004                                 if (tft_text_wrap == 0) break;
2005                                 tft_y += tmph + tft_font_line_space;
2006                                 if (tft_y > (tft_dispWin.y2-tmph)) break;
2007                                 tft_x = tft_dispWin.x1;
2008                         }
2009
2010                         // Let's print the character
2011                         if (tft_cfont.x_size == 0) {
2012                                 // == proportional font
2013                                 if (tft_font_rotate == 0) tft_x += printProportionalChar(tft_x, tft_y) + 1;
2014                                 else {
2015                                         // rotated proportional font
2016                                         offset += rotatePropChar(x, y, offset);
2017                                         TFT_OFFSET = offset;
2018                                 }
2019                         }
2020                         else {
2021                                 if (tft_cfont.bitmap == 1) {
2022                                         // == fixed font
2023                                         if ((ch < tft_cfont.offset) || ((ch-tft_cfont.offset) > tft_cfont.numchars)) ch = tft_cfont.offset;
2024                                         if (tft_font_rotate == 0) {
2025                                                 printChar(ch, tft_x, tft_y);
2026                                                 tft_x += tmpw;
2027                                         }
2028                                         else rotateChar(ch, x, y, i);
2029                                 }
2030                                 else if (tft_cfont.bitmap == 2) {
2031                                         // == 7-segment font ==
2032                                         _draw7seg(tft_x, tft_y, ch, tft_cfont.y_size, tft_cfont.x_size, tft_fg);
2033                                         tft_x += (tmpw + 2);
2034                                 }
2035                         }
2036                 }
2037         }
2038 }
2039
2040
2041 // ================ Service functions ==========================================
2042
2043 // Change the screen rotation.
2044 // Input: m new rotation value (0 to 3)
2045 //=================================
2046 void TFT_setRotation(uint8_t rot) {
2047     if (rot > 3) {
2048         uint8_t madctl = (rot & 0xF8); // for testing, manually set MADCTL register
2049                 if (disp_select() == ESP_OK) {
2050                         disp_spi_transfer_cmd_data(TFT_MADCTL, &madctl, 1);
2051                         disp_deselect();
2052                 }
2053     }
2054         else {
2055                 tft_orientation = rot;
2056         _tft_setRotation(rot);
2057         }
2058
2059         tft_dispWin.x1 = TFT_STATIC_X_OFFSET;
2060         tft_dispWin.y1 = TFT_STATIC_Y_OFFSET;
2061         tft_dispWin.x2 = tft_width + TFT_STATIC_X_OFFSET -1;
2062         tft_dispWin.y2 = tft_height + TFT_STATIC_Y_OFFSET -1;
2063
2064         TFT_fillScreen(tft_bg);
2065 }
2066
2067 // Send the command to invert all of the colors.
2068 // Input: i 0 to disable inversion; non-zero to enable inversion
2069 //==========================================
2070 void TFT_invertDisplay(const uint8_t mode) {
2071     if (disp_select() == ESP_OK) {
2072         if ( mode == INVERT_ON ) disp_spi_transfer_cmd(TFT_INVONN);
2073         else disp_spi_transfer_cmd(TFT_INVOFF);
2074         disp_deselect();
2075     }
2076 }
2077
2078 // Select gamma curve
2079 // Input: gamma = 0~3
2080 //==================================
2081 void TFT_setGammaCurve(uint8_t gm) {
2082     uint8_t gamma_curve = (uint8_t)1 << (gm & (uint8_t)0x03);
2083         if (disp_select() == ESP_OK) {
2084                 disp_spi_transfer_cmd_data(TFT_CMD_GAMMASET, &gamma_curve, 1);
2085                 disp_deselect();
2086         }
2087 }
2088
2089 //===========================================================
2090 color_t HSBtoRGB(float _hue, float _sat, float _brightness) {
2091  float red = 0.0;
2092  float green = 0.0;
2093  float blue = 0.0;
2094
2095  if (_sat == 0.0) {
2096    red = _brightness;
2097    green = _brightness;
2098    blue = _brightness;
2099  } else {
2100    if (_hue == 360.0) {
2101      _hue = 0;
2102    }
2103
2104    int slice = (int)(_hue / 60.0);
2105    float hue_frac = (_hue / 60.0) - slice;
2106
2107    float aa = _brightness * (1.0 - _sat);
2108    float bb = _brightness * (1.0 - _sat * hue_frac);
2109    float cc = _brightness * (1.0 - _sat * (1.0 - hue_frac));
2110
2111    switch(slice) {
2112      case 0:
2113          red = _brightness;
2114          green = cc;
2115          blue = aa;
2116          break;
2117      case 1:
2118          red = bb;
2119          green = _brightness;
2120          blue = aa;
2121          break;
2122      case 2:
2123          red = aa;
2124          green = _brightness;
2125          blue = cc;
2126          break;
2127      case 3:
2128          red = aa;
2129          green = bb;
2130          blue = _brightness;
2131          break;
2132      case 4:
2133          red = cc;
2134          green = aa;
2135          blue = _brightness;
2136          break;
2137      case 5:
2138          red = _brightness;
2139          green = aa;
2140          blue = bb;
2141          break;
2142      default:
2143          red = 0.0;
2144          green = 0.0;
2145          blue = 0.0;
2146          break;
2147    }
2148  }
2149
2150  color_t color;
2151  color.r = ((uint8_t)(red * 255.0)) & 0xFC;
2152  color.g = ((uint8_t)(green * 255.0)) & 0xFC;
2153  color.b = ((uint8_t)(blue * 255.0)) & 0xFC;
2154
2155  return color;
2156 }
2157 //=====================================================================
2158 void TFT_setclipwin(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
2159 {
2160         tft_dispWin.x1 = x1 + TFT_STATIC_X_OFFSET;
2161         tft_dispWin.y1 = y1 + TFT_STATIC_Y_OFFSET;
2162         tft_dispWin.x2 = x2 + TFT_STATIC_X_OFFSET;
2163         tft_dispWin.y2 = y2 + TFT_STATIC_Y_OFFSET;
2164
2165         if (tft_dispWin.x2 >= tft_width + TFT_STATIC_X_OFFSET) tft_dispWin.x2 = tft_width + TFT_STATIC_X_OFFSET -1;
2166         if (tft_dispWin.y2 >= tft_height + TFT_STATIC_Y_OFFSET) tft_dispWin.y2 = tft_height + TFT_STATIC_Y_OFFSET -1;
2167         if (tft_dispWin.x1 > tft_dispWin.x2) tft_dispWin.x1 = tft_dispWin.x2;
2168         if (tft_dispWin.y1 > tft_dispWin.y2) tft_dispWin.y1 = tft_dispWin.y2;
2169 }
2170
2171 //=====================
2172 void TFT_resetclipwin()
2173 {
2174         tft_dispWin.x2 = tft_width + TFT_STATIC_X_OFFSET -1;
2175         tft_dispWin.y2 = tft_height + TFT_STATIC_Y_OFFSET -1;
2176         tft_dispWin.x1 = TFT_STATIC_X_OFFSET;
2177         tft_dispWin.y1 = TFT_STATIC_Y_OFFSET;
2178 }
2179
2180 //==========================================================================
2181 void set_7seg_font_atrib(uint8_t l, uint8_t w, int outline, color_t color) {
2182         if (tft_cfont.bitmap != 2) return;
2183
2184         if (l < 6) l = 6;
2185     if (l > 40) l = 40;
2186     if (w < 1) w = 1;
2187     if (w > (l/2)) w = l/2;
2188     if (w > 12) w = 12;
2189
2190     tft_cfont.x_size = l;
2191         tft_cfont.y_size = w;
2192         tft_cfont.offset = outline;
2193         tft_cfont.color  = color;
2194 }
2195
2196 //==========================================
2197 int TFT_getfontsize(int *width, int* height)
2198 {
2199   if (tft_cfont.bitmap == 1) {
2200     if (tft_cfont.x_size != 0) *width = tft_cfont.x_size;       // fixed width font
2201     else *width = tft_cfont.max_x_size;                                 // proportional font
2202     *height = tft_cfont.y_size;
2203   }
2204   else if (tft_cfont.bitmap == 2) {
2205         // 7-segment font
2206     *width = _7seg_width();
2207     *height = _7seg_height();
2208   }
2209   else {
2210     *width = 0;
2211     *height = 0;
2212     return 0;
2213   }
2214   return 1;
2215 }
2216
2217 //=====================
2218 int TFT_getfontheight()
2219 {
2220   if (tft_cfont.bitmap == 1) return tft_cfont.y_size;                   // Bitmap font
2221   else if (tft_cfont.bitmap == 2) return _7seg_height();        // 7-segment font
2222   return 0;
2223 }
2224
2225 //====================
2226 void TFT_saveClipWin()
2227 {
2228         dispWinTemp.x1 = tft_dispWin.x1;
2229         dispWinTemp.y1 = tft_dispWin.y1;
2230         dispWinTemp.x2 = tft_dispWin.x2;
2231         dispWinTemp.y2 = tft_dispWin.y2;
2232 }
2233
2234 //=======================
2235 void TFT_restoreClipWin()
2236 {
2237         tft_dispWin.x1 = dispWinTemp.x1;
2238         tft_dispWin.y1 = dispWinTemp.y1;
2239         tft_dispWin.x2 = dispWinTemp.x2;
2240         tft_dispWin.y2 = dispWinTemp.y2;
2241 }
2242
2243
2244 // ================ JPG SUPPORT ================================================
2245 // User defined device identifier
2246 typedef struct {
2247         FILE            *fhndl;                 // File handler for input function
2248     int                 x;                              // image top left point X position
2249     int                 y;                              // image top left point Y position
2250     uint8_t             *membuff;               // memory buffer containing the image
2251     uint32_t    bufsize;                // size of the memory buffer
2252     uint32_t    bufptr;                 // memory buffer current position
2253     color_t             *linbuf[2];             // memory buffer used for display output
2254     uint8_t             linbuf_idx;
2255 } JPGIODEV;
2256
2257
2258 // User defined call-back function to input JPEG data from file
2259 //---------------------
2260 static UINT tjd_input (
2261         JDEC* jd,               // Decompression object
2262         BYTE* buff,             // Pointer to the read buffer (NULL:skip)
2263         UINT nd                 // Number of bytes to read/skip from input stream
2264 )
2265 {
2266         int rb = 0;
2267         // Device identifier for the session (5th argument of jd_prepare function)
2268         JPGIODEV *dev = (JPGIODEV*)jd->device;
2269
2270         if (buff) {     // Read nd bytes from the input strem
2271                 rb = fread(buff, 1, nd, dev->fhndl);
2272                 return rb;      // Returns actual number of bytes read
2273         }
2274         else {  // Remove nd bytes from the input stream
2275                 if (fseek(dev->fhndl, nd, SEEK_CUR) >= 0) return nd;
2276                 else return 0;
2277         }
2278 }
2279
2280 // User defined call-back function to input JPEG data from memory buffer
2281 //-------------------------
2282 static UINT tjd_buf_input (
2283         JDEC* jd,               // Decompression object
2284         BYTE* buff,             // Pointer to the read buffer (NULL:skip)
2285         UINT nd                 // Number of bytes to read/skip from input stream
2286 )
2287 {
2288         // Device identifier for the session (5th argument of jd_prepare function)
2289         JPGIODEV *dev = (JPGIODEV*)jd->device;
2290         if (!dev->membuff) return 0;
2291         if (dev->bufptr >= (dev->bufsize + 2)) return 0; // end of stream
2292
2293         if ((dev->bufptr + nd) > (dev->bufsize + 2)) nd = (dev->bufsize + 2) - dev->bufptr;
2294
2295         if (buff) {     // Read nd bytes from the input strem
2296                 memcpy(buff, dev->membuff + dev->bufptr, nd);
2297                 dev->bufptr += nd;
2298                 return nd;      // Returns number of bytes read
2299         }
2300         else {  // Remove nd bytes from the input stream
2301                 dev->bufptr += nd;
2302                 return nd;
2303         }
2304 }
2305
2306 // User defined call-back function to output RGB bitmap to display device
2307 //----------------------
2308 static UINT tjd_output (
2309         JDEC* jd,               // Decompression object of current session
2310         void* bitmap,   // Bitmap data to be output
2311         JRECT* rect             // Rectangular region to output
2312 )
2313 {
2314         // Device identifier for the session (5th argument of jd_prepare function)
2315         JPGIODEV *dev = (JPGIODEV*)jd->device;
2316
2317         // ** Put the rectangular into the display device **
2318         int x;
2319         int y;
2320         int dleft, dtop, dright, dbottom;
2321         BYTE *src = (BYTE*)bitmap;
2322
2323         int left = rect->left + dev->x;
2324         int top = rect->top + dev->y;
2325         int right = rect->right + dev->x;
2326         int bottom = rect->bottom + dev->y;
2327
2328         if ((left > tft_dispWin.x2) || (top > tft_dispWin.y2)) return 1;        // out of screen area, return
2329         if ((right < tft_dispWin.x1) || (bottom < tft_dispWin.y1)) return 1;// out of screen area, return
2330
2331         if (left < tft_dispWin.x1) dleft = tft_dispWin.x1;
2332         else dleft = left;
2333         if (top < tft_dispWin.y1) dtop = tft_dispWin.y1;
2334         else dtop = top;
2335         if (right > tft_dispWin.x2) dright = tft_dispWin.x2;
2336         else dright = right;
2337         if (bottom > tft_dispWin.y2) dbottom = tft_dispWin.y2;
2338         else dbottom = bottom;
2339
2340         if ((dleft > tft_dispWin.x2) || (dtop > tft_dispWin.y2)) return 1;              // out of screen area, return
2341         if ((dright < tft_dispWin.x1) || (dbottom < tft_dispWin.y1)) return 1;  // out of screen area, return
2342
2343         uint32_t len = ((dright-dleft+1) * (dbottom-dtop+1));   // calculate length of data
2344
2345
2346         if ((len > 0) && (len <= JPG_IMAGE_LINE_BUF_SIZE)) {
2347                 uint8_t *dest = (uint8_t *)(dev->linbuf[dev->linbuf_idx]);
2348
2349                 for (y = top; y <= bottom; y++) {
2350                         for (x = left; x <= right; x++) {
2351                                 // Clip to display area
2352                                 if ((x >= dleft) && (y >= dtop) && (x <= dright) && (y <= dbottom)) {
2353                                         *dest++ = (*src++) & 0xFC;
2354                                         *dest++ = (*src++) & 0xFC;
2355                                         *dest++ = (*src++) & 0xFC;
2356                                 }
2357                                 else src += 3; // skip
2358                         }
2359                 }
2360                 wait_trans_finish(1);
2361                 send_data(dleft, dtop, dright, dbottom, len, dev->linbuf[dev->linbuf_idx]);
2362                 dev->linbuf_idx = ((dev->linbuf_idx + 1) & 1);
2363         }
2364         else {
2365                 wait_trans_finish(1);
2366                 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);
2367                 return 0;  // stop decompression
2368         }
2369
2370         return 1;       // Continue to decompression
2371 }
2372
2373 // tft.jpgimage(X, Y, scale, file_name, buf, size]
2374 // X & Y can be < 0 !
2375 //========================================================================================
2376 void TFT_jpg_image(int x, int y, uint8_t scale, const char *fname, uint8_t *buf, int size)
2377 {
2378         JPGIODEV dev;
2379     struct stat sb;
2380         char *work = NULL;              // Pointer to the working buffer (must be 4-byte aligned)
2381         UINT sz_work = 3800;    // Size of the working buffer (must be power of 2)
2382         JDEC jd;                                // Decompression object (70 bytes)
2383         JRESULT rc;
2384
2385         dev.linbuf[0] = NULL;
2386         dev.linbuf[1] = NULL;
2387     dev.linbuf_idx = 0;
2388
2389         dev.fhndl = NULL;
2390     if (fname == NULL) {
2391         // image from buffer
2392         dev.membuff = buf;
2393         dev.bufsize = size;
2394         dev.bufptr = 0;
2395     }
2396     else {
2397         // image from file
2398         dev.membuff = NULL;
2399         dev.bufsize = 0;
2400         dev.bufptr = 0;
2401
2402         if (stat(fname, &sb) != 0) {
2403                 if (tft_image_debug) printf("File error: %ss\r\n", strerror(errno));
2404             goto exit;
2405         }
2406
2407         dev.fhndl = fopen(fname, "r");
2408         if (!dev.fhndl) {
2409                 if (tft_image_debug) printf("Error opening file: %s\r\n", strerror(errno));
2410             goto exit;
2411         }
2412     }
2413
2414         if (scale > 3) scale = 3;
2415
2416         work = malloc(sz_work);
2417         if (work) {
2418                 if (dev.membuff) rc = jd_prepare(&jd, tjd_buf_input, (void *)work, sz_work, &dev);
2419                 else rc = jd_prepare(&jd, tjd_input, (void *)work, sz_work, &dev);
2420                 if (rc == JDR_OK) {
2421                         if (x == CENTER) x = ((tft_dispWin.x2 - tft_dispWin.x1 + 1 - (int)(jd.width >> scale)) / 2) + tft_dispWin.x1;
2422                         else if (x == RIGHT) x = tft_dispWin.x2 + 1 - (int)(jd.width >> scale);
2423
2424                         if (y == CENTER) y = ((tft_dispWin.y2 - tft_dispWin.y1 + 1 - (int)(jd.height >> scale)) / 2) + tft_dispWin.y1;
2425                         else if (y == BOTTOM) y = tft_dispWin.y2 + 1 - (int)(jd.height >> scale);
2426
2427                         if (x < ((tft_dispWin.x2-1) * -1)) x = (tft_dispWin.x2-1) * -1;
2428                         if (y < ((tft_dispWin.y2-1)) * -1) y = (tft_dispWin.y2-1) * -1;
2429                         if (x > (tft_dispWin.x2-1)) x = tft_dispWin.x2 - 1;
2430                         if (y > (tft_dispWin.y2-1)) y = tft_dispWin.y2-1;
2431
2432                         dev.x = x;
2433                         dev.y = y;
2434
2435                         dev.linbuf[0] = heap_caps_malloc(JPG_IMAGE_LINE_BUF_SIZE*3, MALLOC_CAP_DMA);
2436                         if (dev.linbuf[0] == NULL) {
2437                                 if (tft_image_debug) printf("Error allocating line buffer #0\r\n");
2438                                 goto exit;
2439                         }
2440                         dev.linbuf[1] = heap_caps_malloc(JPG_IMAGE_LINE_BUF_SIZE*3, MALLOC_CAP_DMA);
2441                         if (dev.linbuf[1] == NULL) {
2442                                 if (tft_image_debug) printf("Error allocating line buffer #1\r\n");
2443                                 goto exit;
2444                         }
2445
2446                         // Start to decode the JPEG file
2447                         disp_select();
2448                         rc = jd_decomp(&jd, tjd_output, scale);
2449                         disp_deselect();
2450
2451                         if (rc != JDR_OK) {
2452                                 if (tft_image_debug) printf("jpg decompression error %d\r\n", rc);
2453                         }
2454                         if (tft_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);
2455                 }
2456                 else {
2457                         if (tft_image_debug) printf("jpg prepare error %d\r\n", rc);
2458                 }
2459         }
2460         else {
2461                 if (tft_image_debug) printf("work buffer allocation error\r\n");
2462         }
2463
2464 exit:
2465         if (work) free(work);  // free work buffer
2466         if (dev.linbuf[0]) free(dev.linbuf[0]);
2467         if (dev.linbuf[1]) free(dev.linbuf[1]);
2468     if (dev.fhndl) fclose(dev.fhndl);  // close input file
2469 }
2470
2471
2472 //==========================================================================================
2473 int TFT_bmp_image(int x, int y, uint8_t scale, const char *fname, uint8_t *imgbuf, int size)
2474 {
2475         FILE *fhndl = NULL;
2476         struct stat sb;
2477         int i, err=0;
2478         int img_xsize, img_ysize, img_xstart, img_xlen, img_ystart, img_ylen;
2479         int img_pos, img_pix_pos, scan_lines, rd_len;
2480         uint8_t tmpc;
2481         uint16_t wtemp;
2482         uint32_t temp;
2483         int disp_xstart, disp_xend, disp_ystart, disp_yend;
2484         uint8_t buf[56];
2485         char err_buf[64];
2486         uint8_t *line_buf[2] = {NULL,NULL};
2487         uint8_t lb_idx = 0;
2488         uint8_t *scale_buf = NULL;
2489         uint8_t scale_pix;
2490         uint16_t co[3] = {0,0,0};                       // RGB sum
2491         uint8_t npix;
2492
2493         if (scale > 7) scale = 7;
2494         scale_pix = scale+1;    // scale factor ( 1~8 )
2495
2496     if (fname) {
2497         // * File name is given, reading image from file
2498         if (stat(fname, &sb) != 0) {
2499                         sprintf(err_buf, "opening file");
2500                 err = -1;
2501                 goto exit;
2502         }
2503         size = sb.st_size;
2504                 fhndl = fopen(fname, "r");
2505                 if (!fhndl) {
2506                         sprintf(err_buf, "opening file");
2507                         err = -2;
2508                         goto exit;
2509                 }
2510
2511                 i = fread(buf, 1, 54, fhndl);  // read header
2512     }
2513     else {
2514         // * Reading image from buffer
2515         if ((imgbuf) && (size > 54)) {
2516                 memcpy(buf, imgbuf, 54);
2517                 i = 54;
2518         }
2519         else i = 0;
2520     }
2521
2522     sprintf(err_buf, "reading header");
2523         if (i != 54) {err = -3; goto exit;}
2524
2525         // ** Check image header and get image properties
2526         if ((buf[0] != 'B') || (buf[1] != 'M')) {err=-4; goto exit;} // accept only images with 'BM' id
2527
2528         memcpy(&temp, buf+2, 4);                                // file size
2529         if (temp != size) {err=-5; goto exit;}
2530
2531         memcpy(&img_pos, buf+10, 4);                    // start of pixel data
2532
2533         memcpy(&temp, buf+14, 4);                               // BMP header size
2534         if (temp != 40) {err=-6; goto exit;}
2535
2536         memcpy(&wtemp, buf+26, 2);                              // the number of color planes
2537         if (wtemp != 1) {err=-7; goto exit;}
2538
2539         memcpy(&wtemp, buf+28, 2);                              // the number of bits per pixel
2540         if (wtemp != 24) {err=-8; goto exit;}
2541
2542         memcpy(&temp, buf+30, 4);                               // the compression method being used
2543         if (temp != 0) {err=-9; goto exit;}
2544
2545         memcpy(&img_xsize, buf+18, 4);                  // the bitmap width in pixels
2546         memcpy(&img_ysize, buf+22, 4);                  // the bitmap height in pixels
2547
2548
2549         // * scale image dimensions
2550
2551         img_xlen = img_xsize / scale_pix;               // image display horizontal size
2552         img_ylen = img_ysize / scale_pix;               // image display vertical size
2553
2554         if (x == CENTER) x = ((tft_dispWin.x2 - tft_dispWin.x1 + 1 - img_xlen) / 2) + tft_dispWin.x1;
2555         else if (x == RIGHT) x = tft_dispWin.x2 + 1 - img_xlen;
2556
2557         if (y == CENTER) y = ((tft_dispWin.y2 - tft_dispWin.y1 + 1 - img_ylen) / 2) + tft_dispWin.y1;
2558         else if (y == BOTTOM) y = tft_dispWin.y2 + 1 - img_ylen;
2559
2560         if ((x < ((tft_dispWin.x2 + 1) * -1)) || (x > (tft_dispWin.x2 + 1)) || (y < ((tft_dispWin.y2 + 1) * -1)) || (y > (tft_dispWin.y2 + 1))) {
2561                 sprintf(err_buf, "out of display area (%d,%d", x, y);
2562                 err = -10;
2563                 goto exit;
2564         }
2565
2566         // ** set display and image areas
2567         if (x < tft_dispWin.x1) {
2568                 disp_xstart = tft_dispWin.x1;
2569                 img_xstart = -x;        // image pixel line X offset
2570                 img_xlen += x;
2571         }
2572         else {
2573                 disp_xstart = x;
2574                 img_xstart = 0;
2575         }
2576         if (y < tft_dispWin.y1) {
2577                 disp_ystart = tft_dispWin.y1;
2578                 img_ystart = -y;        // image pixel line Y offset
2579                 img_ylen += y;
2580         }
2581         else {
2582                 disp_ystart = y;
2583                 img_ystart = 0;
2584         }
2585         disp_xend = disp_xstart + img_xlen - 1;
2586         disp_yend = disp_ystart + img_ylen - 1;
2587         if (disp_xend > tft_dispWin.x2) {
2588                 disp_xend = tft_dispWin.x2;
2589                 img_xlen = disp_xend - disp_xstart + 1;
2590         }
2591         if (disp_yend > tft_dispWin.y2) {
2592                 disp_yend = tft_dispWin.y2;
2593                 img_ylen = disp_yend - disp_ystart + 1;
2594         }
2595
2596         if ((img_xlen < 8) || (img_ylen < 8) || (img_xstart >= (img_xsize-2)) || ((img_ysize - img_ystart) < 2)) {
2597                 sprintf(err_buf, "image too small");
2598                 err = -11;
2599                 goto exit;
2600         }
2601
2602         // ** Allocate memory for 2 lines of image pixels
2603         line_buf[0] = heap_caps_malloc(img_xsize*3, MALLOC_CAP_DMA);
2604         if (line_buf[0] == NULL) {
2605             sprintf(err_buf, "allocating line buffer #1");
2606                 err=-12;
2607                 goto exit;
2608         }
2609
2610         line_buf[1] = heap_caps_malloc(img_xsize*3, MALLOC_CAP_DMA);
2611         if (line_buf[1] == NULL) {
2612             sprintf(err_buf, "allocating line buffer #2");
2613                 err=-13;
2614                 goto exit;
2615         }
2616
2617         if (scale) {
2618                 // Allocate memory for scale buffer
2619                 rd_len = img_xlen * 3 * scale_pix;
2620                 scale_buf = malloc(rd_len*scale_pix);
2621                 if (scale_buf == NULL) {
2622                         sprintf(err_buf, "allocating scale buffer");
2623                         err=-14;
2624                         goto exit;
2625                 }
2626         }
2627         else rd_len = img_xlen * 3;
2628
2629         // ** ***************************************************** **
2630         // ** BMP images are stored in file from LAST to FIRST line **
2631         // ** ***************************************************** **
2632
2633         /* Used variables:
2634                 img_xsize               horizontal image size in pixels
2635                 img_ysize               number of image lines
2636                 img_xlen                image display horizontal scaled size in pixels
2637                 img_ylen                image display vertical scaled size in pixels
2638                 img_xstart              first pixel in line to be displayed
2639                 img_ystart              first image line to be displayed
2640                 img_xlen                number of pixels in image line to be displayed, starting with 'img_xstart'
2641                 img_ylen                number of lines in image to be displayed, starting with 'img_ystart'
2642                 rd_len                  length of color data which are read from image line in bytes
2643          */
2644
2645         // Set position in image to the first color data (beginning of the LAST line)
2646         img_pos += (img_ystart * (img_xsize*3));
2647         if (fhndl) {
2648                 if (fseek(fhndl, img_pos, SEEK_SET) != 0) {
2649                         sprintf(err_buf, "file seek at %d", img_pos);
2650                         err = -15;
2651                         goto exit;
2652                 }
2653         }
2654
2655         if (tft_image_debug) printf("BMP: image size: (%d,%d) scale: %d disp size: (%d,%d) img xofs: %d img yofs: %d at: %d,%d; line buf: 2* %d scale buf: %d\r\n",
2656                         img_xsize, img_ysize, scale_pix, img_xlen, img_ylen, img_xstart, img_ystart, disp_xstart, disp_ystart, img_xsize*3, ((scale) ? (rd_len*scale_pix) : 0));
2657
2658         // * Select the display
2659         disp_select();
2660
2661         while ((disp_yend >= disp_ystart) && ((img_pos + (img_xsize*3)) <= size)) {
2662                 if (img_pos > size) {
2663                         sprintf(err_buf, "EOF reached: %d > %d", img_pos, size);
2664                         err = -16;
2665                         goto exit1;
2666                 }
2667                 if (scale == 0) {
2668                         // Read the line of color data into color buffer
2669                         if (fhndl) {
2670                                 i = fread(line_buf[lb_idx], 1, img_xsize*3, fhndl);  // read line from file
2671                                 if (i != (img_xsize*3)) {
2672                                         sprintf(err_buf, "file read at %d (%d<>%d)", img_pos, i, img_xsize*3);
2673                                         err = -16;
2674                                         goto exit1;
2675                                 }
2676                         }
2677                         else memcpy(line_buf[lb_idx], imgbuf+img_pos, img_xsize*3);
2678
2679                         if (img_xstart > 0)     memmove(line_buf[lb_idx], line_buf[lb_idx]+(img_xstart*3), rd_len);
2680                         // Convert colors BGR-888 (BMP) -> RGB-888 (DISPLAY) ===
2681                         for (i=0; i < rd_len; i += 3) {
2682                                 tmpc = line_buf[lb_idx][i+2] & 0xfc;                            // save R
2683                                 line_buf[lb_idx][i+2] = line_buf[lb_idx][i] & 0xfc;     // B -> R
2684                                 line_buf[lb_idx][i] = tmpc;                                                     // R -> B
2685                                 line_buf[lb_idx][i+1] &= 0xfc;                                          // G
2686                         }
2687                         img_pos += (img_xsize*3);
2688                 }
2689                 else {
2690                         // scale image, read 'scale_pix' lines and find the average color
2691                         for (scan_lines=0; scan_lines<scale_pix; scan_lines++) {
2692                                 if (img_pos > size) break;
2693                                 if (fhndl) {
2694                                         i = fread(line_buf[lb_idx], 1, img_xsize*3, fhndl);  // read line from file
2695                                         if (i != (img_xsize*3)) {
2696                                                 sprintf(err_buf, "file read at %d (%d<>%d)", img_pos, i, img_xsize*3);
2697                                                 err = -17;
2698                                                 goto exit1;
2699                                         }
2700                                 }
2701                                 else memcpy(line_buf[lb_idx], imgbuf+img_pos, img_xsize*3);
2702                                 img_pos += (img_xsize*3);
2703
2704                                 // copy only data which are displayed to scale buffer
2705                                 memcpy(scale_buf + (rd_len * scan_lines), line_buf[lb_idx]+img_xstart, rd_len);
2706                         }
2707
2708                         // Populate display line buffer
2709                         for (int n=0;n<(img_xlen*3);n += 3) {
2710                                 memset(co, 0, sizeof(co));      // initialize color sum
2711                                 npix = 0;                                       // initialize number of pixels in scale rectangle
2712
2713                                 // sum all pixels in scale rectangle
2714                                 for (int sc_line=0; sc_line<scan_lines; sc_line++) {
2715                                         // Get colors position in scale buffer
2716                                         img_pix_pos = (rd_len * sc_line) + (n * scale_pix);
2717
2718                                         for (int sc_col=0; sc_col<scale_pix; sc_col++) {
2719                                                 co[0] += scale_buf[img_pix_pos];
2720                                                 co[1] += scale_buf[img_pix_pos + 1];
2721                                                 co[2] += scale_buf[img_pix_pos + 2];
2722                                                 npix++;
2723                                         }
2724                                 }
2725                                 // Place the average in display buffer, convert BGR-888 (BMP) -> RGB-888 (DISPLAY)
2726                                 line_buf[lb_idx][n+2] = (uint8_t)(co[0] / npix);        // B
2727                                 line_buf[lb_idx][n+1] = (uint8_t)(co[1] / npix);        // G
2728                                 line_buf[lb_idx][n] = (uint8_t)(co[2] / npix);          // R
2729                         }
2730                 }
2731
2732                 wait_trans_finish(1);
2733                 send_data(disp_xstart, disp_yend, disp_xend, disp_yend, img_xlen, (color_t *)line_buf[lb_idx]);
2734                 lb_idx = (lb_idx + 1) & 1;  // change buffer
2735
2736                 disp_yend--;
2737         }
2738         err = 0;
2739 exit1:
2740         disp_deselect();
2741 exit:
2742         if (scale_buf) free(scale_buf);
2743         if (line_buf[0]) free(line_buf[0]);
2744         if (line_buf[1]) free(line_buf[1]);
2745         if (fhndl) fclose(fhndl);
2746         if ((err) && (tft_image_debug)) printf("Error: %d [%s]\r\n", err, err_buf);
2747
2748         return err;
2749 }
2750
2751
2752 // ============= Touch panel functions =========================================
2753
2754 #if USE_TOUCH == TOUCH_TYPE_XPT2046
2755 //-------------------------------------------------------
2756 static int tp_get_data_xpt2046(uint8_t type, int samples)
2757 {
2758         if (tft_ts_spi == NULL) return 0;
2759
2760         int n, result, val = 0;
2761         uint32_t i = 0;
2762         uint32_t vbuf[18];
2763         uint32_t minval, maxval, dif;
2764
2765     if (samples < 3) samples = 1;
2766     if (samples > 18) samples = 18;
2767
2768     // one dummy read
2769     result = touch_get_data(type);
2770
2771     // read data
2772         while (i < 10) {
2773         minval = 5000;
2774         maxval = 0;
2775                 // get values
2776                 for (n=0;n<samples;n++) {
2777                     result = touch_get_data(type);
2778                         if (result < 0) break;
2779
2780                         vbuf[n] = result;
2781                         if (result < minval) minval = result;
2782                         if (result > maxval) maxval = result;
2783                 }
2784                 if (result < 0) break;
2785                 dif = maxval - minval;
2786                 if (dif < 40) break;
2787                 i++;
2788     }
2789         if (result < 0) return -1;
2790
2791         if (samples > 2) {
2792                 // remove one min value
2793                 for (n = 0; n < samples; n++) {
2794                         if (vbuf[n] == minval) {
2795                                 vbuf[n] = 5000;
2796                                 break;
2797                         }
2798                 }
2799                 // remove one max value
2800                 for (n = 0; n < samples; n++) {
2801                         if (vbuf[n] == maxval) {
2802                                 vbuf[n] = 5000;
2803                                 break;
2804                         }
2805                 }
2806                 for (n = 0; n < samples; n++) {
2807                         if (vbuf[n] < 5000) val += vbuf[n];
2808                 }
2809                 val /= (samples-2);
2810         }
2811         else val = vbuf[0];
2812
2813     return val;
2814 }
2815
2816 //-----------------------------------------------
2817 static int TFT_read_touch_xpt2046(int *x, int* y)
2818 {
2819         int res = 0, result = -1;
2820         if (spi_lobo_device_select(tft_ts_spi, 0) != ESP_OK) return 0;
2821
2822     result = tp_get_data_xpt2046(0xB0, 3);  // Z; pressure; touch detect
2823         if (result <= 50) goto exit;
2824
2825         // touch panel pressed
2826         result = tp_get_data_xpt2046(0xD0, 10);
2827         if (result < 0)  goto exit;
2828
2829         *x = result;
2830
2831         result = tp_get_data_xpt2046(0x90, 10);
2832         if (result < 0)  goto exit;
2833
2834         *y = result;
2835         res = 1;
2836 exit:
2837         spi_lobo_device_deselect(tft_ts_spi);
2838         return res;
2839 }
2840 #endif
2841
2842 //=============================================
2843 int TFT_read_touch(int *x, int* y, uint8_t raw)
2844 {
2845     *x = 0;
2846     *y = 0;
2847         if (tft_ts_spi == NULL) return 0;
2848     #if USE_TOUCH == TOUCH_TYPE_NONE
2849         return 0;
2850     #else
2851         int result = -1;
2852     int X=0, Y=0;
2853
2854     #if USE_TOUCH == TOUCH_TYPE_XPT2046
2855     uint32_t tft_tp_calx = TP_CALX_XPT2046;
2856     uint32_t tft_tp_caly = TP_CALY_XPT2046;
2857         result = TFT_read_touch_xpt2046(&X, &Y);
2858         if (result == 0) return 0;
2859     #elif USE_TOUCH == TOUCH_TYPE_STMPE610
2860     uint32_t tft_tp_calx = TP_CALX_STMPE610;
2861     uint32_t tft_tp_caly = TP_CALY_STMPE610;
2862     uint16_t Xx, Yy, Z=0;
2863     result = stmpe610_get_touch(&Xx, &Yy, &Z);
2864     if (result == 0) return 0;
2865     X = Xx;
2866     Y = Yy;
2867     #else
2868     return 0;
2869     #endif
2870
2871     if (raw) {
2872         *x = X;
2873         *y = Y;
2874         return 1;
2875     }
2876
2877     // Calibrate the result
2878         int tmp;
2879         int xleft   = (tft_tp_calx >> 16) & 0x3FFF;
2880         int xright  = tft_tp_calx & 0x3FFF;
2881         int ytop    = (tft_tp_caly >> 16) & 0x3FFF;
2882         int ybottom = tft_tp_caly & 0x3FFF;
2883
2884         if (((xright - xleft) <= 0) || ((ybottom - ytop) <= 0)) return 0;
2885
2886     #if USE_TOUCH == TOUCH_TYPE_XPT2046
2887         int width = tft_width;
2888         int height = tft_height;
2889         X = ((X - xleft) * height) / (xright - xleft);
2890         Y = ((Y - ytop) * width) / (ybottom - ytop);
2891
2892         if (X < 0) X = 0;
2893         if (X > height-1) X = height-1;
2894         if (Y < 0) Y = 0;
2895         if (Y > width-1) Y = width-1;
2896
2897         switch (tft_orientation) {
2898             case PORTRAIT:
2899                 tmp = X;
2900                 X = width - Y - 1;
2901                 Y = tmp;
2902                 break;
2903             case PORTRAIT_FLIP:
2904                 tmp = X;
2905                 X = Y;
2906                 Y = height - tmp - 1;
2907                 break;
2908             case LANDSCAPE_FLIP:
2909                 X = height - X - 1;
2910                 Y = width - Y - 1;
2911                 break;
2912         }
2913     #elif USE_TOUCH == TOUCH_TYPE_STMPE610
2914         int width = tft_width;
2915         int height = tft_height;
2916         if (tft_width > tft_height) {
2917             width = tft_height;
2918             height = tft_width;
2919         }
2920                 X = ((X - xleft) * width) / (xright - xleft);
2921                 Y = ((Y - ytop) * height) / (ybottom - ytop);
2922
2923                 if (X < 0) X = 0;
2924                 if (X > width-1) X = width-1;
2925                 if (Y < 0) Y = 0;
2926                 if (Y > height-1) Y = height-1;
2927
2928                 switch (tft_orientation) {
2929                         case PORTRAIT_FLIP:
2930                                 X = width - X - 1;
2931                                 Y = height - Y - 1;
2932                                 break;
2933                         case LANDSCAPE:
2934                                 tmp = X;
2935                                 X = Y;
2936                                 Y = width - tmp -1;
2937                                 break;
2938                         case LANDSCAPE_FLIP:
2939                                 tmp = X;
2940                                 X = height - Y -1;
2941                                 Y = tmp;
2942                                 break;
2943                 }
2944     #endif
2945         *x = X;
2946         *y = Y;
2947         return 1;
2948     #endif
2949 }
2950