#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include "debug.h"

const int PINledAlive =  						LED_BUILTIN;
#define LEDBLINK_PERIOD             2000UL                                // 2000 ms
#define LEDFLICK_ON                 50UL                                  // 50 ms
#define LEDON_DIVIDER               10UL                                  // divisor of period
#define LEDBLINK_TIMEOUT            LEDBLINK_PERIOD/LEDON_DIVIDER         //  x-x-.....x-x-.....
#define LED_OFFSTATE 								LOW

#define SCREEN_WIDTH 								128 // OLED display width, in pixels
#define SCREEN_HEIGHT 							64 // OLED display height, in pixels

#define OLED_DRIVER_ADAFRUIT        true
#define OLED_DRIVER_U8G2LIB         !OLED_DRIVER_ADAFRUIT
#define OLED_ADAFRUIT_USE_FONTS     false

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define  OLED_SCROLL_SIZE 	128
const char ScrollFields[] = "Temp:%.1fC, Humd:%.1f%%, Pres:%dmmHg, eCO2:%dppm, TVOC:%dppb, Rate:%.1f%%";

uint32_t lcdOled_fps; // delay for each individual animation frame (same for all the frames inside one animation)
uint32_t lcdOled_tick;
uint32_t lcdOled_FPSms;
uint32_t lcdOled_flipCounter;
bool		 lcdOled_flip;

int 		 x, minX;

typedef struct RTC_t 
  {
		uint32_t	tm_unix_since1970;		
		int32_t		tm_timeZoneOffset;				
    uint8_t   tm_hour;
    uint8_t   tm_min;
    uint8_t   tm_sec; 
    uint32_t  tm_ticks;
	} CLOCK_RTC_t;

#define TIMEZONE_VAL					3		
#define  OLED_CLOCK_XOFFSET 	0 // x-pixels offest
#define  OLED_CLOCK_YOFFSET 	32 // y-pixels offest


typedef enum TRAIN_NO_t {
		TRAIN_NO_1 = 0, 
		TRAIN_NO_2, 
		TRAIN_NO_3, 
		TRAIN_NO_4, 
		TRAIN_NO_NULL, 
		// leave this last entry
		TRAIN_NO_N
	};

typedef enum TRAIN_ST_t {
  TS_NONE = 0,
  TS_DEPARTURE,
  TS_ARRIVAL,
  TS_DELAY,
   // leave this last entry
  TS_N};

typedef struct TRAIN_INFO
  {
		const char*	ti_TrainNo;
		const char*	ti_Time;
		TRAIN_ST_t ti_State;
		const char*	ti_Dest;
		const char*	ti_Orig;
	} TRAIN_INFO_t;


typedef struct TRAIN_TABLE
  {
		TRAIN_INFO_t trTable;
		const char* trState;
		uint8_t	trIndex;
		uint32_t trTimeout;
	} TRAIN_TABLE_t;

typedef struct {
   char description [12];
} description_t;

#define TVAR_R(TRNO, TIME, STATE, DEST, ORIG)   { XSTRTOK(R,TRNO),   XSTR(TIME), (TRAIN_ST_t)STATE, XSTR(DEST), XSTR(ORIG) }
#define TVAR_RE(TRNO, TIME, STATE, DEST, ORIG)  { XSTRTOK(R,TRNO),   XSTR(TIME), (TRAIN_ST_t)STATE, XSTR(DEST), XSTR(ORIG) }
#define TVAR_IR(TRNO, TIME, STATE, DEST, ORIG)  { XSTRTOK(IR,TRNO),  XSTR(TIME), (TRAIN_ST_t)STATE, XSTR(DEST), XSTR(ORIG) }
#define TVAR_IRN(TRNO, TIME, STATE, DEST, ORIG) { XSTRTOK(IRN,TRNO), XSTR(TIME), (TRAIN_ST_t)STATE, XSTR(DEST), XSTR(ORIG) }
#define TVAR_NULL()															{ NULL,              NULL,       (TRAIN_ST_t)TS_NONE,     NULL, NULL       }

TRAIN_TABLE_t*  	eleTrainTbl;
CLOCK_RTC_t   	 	TimeClock;

enum TEXT_ALIGN_t {
  NO_JST = 0,
  LEFT_JST = 1,
  RIGHT_JST = 2,
  CENTER_JST = 3,
  // leave this last entry
  TEXT_N };
	
typedef uint8_t 	mGFX_t;

static bool				OLED_I2C_DEBUG_SPRINT(Print &out, TEXT_ALIGN_t _strJ, mGFX_t _strX, mGFX_t _strY, const char* _fmt, ... );
static bool				OLED_I2C_DEBUG_SPRINT_NUMERIC(Print &out, const char* _fmt,  void* _val);
//#define 					OLED_I2C_PrintMsg(just,posX,posY,str, ...)  		OLED_I2C_DEBUG_SPRINT(oled, just, posX, posY, str, ##__VA_ARGS__ )

#define 					OLED_I2C_PrintMsg(just,posX,posY,fmt, ...) 			OLED_I2C_SetCursor(just, posX, posY, fmt); \
																																	_SerialPrintf(oled, 32, fmt, ##__VA_ARGS__)


/* ======================================= */
/* ======================================= */
#define TRAIN_01_INFO		TVAR_R(   0001, 01:00, TS_DEPARTURE, Destin1, BUCURESTI NORD )	
#define TRAIN_02_INFO		TVAR_RE( 00002, 02:00, TS_ARRIVAL,   Destin2, ORIGIN2 )	
#define TRAIN_03_INFO		TVAR_IR(  0003, 03:00, TS_DELAY,     Destin3, ORIGIN3 )	
#define TRAIN_04_INFO		TVAR_IRN(  004, 04:00, TS_NONE,      Destin4, ORIGIN4 )	
#define TRAIN_NO_NULL	TVAR_NULL()	
/* ======================================= */


// ----------------------------------------------------------------
/*
const TRAIN_INFO_t msg01 [] PROGMEM = TRAIN_01_INFO; 
const TRAIN_INFO_t msg02 [] PROGMEM = TRAIN_02_INFO; 
const TRAIN_INFO_t msg03 [] PROGMEM = TRAIN_03_INFO; 
const TRAIN_INFO_t msg04 [] PROGMEM = TRAIN_04_INFO; 
const TRAIN_INFO_t msg05 [] PROGMEM = TRAIN_NO_NULL;

const TRAIN_INFO_t * const TrainInfoTable[] PROGMEM = { msg01,
                            			        							msg02,
														 			        							msg03,
														 			        							msg04,														 
										   			 			        							msg05 };
*/

const TRAIN_INFO_t TrainInfoTable[][TRAIN_NO_N] PROGMEM =	{	TRAIN_01_INFO,
          														  										TRAIN_02_INFO,
          																									TRAIN_03_INFO,
        												  													TRAIN_04_INFO,
    								  					   													TRAIN_NO_NULL };


const char * TrainStatTable[]  = { " ",
                									 "PLeaca la",
																	 "Soseste de la",
																	 "Intarziere 45min.",														 
																	 "via" };

/* ======================================= */

TRAIN_TABLE_t* TrainTable_Init(void);
bool TrainTable_Update(TRAIN_TABLE_t* ttp);
void TrainTable_Animation(TRAIN_TABLE_t* ttp);
void TrainTable_ShowData(TRAIN_TABLE_t* ttp, bool flip);
void TrainTable_ShowDataScroll(TRAIN_TABLE_t* ttp, bool flip);


static void 	RTC_InitClock(CLOCK_RTC_t* rtc, const char* date, const char* time);
static void 	RTC_ShowClock(CLOCK_RTC_t* rtc, uint8_t xofffset, uint8_t yofffset);

static void 	Oled_DrawHdoted_line(uint8_t x, uint8_t y, uint8_t len, uint8_t dotlen);
static void 	Oled_DrawVdoted_line(uint8_t x, uint8_t y, uint8_t len, uint8_t dotlen);
static void   Oled_DrawFramedoted_line(uint8_t x, uint8_t y, uint8_t hlen, uint8_t vlen, uint8_t dotlen);

static void 	OLED_I2C_Refresh(bool refresh);
static void 	OLED_I2C_Clear(void);
static mGFX_t OLED_I2C_GetGfxStrWidth(/* const String &buf */ const char* buf);
static mGFX_t OLED_I2C_GetGfxFontHeight(void);
static void 	OLED_I2C_DrawCentreString(/* const String &buf */  const char* buf , mGFX_t x, mGFX_t y);
static void		OLED_I2C_SetCursor(TEXT_ALIGN_t _strJ, mGFX_t _strX, mGFX_t _strY, const char* _fmt);
//static bool		OLED_I2C_DEBUG_SPRINT(Print &out, TEXT_ALIGN_t _strJ, mGFX_t _strX, mGFX_t _strY, const char*  _fmt, ... );

void LedAlive_Init(void);
void LedAlive_Blink(int ledport, uint32_t ledtimeout);


/* ================================================================
   =                                                              =
	 ================================================================ */

void dumpTable(void)
{
	/*
	TRAIN_INFO_t* tit = (TRAIN_INFO_t *) malloc(sizeof(TRAIN_INFO_t)); // allocate initialized with 0 space for new table
	
	SerialPrintf("\n");
  SerialPrintf("[TrainStatTable] > %s %d\n", "Elements:", BUFFLEN(TrainStatTable));
  for (uint8_t i=0; i < BUFFLEN(TrainStatTable); i++) { 
    SerialPrintf("[TrainStatTable][%d] @0x%x> \t%s\n" , i, &TrainStatTable[i], TrainStatTable[i]);  
	}
  SerialPrintf("\n");
  SerialPrintf("[TrainInfoTable] > %s %d\n", "Elements:", BUFFLEN(TrainInfoTable));
  for (uint8_t i=0; i < BUFFLEN(TrainInfoTable); i++) { 
		memcpy_P (tit, (TRAIN_INFO_t *)&TrainInfoTable[i], sizeof(TRAIN_INFO_t));
    SerialPrintf("[TrainInfoTable][%d] @0x%x> \t%s\n" , i, &TrainInfoTable[i], TrainInfoTable[i]);  
		//SerialPrintf("[TrainInfoTable][%d] @0x%x> \t%s\n" , i, &tit->ti_TrainNo, tit->ti_TrainNo); 
	}
	free(tit);
	tit = NULL;
	*/
}

/*
		const char*	ti_TrainNo;
		const char*	ti_Time;
		TRAIN_STAT_t ti_State;
		const char*	ti_Dest;
		const char*	ti_Orig;
*/

/* ================================================================
   =                                                              =
	 ================================================================ */
void setup()
{
  Serial.begin(115200);
	//Wire1.begin();
  
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  // Clear the buffer
  oled.clearDisplay();

  oled.setTextSize(2);
  oled.setTextColor(WHITE);
  oled.setTextWrap(false);

	lcdOled_fps = 10;
	lcdOled_tick = lcdOled_fps; 
	lcdOled_flipCounter = 0;
	lcdOled_flip = !lcdOled_flip;
  x = oled.width();	
  lcdOled_FPSms = millis();

	pinMode(A1, INPUT);   								// randon seed A1 noise
  randomSeed(analogRead(A1));

	SerialPrintf("Free RAM: %d\n", freeRAM());
	//dumpPGM(0x0060UL, 0x3800);

	//dumpRAM((uint16_t)&__data_start ,dataBssCount());
	//dumpRAM((uint16_t)&__data_start ,0x800);

	eleTrainTbl = TrainTable_Init();
	RTC_InitClock(&TimeClock, (const char*)&compilation_date, (const char*)&compilation_time);
	LedAlive_Init();
	
	//dumpPGM(0x0060UL, 0x3800);
	SerialPrintf("Free RAM: %d\n", freeRAM());
	//dumpRAM((uint16_t)&__data_start ,dataBssCount());
	dumpRAM((uint16_t)&__data_start ,0x800);
	//dumpTable();
}

/* ================================================================
   =                                                              =
	 ================================================================ */
void loop() {
	TrainTable_Update(eleTrainTbl);
  TrainTable_Animation(eleTrainTbl);
	LedAlive_Blink(PINledAlive, LEDBLINK_TIMEOUT);
}

/* ================================================================
   =                                                              =
	 ================================================================ */
TRAIN_TABLE_t* TrainTable_Init(void)
{
	//TRAIN_TABLE_t* temp = (TRAIN_TABLE_t *) malloc(sizeof(TRAIN_TABLE_t)); 	// allocate space for new menu node
	TRAIN_TABLE_t* temp = (TRAIN_TABLE_t *) calloc(1 ,sizeof(TRAIN_TABLE_t)); // allocate initialized with 0 space for new table
	
	if (NULL != temp) {
		temp->trIndex = random(0, BUFFLEN(TrainInfoTable));
		temp->trTimeout = millis();
		memcpy_P ((TRAIN_INFO_t *)&temp->trTable, (TRAIN_INFO_t *)&TrainInfoTable[temp->trIndex], sizeof(TRAIN_INFO_t));
  	temp->trState = TrainStatTable[temp->trTable.ti_State];

		/*
		temp->trTimeout = millis();
		//memcpy_P ((TRAIN_INFO_t *)&temp->trTable, (TRAIN_INFO_t *)&TrainInfoTable[temp->trIndex], sizeof(TRAIN_INFO_t));
		memcpy_P ((TRAIN_INFO_t *)&temp->trTable, &TrainInfoTable[temp->trIndex], sizeof(TRAIN_INFO_t));
		temp->trStat = TrainStatTable[temp->trTable.ti_State];
		*/
		//for(uint8_t i = 0; i < BUFFLEN(TrainInfoTable); i++) {
		//	memcpy_P ((TRAIN_INFO_t *)&temp->trTable[i], TrainInfoTable[i], sizeof(TRAIN_INFO_t));
		//	memcpy_P ((TRAIN_INFO_t *)&temp->trTable[i], pgm_read_dword(&TrTablePGM[i]), sizeof(TRAIN_INFO_t));
		//}
	}
	return temp;
}

/* ================================================================
   =                                                              =
	 ================================================================ */
bool TrainTable_Update(TRAIN_TABLE_t* ttp)
{
	bool retVal;

	retVal = false;
	if (millis() - ttp->trTimeout > 5000) {
		ttp->trTimeout = millis();
		ttp->trIndex = (ttp->trIndex + 1) % (BUFFLEN(TrainInfoTable));
		memcpy_P ((TRAIN_INFO_t *)&ttp->trTable, (TRAIN_INFO_t *)&TrainInfoTable[ttp->trIndex], sizeof(TRAIN_INFO_t));
		ttp->trState = TrainStatTable[ttp->trTable.ti_State];
		retVal = true;
	}
	return retVal;
}


/* ================================================================
   =                                                              =
	 ================================================================ */
void TrainTable_Animation(TRAIN_TABLE_t* ttp)
{	
  if ((millis()-lcdOled_FPSms) > (1000 / lcdOled_fps))		
	{
		lcdOled_FPSms = millis();
		OLED_I2C_Clear();	
		//oled.setFont();
		
		Oled_DrawFramedoted_line(0, 0, 							 oled.width()-1, oled.height()-1, 8);
		Oled_DrawFramedoted_line(0, oled.height()/2, oled.width()-1, oled.height()-1, 8);

    RTC_ShowClock(&TimeClock, OLED_CLOCK_XOFFSET, OLED_CLOCK_YOFFSET);

		TrainTable_ShowData(ttp, lcdOled_flip);
			//TrainTable_ShowDataScroll(ttp, lcdOled_flip);
		OLED_I2C_Refresh(true);
	}
}

/* ================================================================
   =                                                              =
	 ================================================================ */
void TrainTable_ShowData(TRAIN_TABLE_t* ttp, bool flip)
{
  oled.setTextSize(1);
	oled.setFont();

	OLED_I2C_PrintMsg(NO_JST,  0, 8, "%s", ttp->trTable.ti_Time);
	OLED_I2C_PrintMsg(NO_JST,  0, 18, "%s", ttp->trTable.ti_TrainNo);
	OLED_I2C_PrintMsg(NO_JST, 44, 18, "%s", ttp->trTable.ti_Dest);
	OLED_I2C_PrintMsg(NO_JST, 44, 31, "%s", ttp->trTable.ti_Orig);
	OLED_I2C_PrintMsg(NO_JST, 44, 8,  "%s", ttp->trState);	

	//OLED_I2C_PrintMsg(NO_JST, 44, 8, "%s", "Depart to");
	//OLED_I2C_PrintMsg(NO_JST, 44, 8, "%s", ttp->trState);	
	//OLED_I2C_PrintMsg(NO_JST, 44, 8, "%d", ttp->trTable.ti_State);
	//OLED_I2C_PrintMsg(NO_JST, 44, 8, "%s", TrainStatTable[ttp->trTable.ti_State]);

	//if(!flip) {
	//	OLED_I2C_PrintMsg(NO_JST, 38, 9, "Train: %i", ttp->msgno);
	//}
}

/* ================================================================
   =                                                              =
	 ================================================================ */
void TrainTable_ShowDataScroll(TRAIN_TABLE_t* ttp, bool flip)
{

}

/* ================================================================
   =                                                              =
	 ================================================================ */
static void RTC_InitClock(CLOCK_RTC_t* rtc, const char* date, const char* time)
{
	char buff[16];

	rtc->tm_timeZoneOffset = TIMEZONE_VAL;
	memcpy(buff, time, 8);
	 				SerialPrintf("[RTC] time: 	%s\n", buff);		// HH:mm:ss
  rtc->tm_hour = conv2d(buff);
	if (rtc->tm_timeZoneOffset < 0) {
		if(rtc->tm_hour < (uint8_t)abs(rtc->tm_timeZoneOffset)) {
			rtc->tm_hour = 24 - (uint8_t)abs(rtc->tm_timeZoneOffset);
		} else {
			rtc->tm_hour -= (uint8_t)abs(rtc->tm_timeZoneOffset);
		}
	} else {
		if((rtc->tm_hour + (uint8_t)abs(rtc->tm_timeZoneOffset)) > 23) {
			rtc->tm_hour += (uint8_t)abs(rtc->tm_timeZoneOffset);
			rtc->tm_hour -= 24;  
		} else {
			rtc->tm_hour += TIMEZONE_VAL;
		}
	}
  rtc->tm_min  = conv2d(buff + 3);
  rtc->tm_sec  = conv2d(buff + 6);
	rtc->tm_ticks = millis();
}

/* ================================================================
   =                                                              =
	 ================================================================ */
static void RTC_ShowClock(CLOCK_RTC_t* rtc, uint8_t xofffset, uint8_t yofffset)
{
	float angle;
	
  if (millis() - rtc->tm_ticks > 1000) {
    rtc->tm_ticks = millis();
	  if (++rtc->tm_sec > 59) {
		  rtc->tm_sec = 0;
		  if (++rtc->tm_min > 59) {
			  rtc->tm_min = 0;
			  if (++rtc->tm_hour > 23) {
				  rtc->tm_hour = 0;
			  }
		  }
	  }
  }
	
	if (xofffset > (oled.width() - 20)) xofffset = 0;
	if (yofffset > (oled.height() - 20)) yofffset = 0;
	
	// draw clock ticks
	for(int z=0; z<360;z=z+30)
	{
		angle = (float)z / 57.3; //Convert degrees to radians
		int x1=(16+(sin(angle)*15));
		int y1=(15-(cos(angle)*15));
		int x2=(16+(sin(angle)*(12)));
		int y2=(15-(cos(angle)*(12)));
		oled.drawLine(xofffset + x1, yofffset + y1, xofffset + x2, yofffset + y2, WHITE);
	}
	
	// draw clock hour
	angle=((float)rtc->tm_hour * 30 + (float)rtc->tm_min / 2) / 57.3 ; //Convert degrees to radians
	int x2=(16+(sin(angle)*(9)));
	int y2=(15-(cos(angle)*(9)));
	oled.drawLine(xofffset + 16, yofffset + 15, xofffset + x2, yofffset + y2, WHITE);

	// draw clock minute
	angle=((float)rtc->tm_min * 6 / 57.3) ; //Convert degrees to radians
	x2=(16+(sin(angle)*(11)));
	y2=(15-(cos(angle)*(11)));
	oled.drawLine(xofffset + 16, yofffset + 15, xofffset + x2, yofffset + y2, WHITE);

	// draw clock second
	angle=((float)rtc->tm_sec * 6 / 57.3) ; //Convert degrees to radians
	x2=(16+(sin(angle)*(13)));
	y2=(15-(cos(angle)*(13)));
	oled.drawLine(xofffset + 16, yofffset + 15, xofffset + x2, yofffset + y2, WHITE);
}

/* ================================================================
   =                                                              =
	 ================================================================ */
static void Oled_DrawHdoted_line(uint8_t x, uint8_t y, uint8_t len, uint8_t dotlen)
{
  if (len > oled.width()) len = oled.width();
  if (dotlen > len/4) dotlen = len/4;
	
  for (uint8_t i = x; i < len; i++) {
    //oled.drawFastHLine(x, y, len, WHITE);
    if (0 == (i % dotlen)) {
      oled.drawPixel(i, y, WHITE);
    }
  }
}

/* ================================================================
   =                                                              =
	 ================================================================ */
static void Oled_DrawVdoted_line(uint8_t x, uint8_t y, uint8_t len, uint8_t dotlen)
{
  if (len > oled.height()) len = oled.height();
 	if (dotlen > len/4) dotlen = len/4;

  for (uint8_t i = y; i < len; i++) {
    //oled.drawFastVLine(x, y, len, WHITE);
    if (0 == (i % dotlen)) {
      oled.drawPixel(x, i, WHITE);
    }
  }
}

/* ================================================================
   =                                                              =
	 ================================================================ */
static void Oled_DrawFramedoted_line(uint8_t x, uint8_t y, uint8_t hlen, uint8_t vlen, uint8_t dotlen)
{
  Oled_DrawHdoted_line(x,  y,  		  hlen,  dotlen);
  Oled_DrawHdoted_line(x,  y+vlen,   hlen,  dotlen);
  Oled_DrawVdoted_line(x,  		  y,  vlen,  dotlen);
  Oled_DrawVdoted_line(x + hlen, y,  vlen,  dotlen); 
}

 /**
 * =================================================================
 * =  D I S P L A Y - O L E D - R A M - B U F F E R - T O - O L E D = 
 * =================================================================
 */
static void OLED_I2C_Refresh(bool refresh)
{
	if (!refresh) return;
	#if (OLED_DRIVER_ADAFRUIT) 	
		oled.display();
	#endif

	#if (OLED_DRIVER_U8G2LIB) 
		oled.sendBuffer();
	#endif
}

/**
 * =================================================================
 * =  D I S P L A Y - O L E D - C L E A R  - R A M - B U F F E R   = 
 * =================================================================
 */
static void OLED_I2C_Clear(void)
{
	#if (OLED_DRIVER_ADAFRUIT) 	
		oled.clearDisplay();
	#endif

	#if (OLED_DRIVER_U8G2LIB) 
		oled.clearBuffer();
	#endif
}

/**
 * =================================================================
 * =  D I S P L A Y - O L E D - G E T - G F X - S T R I N G - L E N= 
 * =================================================================
 */
static mGFX_t OLED_I2C_GetGfxStrWidth(/* const String &buf */ const char* buf)
{
	#if (OLED_DRIVER_ADAFRUIT)
		int16_t x1, y1;
		uint16_t w, h;
		oled.getTextBounds("O", (SCREEN_WIDTH/2), (SCREEN_HEIGHT/2), &x1, &y1, &w, &h); //calc width of char
		#if(OLED_ADAFRUIT_USE_FONTS)
			w += 1;															// add additional 1 pixel vertical spacing
		#endif
		w *= strlen(buf);									// calculate number of chars by font pixel width on first char 
		if (w > SCREEN_WIDTH) w = SCREEN_WIDTH;
		return (mGFX_t)w;
	#endif
	#if (OLED_DRIVER_U8G2LIB)
		return (mGFX_t)oled.getStrWidth(buf);
	#endif
}

/**
 * =================================================================
 * =  D I S P L A Y - O L E D - G E T - G F X - F O N T - H E I G H T = 
 * =================================================================
 */
static mGFX_t OLED_I2C_GetGfxFontHeight(void)
{
	#if (OLED_DRIVER_ADAFRUIT)
		int16_t x1, y1;
		uint16_t w, h;
		oled.getTextBounds("A", (SCREEN_WIDTH/2), (SCREEN_HEIGHT/2), &x1, &y1, &w, &h); //calc width of char
		return (mGFX_t)h;
	#endif
	#if (OLED_DRIVER_U8G2LIB)
		return (mGFX_t)oled.getMaxCharHeight() - 5;	// "A"
	#endif
}

/**
 * =================================================================
 * =  D I S P L A Y - O L E D - D R A W - A T - C E N T E R 		  = 
 * =================================================================
 */
static void OLED_I2C_DrawCentreString(/* const String &buf */  const char* buf , mGFX_t x, mGFX_t y)
{
	#if (OLED_DRIVER_ADAFRUIT)
    int16_t x1, y1;
    uint16_t w, h;
    oled.getTextBounds(buf, x, y, &x1, &y1, &w, &h); //calc width of new string
    oled.setCursor(x - w / 2, y);
    oled.print(buf);
	#endif
}

 /**
 * =================================================================
 * =  D I S P L A Y - O L E D - S E T - T E X T - C U R S O R      =  
 * =================================================================
 */
static void		OLED_I2C_SetCursor(TEXT_ALIGN_t _strJ, mGFX_t _strX, mGFX_t _strY, const char* _fmt)
{
	#if((OLED_DRIVER_ADAFRUIT) && (!OLED_ADAFRUIT_USE_FONTS))
		_strY -= OLED_I2C_GetGfxFontHeight();												// Y offset correction for Adfruit standard fonts
	#endif
	switch (_strJ) {
		case LEFT_JST: _strX = 0; break;
		case CENTER_JST: _strX = (SCREEN_WIDTH - OLED_I2C_GetGfxStrWidth(_fmt)) >> 1;	break;
		case RIGHT_JST: _strX  = (SCREEN_WIDTH - OLED_I2C_GetGfxStrWidth(_fmt));		 	break;
		case NO_JST:
		default: break;
	}
	oled.setCursor(_strX, _strY);
}

 /**
 * =================================================================
 * =  D I S P L A Y - O L E D - P R I N T - F O R M A T E D - S T R = 
 * =================================================================
 */ 
static bool	OLED_I2C_DEBUG_SPRINT(Print &out, TEXT_ALIGN_t _strJ, mGFX_t _strX, mGFX_t _strY, const char*  _fmt, ... )
{	
	bool retBit = true;
	
	OLED_I2C_SetCursor(_strJ, _strX, _strY, _fmt);
	
	va_list argv;
	va_start(argv, _fmt);
	
	for (int i = 0; _fmt[i] != '\0'; i++) {
		if (_fmt[i] == '%') {
			// Look for specification of number of decimal places
			int places = 2;
			if (_fmt[i+1] == '.') i++;     //allows %.4f precision like in stdio printf (%4f will still work).
			if (_fmt[i+1] >= '0' && _fmt[i+1] <= '9') {
				places = _fmt[i+1] - '0';
				i++;
			}
			switch (_fmt[++i]) {
				case 'B': out.print(F("0b")); // Fall through intended
				case 'b': out.print(va_arg(argv, int), BIN); break;
				case 'c': out.print((char) va_arg(argv, int)); break;
				case 'd': case 'i': out.print(va_arg(argv, int), DEC); break;
				case 'f': out.print(va_arg(argv, double), places); break;
				case 'l': out.print(va_arg(argv, long), DEC); break;
				case 'o': out.print(va_arg(argv, int) == 0 ? F("Off") : F("On")); break;
				case 's': out.print(va_arg(argv, const char*)); break;
				case 'S': out.print(va_arg(argv, const String)); break;
				case 'X': out.print(F("0x")); // Fall through intended
				case 'x': out.print(va_arg(argv, int), HEX); break;
				case '%': out.print(_fmt[i]); break;
				case 'p':
				case 'q':	out.print(F("[#]"));
				case '0':
				case 	0 :	retBit = !retBit; break;
				default: out.print(F("?")); break;
			}
		} else out.print(_fmt[i]);
	}
	va_end(argv);
	return retBit;
}


/* ================================================================
   =                                                              =
	 ================================================================ */
void LedAlive_Init(void) 
{
	pinMode(PINledAlive, OUTPUT);   // Onboard LED;
  digitalWrite(PINledAlive, LED_OFFSTATE);           // led off
}
		
/* ================================================================
   =                                                              =
	 ================================================================ */
void LedAlive_Blink(int ledport, uint32_t ledtimeout)
{
  if (ledtimeout == 0) return;
  
  static uint32_t ledPeriodTimeout = millis();
  static uint32_t ledOnTimeout = millis();
  static uint32_t ledCount = 0;
  
  if ((millis()-ledPeriodTimeout) > ledtimeout) {
    ledPeriodTimeout += ledtimeout;                           // reset Period counter  
    if( ++ledCount > LEDON_DIVIDER-1) {
      ledCount = 0;               // reset ON counter
      digitalWrite(ledport, LED_OFFSTATE);                             // led Off
    }
  }
  if (ledCount > LEDON_DIVIDER-2){
    if ((millis() - ledOnTimeout) > LEDFLICK_ON) {
      digitalWrite(ledport, !digitalRead(ledport));           // led toggle
      ledOnTimeout += LEDFLICK_ON;                            // reset ON counter      
    }
  }
}