/*
 *   Copyright (C) 2016,2017,2018,2020 by Jonathan Naylor G4KLX
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include "OLED.h"
#include "Log.h"
#include "NetworkInfo.h"
#include "Thread.h"

#include <cstring>
#include <cassert>

const uint16_t BLACK = 0U;
const uint16_t WHITE = 1U;

const uint16_t OLED_STATUSBAR = 0U;
const uint16_t OLED_LINE1 = 8U; //16
const uint16_t OLED_LINE2 = 18U; //26 
const uint16_t OLED_LINE3 = 28U; //36
const uint16_t OLED_LINE4 = 37U; //46
const uint16_t OLED_LINE5 = 47U; //56
const uint16_t OLED_LINE6 = 57U;

const uint8_t SSD_Command_Mode = 0x00U;  /* C0 and DC bit are 0         */
const uint8_t SSD_Data_Mode    = 0x40U;  /* C0 bit is 0 and DC bit is 1 */

const uint8_t SSD_Set_Segment_Remap  = 0xA0U;
const uint8_t SSD_Inverse_Display    = 0xA7U;
const uint8_t SSD_Set_Muliplex_Ratio = 0xA8U;

const uint8_t SSD_Display_Off = 0xAEU;
const uint8_t SSD_Display_On  = 0xAFU;

const uint8_t SSD_Set_ContrastLevel = 0x81U;

const uint8_t SSD_External_Vcc = 0x01U;
const uint8_t SSD_Internal_Vcc = 0x02U;

const uint8_t SSD_Set_Column_Address = 0x21U;
const uint8_t SSD_Set_Page_Address   = 0x22U;

const uint8_t SSD_Activate_Scroll   = 0x2FU;
const uint8_t SSD_Deactivate_Scroll = 0x2EU;

const uint8_t SSD_Right_Horizontal_Scroll = 0x26U;
const uint8_t SSD_Left_Horizontal_Scroll  = 0x27U;

const uint8_t SSD1306_Entire_Display_Resume = 0xA4U;
const uint8_t SSD1306_Entire_Display_On     = 0xA5U;

const uint8_t SSD1306_Normal_Display = 0xA6U;

const uint8_t SSD1306_Set_Display_Offset                   = 0xD3U;
const uint8_t SSD1306_Set_Com_Pins                         = 0xDAU;
const uint8_t SSD1306_Set_Vcomh_Deselect_Level             = 0xDBU;
const uint8_t SSD1306_Set_Display_Clock_Div                = 0xD5U;
const uint8_t SSD1306_Set_Precharge_Period                 = 0xD9U;
const uint8_t SSD1306_Set_Lower_Column_Start_Address       = 0x00U;
const uint8_t SSD1306_Set_Higher_Column_Start_Address      = 0x10U;
const uint8_t SSD1306_Set_Start_Line                       = 0x40U;
const uint8_t SSD1306_Set_Memory_Mode                      = 0x20U;
const uint8_t SSD1306_Set_Com_Output_Scan_Direction_Normal = 0xC0U;
const uint8_t SSD1306_Set_Com_Output_Scan_Direction_Remap  = 0xC8U;
const uint8_t SSD1306_Charge_Pump_Setting                  = 0x8DU;

// Scrolling #defines
const uint8_t SSD1306_SET_VERTICAL_SCROLL_AREA             = 0xA3U;
const uint8_t SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL = 0x29U;
const uint8_t SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL  = 0x2AU;

const uint8_t SSD1308_Normal_Display = 0xA6U;

const uint8_t SSD1327_Set_Display_Start_Line = 0xA1U;
const uint8_t SSD1327_Set_Display_Offset     = 0xA2U;
const uint8_t SSD1327_Normal_Display         = 0xA4U;
const uint8_t SSD1327_Set_Display_Clock_Div  = 0xB3U;
const uint8_t SSD1327_Set_Command_Lock       = 0xFDU;
const uint8_t SSD1327_Set_Column_Address     = 0x15U;
const uint8_t SSD1327_Set_Row_Address        = 0x75U;

const uint8_t SH1106_Set_Page_Address = 0xB0U;

// Arduino Compatible Macro
#define _BV(bit) (1 << (bit))

const unsigned char OLED_ADAFRUIT_SPI_128x32 = 0U;
const unsigned char OLED_ADAFRUIT_SPI_128x64 = 1U;
const unsigned char OLED_ADAFRUIT_I2C_128x32 = 2U;
const unsigned char OLED_ADAFRUIT_I2C_128x64 = 3U;
const unsigned char OLED_SEEED_I2C_128x64    = 4U;
const unsigned char OLED_SEEED_I2C_96x96     = 5U;
const unsigned char OLED_SH1106_I2C_128x64   = 6U;

// standard ascii 5x7 font
const uint8_t FONT[] = {
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x3EU, 0x5BU, 0x4FU, 0x5BU, 0x3EU,
	0x3EU, 0x6BU, 0x4FU, 0x6BU, 0x3EU,
	0x1CU, 0x3EU, 0x7CU, 0x3EU, 0x1CU,
	0x18U, 0x3CU, 0x7EU, 0x3CU, 0x18U,
	0x1CU, 0x57U, 0x7DU, 0x57U, 0x1CU,
	0x1CU, 0x5EU, 0x7FU, 0x5EU, 0x1CU,
	0x00U, 0x18U, 0x3CU, 0x18U, 0x00U,
	0xFFU, 0xE7U, 0xC3U, 0xE7U, 0xFFU,
	0x00U, 0x18U, 0x24U, 0x18U, 0x00U,
	0xFFU, 0xE7U, 0xDBU, 0xE7U, 0xFFU,
	0x30U, 0x48U, 0x3AU, 0x06U, 0x0EU,
	0x26U, 0x29U, 0x79U, 0x29U, 0x26U,
	0x40U, 0x7FU, 0x05U, 0x05U, 0x07U,
	0x40U, 0x7FU, 0x05U, 0x25U, 0x3FU,
	0x5AU, 0x3CU, 0xE7U, 0x3CU, 0x5AU,
	0x7FU, 0x3EU, 0x1CU, 0x1CU, 0x08U,
	0x08U, 0x1CU, 0x1CU, 0x3EU, 0x7FU,
	0x14U, 0x22U, 0x7FU, 0x22U, 0x14U,
	0x5FU, 0x5FU, 0x00U, 0x5FU, 0x5FU,
	0x06U, 0x09U, 0x7FU, 0x01U, 0x7FU,
	0x00U, 0x66U, 0x89U, 0x95U, 0x6AU,
	0x60U, 0x60U, 0x60U, 0x60U, 0x60U,
	0x94U, 0xA2U, 0xFFU, 0xA2U, 0x94U,
	0x08U, 0x04U, 0x7EU, 0x04U, 0x08U,
	0x10U, 0x20U, 0x7EU, 0x20U, 0x10U,
	0x08U, 0x08U, 0x2AU, 0x1CU, 0x08U,
	0x08U, 0x1CU, 0x2AU, 0x08U, 0x08U,
	0x1EU, 0x10U, 0x10U, 0x10U, 0x10U,
	0x0CU, 0x1EU, 0x0CU, 0x1EU, 0x0CU,
	0x30U, 0x38U, 0x3EU, 0x38U, 0x30U,
	0x06U, 0x0EU, 0x3EU, 0x0EU, 0x06U,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0x00U, 0x5FU, 0x00U, 0x00U,
	0x00U, 0x07U, 0x00U, 0x07U, 0x00U,
	0x14U, 0x7FU, 0x14U, 0x7FU, 0x14U,
	0x24U, 0x2AU, 0x7FU, 0x2AU, 0x12U,
	0x23U, 0x13U, 0x08U, 0x64U, 0x62U,
	0x36U, 0x49U, 0x56U, 0x20U, 0x50U,
	0x00U, 0x08U, 0x07U, 0x03U, 0x00U,
	0x00U, 0x1CU, 0x22U, 0x41U, 0x00U,
	0x00U, 0x41U, 0x22U, 0x1CU, 0x00U,
	0x2AU, 0x1CU, 0x7FU, 0x1CU, 0x2AU,
	0x08U, 0x08U, 0x3EU, 0x08U, 0x08U,
	0x00U, 0x80U, 0x70U, 0x30U, 0x00U,
	0x08U, 0x08U, 0x08U, 0x08U, 0x08U,
	0x00U, 0x00U, 0x60U, 0x60U, 0x00U,
	0x20U, 0x10U, 0x08U, 0x04U, 0x02U,
	0x3EU, 0x51U, 0x49U, 0x45U, 0x3EU,
	0x00U, 0x42U, 0x7FU, 0x40U, 0x00U,
	0x72U, 0x49U, 0x49U, 0x49U, 0x46U,
	0x21U, 0x41U, 0x49U, 0x4DU, 0x33U,
	0x18U, 0x14U, 0x12U, 0x7FU, 0x10U,
	0x27U, 0x45U, 0x45U, 0x45U, 0x39U,
	0x3CU, 0x4AU, 0x49U, 0x49U, 0x31U,
	0x41U, 0x21U, 0x11U, 0x09U, 0x07U,
	0x36U, 0x49U, 0x49U, 0x49U, 0x36U,
	0x46U, 0x49U, 0x49U, 0x29U, 0x1EU,
	0x00U, 0x00U, 0x14U, 0x00U, 0x00U,
	0x00U, 0x40U, 0x34U, 0x00U, 0x00U,
	0x00U, 0x08U, 0x14U, 0x22U, 0x41U,
	0x14U, 0x14U, 0x14U, 0x14U, 0x14U,
	0x00U, 0x41U, 0x22U, 0x14U, 0x08U,
	0x02U, 0x01U, 0x59U, 0x09U, 0x06U,
	0x3EU, 0x41U, 0x5DU, 0x59U, 0x4EU,
	0x7CU, 0x12U, 0x11U, 0x12U, 0x7CU,
	0x7FU, 0x49U, 0x49U, 0x49U, 0x36U,
	0x3EU, 0x41U, 0x41U, 0x41U, 0x22U,
	0x7FU, 0x41U, 0x41U, 0x41U, 0x3EU,
	0x7FU, 0x49U, 0x49U, 0x49U, 0x41U,
	0x7FU, 0x09U, 0x09U, 0x09U, 0x01U,
	0x3EU, 0x41U, 0x41U, 0x51U, 0x73U,
	0x7FU, 0x08U, 0x08U, 0x08U, 0x7FU,
	0x00U, 0x41U, 0x7FU, 0x41U, 0x00U,
	0x20U, 0x40U, 0x41U, 0x3FU, 0x01U,
	0x7FU, 0x08U, 0x14U, 0x22U, 0x41U,
	0x7FU, 0x40U, 0x40U, 0x40U, 0x40U,
	0x7FU, 0x02U, 0x1CU, 0x02U, 0x7FU,
	0x7FU, 0x04U, 0x08U, 0x10U, 0x7FU,
	0x3EU, 0x41U, 0x41U, 0x41U, 0x3EU,
	0x7FU, 0x09U, 0x09U, 0x09U, 0x06U,
	0x3EU, 0x41U, 0x51U, 0x21U, 0x5EU,
	0x7FU, 0x09U, 0x19U, 0x29U, 0x46U,
	0x26U, 0x49U, 0x49U, 0x49U, 0x32U,
	0x03U, 0x01U, 0x7FU, 0x01U, 0x03U,
	0x3FU, 0x40U, 0x40U, 0x40U, 0x3FU,
	0x1FU, 0x20U, 0x40U, 0x20U, 0x1FU,
	0x3FU, 0x40U, 0x38U, 0x40U, 0x3FU,
	0x63U, 0x14U, 0x08U, 0x14U, 0x63U,
	0x03U, 0x04U, 0x78U, 0x04U, 0x03U,
	0x61U, 0x59U, 0x49U, 0x4DU, 0x43U,
	0x00U, 0x7FU, 0x41U, 0x41U, 0x41U,
	0x02U, 0x04U, 0x08U, 0x10U, 0x20U,
	0x00U, 0x41U, 0x41U, 0x41U, 0x7FU,
	0x04U, 0x02U, 0x01U, 0x02U, 0x04U,
	0x40U, 0x40U, 0x40U, 0x40U, 0x40U,
	0x00U, 0x03U, 0x07U, 0x08U, 0x00U,
	0x20U, 0x54U, 0x54U, 0x78U, 0x40U,
	0x7FU, 0x28U, 0x44U, 0x44U, 0x38U,
	0x38U, 0x44U, 0x44U, 0x44U, 0x28U,
	0x38U, 0x44U, 0x44U, 0x28U, 0x7FU,
	0x38U, 0x54U, 0x54U, 0x54U, 0x18U,
	0x00U, 0x08U, 0x7EU, 0x09U, 0x02U,
	0x18U, 0xA4U, 0xA4U, 0x9CU, 0x78U,
	0x7FU, 0x08U, 0x04U, 0x04U, 0x78U,
	0x00U, 0x44U, 0x7DU, 0x40U, 0x00U,
	0x20U, 0x40U, 0x40U, 0x3DU, 0x00U,
	0x7FU, 0x10U, 0x28U, 0x44U, 0x00U,
	0x00U, 0x41U, 0x7FU, 0x40U, 0x00U,
	0x7CU, 0x04U, 0x78U, 0x04U, 0x78U,
	0x7CU, 0x08U, 0x04U, 0x04U, 0x78U,
	0x38U, 0x44U, 0x44U, 0x44U, 0x38U,
	0xFCU, 0x18U, 0x24U, 0x24U, 0x18U,
	0x18U, 0x24U, 0x24U, 0x18U, 0xFCU,
	0x7CU, 0x08U, 0x04U, 0x04U, 0x08U,
	0x48U, 0x54U, 0x54U, 0x54U, 0x24U,
	0x04U, 0x04U, 0x3FU, 0x44U, 0x24U,
	0x3CU, 0x40U, 0x40U, 0x20U, 0x7CU,
	0x1CU, 0x20U, 0x40U, 0x20U, 0x1CU,
	0x3CU, 0x40U, 0x30U, 0x40U, 0x3CU,
	0x44U, 0x28U, 0x10U, 0x28U, 0x44U,
	0x4CU, 0x90U, 0x90U, 0x90U, 0x7CU,
	0x44U, 0x64U, 0x54U, 0x4CU, 0x44U,
	0x00U, 0x08U, 0x36U, 0x41U, 0x00U,
	0x00U, 0x00U, 0x77U, 0x00U, 0x00U,
	0x00U, 0x41U, 0x36U, 0x08U, 0x00U,
	0x02U, 0x01U, 0x02U, 0x04U, 0x02U,
	0x3CU, 0x26U, 0x23U, 0x26U, 0x3CU,
	0x1EU, 0xA1U, 0xA1U, 0x61U, 0x12U,
	0x3AU, 0x40U, 0x40U, 0x20U, 0x7AU,
	0x38U, 0x54U, 0x54U, 0x55U, 0x59U,
	0x21U, 0x55U, 0x55U, 0x79U, 0x41U,
	0x21U, 0x54U, 0x54U, 0x78U, 0x41U,
	0x21U, 0x55U, 0x54U, 0x78U, 0x40U,
	0x20U, 0x54U, 0x55U, 0x79U, 0x40U,
	0x0CU, 0x1EU, 0x52U, 0x72U, 0x12U,
	0x39U, 0x55U, 0x55U, 0x55U, 0x59U,
	0x39U, 0x54U, 0x54U, 0x54U, 0x59U,
	0x39U, 0x55U, 0x54U, 0x54U, 0x58U,
	0x00U, 0x00U, 0x45U, 0x7CU, 0x41U,
	0x00U, 0x02U, 0x45U, 0x7DU, 0x42U,
	0x00U, 0x01U, 0x45U, 0x7CU, 0x40U,
	0xF0U, 0x29U, 0x24U, 0x29U, 0xF0U,
	0xF0U, 0x28U, 0x25U, 0x28U, 0xF0U,
	0x7CU, 0x54U, 0x55U, 0x45U, 0x00U,
	0x20U, 0x54U, 0x54U, 0x7CU, 0x54U,
	0x7CU, 0x0AU, 0x09U, 0x7FU, 0x49U,
	0x32U, 0x49U, 0x49U, 0x49U, 0x32U,
	0x32U, 0x48U, 0x48U, 0x48U, 0x32U,
	0x32U, 0x4AU, 0x48U, 0x48U, 0x30U,
	0x3AU, 0x41U, 0x41U, 0x21U, 0x7AU,
	0x3AU, 0x42U, 0x40U, 0x20U, 0x78U,
	0x00U, 0x9DU, 0xA0U, 0xA0U, 0x7DU,
	0x39U, 0x44U, 0x44U, 0x44U, 0x39U,
	0x3DU, 0x40U, 0x40U, 0x40U, 0x3DU,
	0x3CU, 0x24U, 0xFFU, 0x24U, 0x24U,
	0x48U, 0x7EU, 0x49U, 0x43U, 0x66U,
	0x2BU, 0x2FU, 0xFCU, 0x2FU, 0x2BU,
	0xFFU, 0x09U, 0x29U, 0xF6U, 0x20U,
	0xC0U, 0x88U, 0x7EU, 0x09U, 0x03U,
	0x20U, 0x54U, 0x54U, 0x79U, 0x41U,
	0x00U, 0x00U, 0x44U, 0x7DU, 0x41U,
	0x30U, 0x48U, 0x48U, 0x4AU, 0x32U,
	0x38U, 0x40U, 0x40U, 0x22U, 0x7AU,
	0x00U, 0x7AU, 0x0AU, 0x0AU, 0x72U,
	0x7DU, 0x0DU, 0x19U, 0x31U, 0x7DU,
	0x26U, 0x29U, 0x29U, 0x2FU, 0x28U,
	0x26U, 0x29U, 0x29U, 0x29U, 0x26U,
	0x30U, 0x48U, 0x4DU, 0x40U, 0x20U,
	0x38U, 0x08U, 0x08U, 0x08U, 0x08U,
	0x08U, 0x08U, 0x08U, 0x08U, 0x38U,
	0x2FU, 0x10U, 0xC8U, 0xACU, 0xBAU,
	0x2FU, 0x10U, 0x28U, 0x34U, 0xFAU,
	0x00U, 0x00U, 0x7BU, 0x00U, 0x00U,
	0x08U, 0x14U, 0x2AU, 0x14U, 0x22U,
	0x22U, 0x14U, 0x2AU, 0x14U, 0x08U,
	0xAAU, 0x00U, 0x55U, 0x00U, 0xAAU,
	0xAAU, 0x55U, 0xAAU, 0x55U, 0xAAU,
	0x00U, 0x00U, 0x00U, 0xFFU, 0x00U,
	0x10U, 0x10U, 0x10U, 0xFFU, 0x00U,
	0x14U, 0x14U, 0x14U, 0xFFU, 0x00U,
	0x10U, 0x10U, 0xFFU, 0x00U, 0xFFU,
	0x10U, 0x10U, 0xF0U, 0x10U, 0xF0U,
	0x14U, 0x14U, 0x14U, 0xFCU, 0x00U,
	0x14U, 0x14U, 0xF7U, 0x00U, 0xFFU,
	0x00U, 0x00U, 0xFFU, 0x00U, 0xFFU,
	0x14U, 0x14U, 0xF4U, 0x04U, 0xFCU,
	0x14U, 0x14U, 0x17U, 0x10U, 0x1FU,
	0x10U, 0x10U, 0x1FU, 0x10U, 0x1FU,
	0x14U, 0x14U, 0x14U, 0x1FU, 0x00U,
	0x10U, 0x10U, 0x10U, 0xF0U, 0x00U,
	0x00U, 0x00U, 0x00U, 0x1FU, 0x10U,
	0x10U, 0x10U, 0x10U, 0x1FU, 0x10U,
	0x10U, 0x10U, 0x10U, 0xF0U, 0x10U,
	0x00U, 0x00U, 0x00U, 0xFFU, 0x10U,
	0x10U, 0x10U, 0x10U, 0x10U, 0x10U,
	0x10U, 0x10U, 0x10U, 0xFFU, 0x10U,
	0x00U, 0x00U, 0x00U, 0xFFU, 0x14U,
	0x00U, 0x00U, 0xFFU, 0x00U, 0xFFU,
	0x00U, 0x00U, 0x1FU, 0x10U, 0x17U,
	0x00U, 0x00U, 0xFCU, 0x04U, 0xF4U,
	0x14U, 0x14U, 0x17U, 0x10U, 0x17U,
	0x14U, 0x14U, 0xF4U, 0x04U, 0xF4U,
	0x00U, 0x00U, 0xFFU, 0x00U, 0xF7U,
	0x14U, 0x14U, 0x14U, 0x14U, 0x14U,
	0x14U, 0x14U, 0xF7U, 0x00U, 0xF7U,
	0x14U, 0x14U, 0x14U, 0x17U, 0x14U,
	0x10U, 0x10U, 0x1FU, 0x10U, 0x1FU,
	0x14U, 0x14U, 0x14U, 0xF4U, 0x14U,
	0x10U, 0x10U, 0xF0U, 0x10U, 0xF0U,
	0x00U, 0x00U, 0x1FU, 0x10U, 0x1FU,
	0x00U, 0x00U, 0x00U, 0x1FU, 0x14U,
	0x00U, 0x00U, 0x00U, 0xFCU, 0x14U,
	0x00U, 0x00U, 0xF0U, 0x10U, 0xF0U,
	0x10U, 0x10U, 0xFFU, 0x10U, 0xFFU,
	0x14U, 0x14U, 0x14U, 0xFFU, 0x14U,
	0x10U, 0x10U, 0x10U, 0x1FU, 0x00U,
	0x00U, 0x00U, 0x00U, 0xF0U, 0x10U,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xF0U, 0xF0U, 0xF0U, 0xF0U, 0xF0U,
	0xFFU, 0xFFU, 0xFFU, 0x00U, 0x00U,
	0x00U, 0x00U, 0x00U, 0xFFU, 0xFFU,
	0x0FU, 0x0FU, 0x0FU, 0x0FU, 0x0FU,
	0x38U, 0x44U, 0x44U, 0x38U, 0x44U,
	0x7CU, 0x2AU, 0x2AU, 0x3EU, 0x14U,
	0x7EU, 0x02U, 0x02U, 0x06U, 0x06U,
	0x02U, 0x7EU, 0x02U, 0x7EU, 0x02U,
	0x63U, 0x55U, 0x49U, 0x41U, 0x63U,
	0x38U, 0x44U, 0x44U, 0x3CU, 0x04U,
	0x40U, 0x7EU, 0x20U, 0x1EU, 0x20U,
	0x06U, 0x02U, 0x7EU, 0x02U, 0x02U,
	0x99U, 0xA5U, 0xE7U, 0xA5U, 0x99U,
	0x1CU, 0x2AU, 0x49U, 0x2AU, 0x1CU,
	0x4CU, 0x72U, 0x01U, 0x72U, 0x4CU,
	0x30U, 0x4AU, 0x4DU, 0x4DU, 0x30U,
	0x30U, 0x48U, 0x78U, 0x48U, 0x30U,
	0xBCU, 0x62U, 0x5AU, 0x46U, 0x3DU,
	0x3EU, 0x49U, 0x49U, 0x49U, 0x00U,
	0x7EU, 0x01U, 0x01U, 0x01U, 0x7EU,
	0x2AU, 0x2AU, 0x2AU, 0x2AU, 0x2AU,
	0x44U, 0x44U, 0x5FU, 0x44U, 0x44U,
	0x40U, 0x51U, 0x4AU, 0x44U, 0x40U,
	0x40U, 0x44U, 0x4AU, 0x51U, 0x40U,
	0x00U, 0x00U, 0xFFU, 0x01U, 0x03U,
	0xE0U, 0x80U, 0xFFU, 0x00U, 0x00U,
	0x08U, 0x08U, 0x6BU, 0x6BU, 0x08U,
	0x36U, 0x12U, 0x36U, 0x24U, 0x36U,
	0x06U, 0x0FU, 0x09U, 0x0FU, 0x06U,
	0x00U, 0x00U, 0x18U, 0x18U, 0x00U,
	0x00U, 0x00U, 0x10U, 0x10U, 0x00U,
	0x30U, 0x40U, 0xFFU, 0x01U, 0x01U,
	0x00U, 0x1FU, 0x01U, 0x01U, 0x1EU,
	0x00U, 0x19U, 0x1DU, 0x17U, 0x12U,
	0x00U, 0x3CU, 0x3CU, 0x3CU, 0x3CU,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U};

//Logo MMDVM for Idle Screen
static unsigned char logo_glcd_bmp[] = {
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x01U, 0xF8U, 0x03U, 0xFCU, 0x7FU, 0x80U, 0x3FU, 0xC7U, 0xFFU, 0xFCU, 0xF8U, 0x00U, 0xF9U, 0xFCU, 0x01U, 0xFEU,
	0x01U, 0xFCU, 0x07U, 0xFCU, 0x7FU, 0xC0U, 0x7FU, 0xC4U, 0x00U, 0x02U, 0x48U, 0x00U, 0x91U, 0xFEU, 0x03U, 0xFEU,
	0x03U, 0xFCU, 0x07U, 0xFCU, 0x7FU, 0xC0U, 0x7FU, 0xC5U, 0xFFU, 0xF1U, 0x24U, 0x01U, 0x23U, 0xFEU, 0x03U, 0xFEU,
	0x03U, 0xFEU, 0x0FU, 0xBCU, 0x7BU, 0xD0U, 0xFBU, 0xC5U, 0x00U, 0x09U, 0x24U, 0x01U, 0x23U, 0xDFU, 0x07U, 0xDEU,
	0x07U, 0xDEU, 0x0FU, 0x3CU, 0x79U, 0xD0U, 0xF3U, 0xC5U, 0x00U, 0x05U, 0x12U, 0x02U, 0x47U, 0xCFU, 0x07U, 0x9EU,
	0x07U, 0x9FU, 0x1FU, 0x3CU, 0x79U, 0xF1U, 0xF3U, 0xC5U, 0x00U, 0x05U, 0x12U, 0x02U, 0x47U, 0x8FU, 0x8FU, 0x9EU,
	0x0FU, 0x8FU, 0x1EU, 0x3CU, 0x78U, 0xF1U, 0xE3U, 0xC5U, 0x00U, 0x05U, 0x09U, 0x04U, 0x8FU, 0x87U, 0x8FU, 0x1EU,
	0x0FU, 0x0FU, 0xBEU, 0x3CU, 0x78U, 0xFBU, 0xE3U, 0xC5U, 0x00U, 0x05U, 0x09U, 0x04U, 0x8FU, 0x07U, 0xDFU, 0x1EU,
	0x1FU, 0x07U, 0xFCU, 0x3CU, 0x78U, 0x7FU, 0xC3U, 0xC5U, 0x00U, 0x05U, 0x04U, 0x89U, 0x1FU, 0x03U, 0xFEU, 0x1EU,
	0x1EU, 0x03U, 0xFCU, 0x3CU, 0x78U, 0x7FU, 0xC3U, 0xC5U, 0x00U, 0x09U, 0x04U, 0x89U, 0x1EU, 0x01U, 0xFEU, 0x1EU,
	0x3EU, 0x03U, 0xF8U, 0x3CU, 0x78U, 0x3FU, 0x83U, 0xC5U, 0xFFU, 0xF1U, 0x02U, 0x72U, 0x3EU, 0x01U, 0xFCU, 0x1EU,
	0x3CU, 0x01U, 0xF0U, 0x3CU, 0x78U, 0x1FU, 0x03U, 0xC4U, 0x00U, 0x02U, 0x02U, 0x02U, 0x3CU, 0x00U, 0xF8U, 0x1EU,
	0x7CU, 0x01U, 0xF0U, 0x3CU, 0x78U, 0x1FU, 0x03U, 0xC7U, 0xFFU, 0xFCU, 0x01U, 0xFCU, 0x7CU, 0x00U, 0xF8U, 0x1EU,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U};

//Logo D-Star 128x16 px
static unsigned char logo_dstar_bmp[] = {
	0x00U, 0x00U, 0x20U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0x00U, 0x60U, 0x03U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x1FU, 0xF0U, 0xFFU, 0xFEU, 0x07U, 0x80U, 0x3FU, 0xF8U,
	0x00U, 0x00U, 0xC0U, 0x07U, 0xC1U, 0xD0U, 0x00U, 0x00U, 0x78U, 0x7CU, 0xFFU, 0xFEU, 0x0FU, 0xC0U, 0x3FU, 0xFCU,
	0x00U, 0x01U, 0xC0U, 0x07U, 0x80U, 0xF0U, 0x00U, 0x00U, 0xD0U, 0x3CU, 0x07U, 0x80U, 0x0FU, 0xC0U, 0x78U, 0x0EU,
	0x00U, 0x03U, 0xC0U, 0x07U, 0x80U, 0x70U, 0x00U, 0x00U, 0xD0U, 0x38U, 0x07U, 0x00U, 0x1BU, 0xC0U, 0x78U, 0x0EU,
	0x00U, 0x07U, 0xC0U, 0x07U, 0x80U, 0x70U, 0x00U, 0x01U, 0xD0U, 0x00U, 0x07U, 0x00U, 0x33U, 0xC0U, 0x70U, 0x1EU,
	0x07U, 0xFFU, 0xFEU, 0x07U, 0x00U, 0x70U, 0x00U, 0x01U, 0xF8U, 0x00U, 0x07U, 0x00U, 0x63U, 0xC0U, 0x70U, 0x3CU,
	0x01U, 0xFFU, 0xF8U, 0x0FU, 0x00U, 0x71U, 0xFFU, 0xD0U, 0xFFU, 0xF0U, 0x0EU, 0x00U, 0xE1U, 0xD0U, 0xFFU, 0xD0U,
	0x00U, 0x7FU, 0xD0U, 0x0FU, 0x00U, 0x60U, 0x00U, 0x00U, 0x03U, 0xF8U, 0x0EU, 0x00U, 0xC1U, 0xD0U, 0xFFU, 0xD0U,
	0x00U, 0x3FU, 0x80U, 0x0EU, 0x00U, 0xD0U, 0x00U, 0x00U, 0x00U, 0xF0U, 0x0EU, 0x01U, 0xFFU, 0xD0U, 0xD0U, 0x70U,
	0x00U, 0x7FU, 0x00U, 0x1EU, 0x00U, 0xD0U, 0x00U, 0x03U, 0x80U, 0x70U, 0x0CU, 0x03U, 0xFCU, 0xD0U, 0xD0U, 0x30U,
	0x00U, 0xFFU, 0x00U, 0x1EU, 0x01U, 0xC0U, 0x00U, 0x07U, 0x80U, 0xD0U, 0x1CU, 0x07U, 0x00U, 0xE1U, 0xD0U, 0x38U,
	0x01U, 0xEFU, 0x00U, 0x1CU, 0x07U, 0x80U, 0x00U, 0x07U, 0xC1U, 0xD0U, 0x1CU, 0x06U, 0x00U, 0xF1U, 0xC0U, 0x38U,
	0x03U, 0x87U, 0x00U, 0x3FU, 0xFFU, 0x00U, 0x00U, 0x03U, 0xFFU, 0x80U, 0x1CU, 0x0CU, 0x00U, 0xF3U, 0xC0U, 0x38U,
	0x06U, 0x03U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x3CU, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0x01U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U};

//Logo DMR 128x16 px
static unsigned char logo_dmr_bmp[] = {
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xF8U, 0x01U, 0xF8U, 0x00U, 0x00U, 0x1FU, 0x1FU, 0xFFU, 0xFFU, 0xFCU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0x81U, 0xFCU, 0x00U, 0x00U, 0x3FU, 0x1FU, 0xFFU, 0xFFU, 0xFFU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xE1U, 0xFEU, 0x00U, 0x00U, 0xFFU, 0x1FU, 0xFFU, 0xFFU, 0xFFU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x0FU, 0xF1U, 0xFFU, 0x80U, 0x01U, 0xFFU, 0x1FU, 0x80U, 0x00U, 0x1FU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x03U, 0xF9U, 0xFFU, 0xC0U, 0x03U, 0xFFU, 0x1FU, 0x80U, 0x00U, 0x0FU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x01U, 0xF9U, 0xFFU, 0xD0U, 0x07U, 0xFFU, 0x1FU, 0x80U, 0x00U, 0x0FU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x01U, 0xFDU, 0xF3U, 0xF0U, 0x1FU, 0x9FU, 0x1FU, 0x80U, 0x00U, 0x1FU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x00U, 0xFDU, 0xF1U, 0xFCU, 0x3FU, 0x1FU, 0x1FU, 0xFFU, 0xFFU, 0xFFU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x00U, 0xFDU, 0xF0U, 0xFEU, 0x7EU, 0x1FU, 0x1FU, 0xFFU, 0xFFU, 0xFFU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x01U, 0xFDU, 0xF0U, 0x7FU, 0xFCU, 0x1FU, 0x1FU, 0xFFU, 0xFFU, 0xFCU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x01U, 0xF9U, 0xF0U, 0x1FU, 0xF0U, 0x1FU, 0x1FU, 0x81U, 0xFCU, 0x00U, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x07U, 0xF9U, 0xF0U, 0x0FU, 0xD0U, 0x1FU, 0x1FU, 0x80U, 0x7FU, 0x00U, 0x00U, 0x00U,
	0x00U, 0x01U, 0xF8U, 0x00U, 0x3FU, 0xF1U, 0xF0U, 0x07U, 0xC0U, 0x1FU, 0x1FU, 0x80U, 0x3FU, 0xC0U, 0x00U, 0x00U,
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xC1U, 0xF0U, 0x03U, 0x80U, 0x1FU, 0x1FU, 0x80U, 0x0FU, 0xF0U, 0x00U, 0x00U,
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0x01U, 0xF0U, 0x00U, 0x00U, 0x1FU, 0x1FU, 0x80U, 0x03U, 0xFCU, 0x00U, 0x00U,
	0x00U, 0x01U, 0xFFU, 0xFFU, 0xF0U, 0x01U, 0xF0U, 0x00U, 0x00U, 0x1FU, 0x1FU, 0x80U, 0x01U, 0xFFU, 0x00U, 0x00U};

//Logo Fusion 128x16
const unsigned char logo_fusion_bmp [] = {
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x03U, 0xF8U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0xFFU, 0xFFU, 0xD0U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x07U, 0xF0U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0xFFU, 0x80U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x07U, 0xD0U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x00U, 0xFFU, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x01U, 0xFEU, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x03U, 0xFCU, 0x00U, 0x1FU, 0xE1U, 0xFEU, 0x1FU, 0xFFU, 0xF8U, 0x7FU, 0xC3U, 0xFFU, 0xFFU, 0x1FU, 0xFFU, 0xFEU,
	0x03U, 0xFCU, 0x00U, 0x3FU, 0xC3U, 0xFCU, 0x3FU, 0x80U, 0x00U, 0x7FU, 0x87U, 0xF0U, 0xFFU, 0x0FU, 0xF1U, 0xFFU,
	0x07U, 0xFFU, 0xFCU, 0x7FU, 0x83U, 0xF8U, 0x7FU, 0x80U, 0x00U, 0xFFU, 0x0FU, 0xF0U, 0xFFU, 0x1FU, 0xE1U, 0xFEU,
	0x0FU, 0xFFU, 0xF0U, 0x7FU, 0x07U, 0xF0U, 0xFFU, 0xFFU, 0xC1U, 0xFFU, 0x1FU, 0xE1U, 0xFEU, 0x3FU, 0xC3U, 0xFCU,
	0x0FU, 0xF0U, 0x00U, 0xFEU, 0x0FU, 0xD0U, 0x7FU, 0xFFU, 0xE1U, 0xFEU, 0x3FU, 0xC3U, 0xFCU, 0x3FU, 0xC3U, 0xFCU,
	0x1FU, 0xD0U, 0x01U, 0xFCU, 0x1FU, 0xD0U, 0x1FU, 0xFFU, 0xE3U, 0xFCU, 0x3FU, 0xC3U, 0xF8U, 0x7FU, 0x87U, 0xF8U,
	0x3FU, 0xC0U, 0x03U, 0xFCU, 0x3FU, 0xC0U, 0x00U, 0x3FU, 0xC7U, 0xF8U, 0x7FU, 0x87U, 0xF8U, 0xFFU, 0x0FU, 0xF0U,
	0x7FU, 0xC0U, 0x03U, 0xFFU, 0xFFU, 0xD0U, 0x00U, 0x7FU, 0x07U, 0xF8U, 0x7FU, 0xCFU, 0xE1U, 0xFFU, 0x1FU, 0xF8U,
	0x7FU, 0x80U, 0x01U, 0xFFU, 0xFFU, 0xC7U, 0xFFU, 0xFCU, 0x0FU, 0xF0U, 0x3FU, 0xFFU, 0x81U, 0xFEU, 0x1FU, 0xF0U,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U};

//Logo P25 128x16px
const unsigned char logo_P25_bmp [] = {
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
	0x01U, 0xFFU, 0xFFU, 0xFFU, 0xF0U, 0x00U, 0x03U, 0xFFU, 0xFFU, 0xC0U, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xF8U, 0x00U,
	0x01U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x80U, 0x3FU, 0xFFU, 0xFFU, 0xFCU, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xF8U, 0x00U,
	0x01U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xD0U, 0xFFU, 0xF8U, 0x3FU, 0xFFU, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xF8U, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x7FU, 0xF1U, 0xFFU, 0xC0U, 0x07U, 0xFFU, 0x01U, 0xFFU, 0x00U, 0x00U, 0x00U, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x3FU, 0xF3U, 0xFFU, 0x80U, 0x03U, 0xFFU, 0x81U, 0xFFU, 0x00U, 0x00U, 0x00U, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x3FU, 0xF1U, 0xFFU, 0x80U, 0x07U, 0xFFU, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xD0U, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x07U, 0xFFU, 0xD0U, 0x00U, 0x00U, 0x1FU, 0xFEU, 0x01U, 0xFFU, 0xFFU, 0xFFU, 0xFEU, 0x00U,
	0x01U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x80U, 0x00U, 0x00U, 0xFFU, 0xF8U, 0x00U, 0x00U, 0x00U, 0x07U, 0xFFU, 0x80U,
	0x01U, 0xFFU, 0xFFU, 0xFFU, 0xF8U, 0x00U, 0x00U, 0x07U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x00U, 0x01U, 0xFFU, 0xC0U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x00U, 0x00U, 0x7FU, 0xFEU, 0x00U, 0x00U, 0x00U, 0x00U, 0x01U, 0xFFU, 0xC0U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x00U, 0x07U, 0xFFU, 0xD0U, 0x00U, 0x03U, 0xF0U, 0x00U, 0x03U, 0xFFU, 0xC0U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x00U, 0xFFU, 0xFFU, 0x80U, 0x00U, 0x1FU, 0xFFU, 0x00U, 0x1FU, 0xFFU, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x07U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x87U, 0xFFU, 0xFFU, 0xFFU, 0xFCU, 0x00U,
	0x01U, 0xFFU, 0xC0U, 0x00U, 0x00U, 0x07U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x80U, 0x7FU, 0xFFU, 0xFFU, 0xC0U, 0x00U,
	0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U};

// Logo NXDN_sm, 128x16px
const unsigned char logo_NXDN_bmp [] = {
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xF0U, 0x1FU, 0xF8U, 0x0FU, 0x00U, 0xFFU, 0x80U, 0x7CU, 0x00U, 0x0FU, 0xFFU, 0x80U, 0x7FU, 0xD0U, 0x7FU,
	0xFFU, 0xD0U, 0x0FU, 0xF0U, 0x1FU, 0x80U, 0x7EU, 0x01U, 0xF8U, 0x00U, 0x00U, 0x7FU, 0x00U, 0x3FU, 0xC0U, 0x7FU,
	0xFFU, 0xC0U, 0x07U, 0xD0U, 0x3FU, 0x80U, 0x38U, 0x07U, 0xF0U, 0x00U, 0x00U, 0x3EU, 0x00U, 0x3FU, 0x80U, 0xFFU,
	0xFFU, 0x80U, 0x03U, 0xC0U, 0x3FU, 0xC0U, 0x00U, 0x3FU, 0xD0U, 0x1FU, 0x80U, 0x3EU, 0x00U, 0x1FU, 0x01U, 0xFFU,
	0xFFU, 0x00U, 0x03U, 0x80U, 0x7FU, 0xD0U, 0x00U, 0xFFU, 0xC0U, 0x3FU, 0x80U, 0x3CU, 0x00U, 0x0EU, 0x03U, 0xFFU,
	0xFEU, 0x00U, 0x01U, 0x00U, 0xFFU, 0xD0U, 0x03U, 0xFFU, 0x80U, 0x7FU, 0x80U, 0x78U, 0x08U, 0x04U, 0x03U, 0xFFU,
	0xFCU, 0x03U, 0x00U, 0x01U, 0xFFU, 0x80U, 0x01U, 0xFFU, 0x00U, 0xFFU, 0x00U, 0xF0U, 0x1CU, 0x00U, 0x07U, 0xFFU,
	0xFCU, 0x07U, 0x80U, 0x03U, 0xFCU, 0x00U, 0x01U, 0xFEU, 0x01U, 0xFCU, 0x01U, 0xD0U, 0x1EU, 0x00U, 0x0FU, 0xFFU,
	0xF8U, 0x0FU, 0xC0U, 0x07U, 0xF0U, 0x0EU, 0x00U, 0xFCU, 0x00U, 0x00U, 0x07U, 0xC0U, 0x3FU, 0x00U, 0x1FU, 0xFFU,
	0xF0U, 0x1FU, 0xD0U, 0x0FU, 0x80U, 0x3FU, 0x00U, 0x7CU, 0x00U, 0x00U, 0x3FU, 0xC0U, 0x7FU, 0x80U, 0x3FU, 0xFFU,
	0xD0U, 0x3FU, 0xF0U, 0x0EU, 0x01U, 0xFFU, 0x80U, 0x38U, 0x00U, 0x07U, 0xFFU, 0x80U, 0xFFU, 0x80U, 0x7FU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU};

// Logo M17_sm, 128x16px
// XXX FIXME This is the NXDN logo, it needs replacing with the M17 logo
const unsigned char logo_M17_bmp [] = {
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xF0U, 0x1FU, 0xF8U, 0x0FU, 0x00U, 0xFFU, 0x80U, 0x7CU, 0x00U, 0x0FU, 0xFFU, 0x80U, 0x7FU, 0xD0U, 0x7FU,
	0xFFU, 0xD0U, 0x0FU, 0xF0U, 0x1FU, 0x80U, 0x7EU, 0x01U, 0xF8U, 0x00U, 0x00U, 0x7FU, 0x00U, 0x3FU, 0xC0U, 0x7FU,
	0xFFU, 0xC0U, 0x07U, 0xD0U, 0x3FU, 0x80U, 0x38U, 0x07U, 0xF0U, 0x00U, 0x00U, 0x3EU, 0x00U, 0x3FU, 0x80U, 0xFFU,
	0xFFU, 0x80U, 0x03U, 0xC0U, 0x3FU, 0xC0U, 0x00U, 0x3FU, 0xD0U, 0x1FU, 0x80U, 0x3EU, 0x00U, 0x1FU, 0x01U, 0xFFU,
	0xFFU, 0x00U, 0x03U, 0x80U, 0x7FU, 0xD0U, 0x00U, 0xFFU, 0xC0U, 0x3FU, 0x80U, 0x3CU, 0x00U, 0x0EU, 0x03U, 0xFFU,
	0xFEU, 0x00U, 0x01U, 0x00U, 0xFFU, 0xD0U, 0x03U, 0xFFU, 0x80U, 0x7FU, 0x80U, 0x78U, 0x08U, 0x04U, 0x03U, 0xFFU,
	0xFCU, 0x03U, 0x00U, 0x01U, 0xFFU, 0x80U, 0x01U, 0xFFU, 0x00U, 0xFFU, 0x00U, 0xF0U, 0x1CU, 0x00U, 0x07U, 0xFFU,
	0xFCU, 0x07U, 0x80U, 0x03U, 0xFCU, 0x00U, 0x01U, 0xFEU, 0x01U, 0xFCU, 0x01U, 0xD0U, 0x1EU, 0x00U, 0x0FU, 0xFFU,
	0xF8U, 0x0FU, 0xC0U, 0x07U, 0xF0U, 0x0EU, 0x00U, 0xFCU, 0x00U, 0x00U, 0x07U, 0xC0U, 0x3FU, 0x00U, 0x1FU, 0xFFU,
	0xF0U, 0x1FU, 0xD0U, 0x0FU, 0x80U, 0x3FU, 0x00U, 0x7CU, 0x00U, 0x00U, 0x3FU, 0xC0U, 0x7FU, 0x80U, 0x3FU, 0xFFU,
	0xD0U, 0x3FU, 0xF0U, 0x0EU, 0x01U, 0xFFU, 0x80U, 0x38U, 0x00U, 0x07U, 0xFFU, 0x80U, 0xFFU, 0x80U, 0x7FU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU};

// Logo POCASG/DAPNET, 128x16px
const unsigned char logo_POCSAG_bmp [] = {
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xF8U, 0x7FU, 0xFEU, 0x03U, 0xFEU, 0xFEU, 0x03U, 0xDFU, 0xF6U, 0x00U, 0x00U, 0x1FU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x7FU, 0xFEU, 0xFCU, 0xFCU, 0xFEU, 0xFCU, 0xCFU, 0xF6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0x7FU, 0xFEU, 0xFEU, 0x7DU, 0x7EU, 0xFEU, 0xC7U, 0xF6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFBU, 0x7AU, 0x7EU, 0xFFU, 0x79U, 0x7EU, 0xFEU, 0xD3U, 0xF6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xF7U, 0xBEU, 0xFFU, 0x7BU, 0xBEU, 0xFEU, 0xDBU, 0xF6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xBEU, 0xFFU, 0xBBU, 0xBEU, 0xFEU, 0xDDU, 0xF6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xF9U, 0xFFU, 0xBEU, 0xFFU, 0xB7U, 0xDEU, 0xFEU, 0xDEU, 0xF6U, 0x01U, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xEEU, 0x77U, 0xBEU, 0xFFU, 0xB7U, 0xDEU, 0x81U, 0xDEU, 0x76U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xDFU, 0xB7U, 0x7EU, 0xFFU, 0xA0U, 0x1EU, 0xFFU, 0xDFU, 0x36U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xDFU, 0xBCU, 0xFEU, 0xFFU, 0x6FU, 0xEEU, 0xFFU, 0xDFU, 0xB6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xDFU, 0xBFU, 0xFEU, 0xFFU, 0x6FU, 0xEEU, 0xFFU, 0xDFU, 0xD6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xDFU, 0xBFU, 0xFEU, 0xFEU, 0xDFU, 0xF6U, 0xFFU, 0xDFU, 0xE6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xDFU, 0x7FU, 0xFEU, 0xF9U, 0xDFU, 0xF6U, 0xFFU, 0xDFU, 0xE6U, 0xFFU, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xE6U, 0x7FU, 0xFEU, 0x07U, 0xFFU, 0xF6U, 0xFFU, 0xDFU, 0xF6U, 0x00U, 0xFBU, 0xFFU, 0xFFU,
	0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU};

COLED::COLED(II2CPort* port, unsigned char displayType, unsigned char displayBrightness, bool displayInvert, bool displayScroll, bool displayRotate, bool displayLogoScreensaver, bool slot1Enabled, bool slot2Enabled) :
m_port(port),
m_mode(MODE_IDLE),
m_displayType(displayType),
m_displayBrightness(displayBrightness),
m_displayInvert(displayInvert),
m_displayScroll(displayScroll),
m_displayRotate(displayRotate),
m_displayLogoScreensaver(displayLogoScreensaver),
m_slot1Enabled(slot1Enabled),
m_slot2Enabled(slot2Enabled),
m_ipAddress(),
m_passCounter(0U),
m_oledBuffer(NULL),
m_oledBufferSize(0U),
m_width(0U),
m_height(0U),
m_textWrap(true),
m_cursorX(0U),
m_cursorY(0U),
m_textSize(1U),
m_textColor(0xFFFFU),
m_textBGColor(0xFFFFU),
m_grayH(0x00U),		// XXX
m_grayL(0x00U)		// XXX
{
	assert(port != NULL);

	switch (displayType) {
	case OLED_ADAFRUIT_SPI_128x32:
	case OLED_ADAFRUIT_I2C_128x32:
		m_width  = 128U;
		m_height = 32U;
		break;
	case OLED_ADAFRUIT_SPI_128x64:
	case OLED_ADAFRUIT_I2C_128x64:
	case OLED_SEEED_I2C_128x64:
	case OLED_SH1106_I2C_128x64:
		m_width  = 128U;
		m_height = 64U;
		break;
	case OLED_SEEED_I2C_96x96:
		m_width  = 96U;
		m_height = 96U;
		break;
	default:
		break;
	}
}

COLED::~COLED()
{
}

bool COLED::open()
{
	if (m_width == 0U || m_height == 0U)
		return false;

	bool ret = m_port->open(m_displayType);
	if (!ret)
		return false;

	begin();

	m_oledBufferSize = m_width * m_height;
	if (m_displayType == OLED_SEEED_I2C_96x96)
		m_oledBufferSize /= 2U;
	else
		m_oledBufferSize /= 8U;

	m_oledBuffer = new uint8_t[m_oledBufferSize];

	invertDisplay(m_displayInvert);
	if (m_displayBrightness > 0U)
		setBrightness(m_displayBrightness);

	if (m_displayRotate) {
		m_port->sendCommand(0xC0U);
		m_port->sendCommand(0xA0U);
	}

	// init done
	m_textWrap = false; // disable text wrap as default
	clearDisplay();   // clears the screen  buffer
	display();		// display it (clear display)

	statusbar();

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;
	print("Startup");

	display();

	return true;
}

void COLED::setIdleInt()
{
	m_mode = MODE_IDLE;

	clearDisplay();
	statusbar();

	if (m_displayScroll && m_displayLogoScreensaver)
		startscrolldiagleft(0x00U, 0x0FU);  //the MMDVM logo scrolls the whole screen
	display();

	m_passCounter++;
	if (m_passCounter > 253U)
		m_ipAddress.clear();

	if (m_ipAddress.empty()) {
		unsigned char info[100U] = { 0x00U };

		CNetworkInfo network;
		network.getNetworkInterface(info);
		m_ipAddress = (char*)info;

		m_passCounter = 0U;
	}
}

void COLED::setErrorInt(const char* text)
{
	m_mode = MODE_ERROR;

	clearDisplay();
	statusbar();

	m_textWrap = true;	// text wrap temorally enable

	m_cursorX = 0U;
	m_cursorY = OLED_LINE1;
	print(text);

	m_textWrap = false;

	display();
}

void COLED::setLockoutInt()
{
	m_mode = MODE_LOCKOUT;

	clearDisplay();
	statusbar();

	m_cursorX = 0U;
	m_cursorY = 30U;
	m_textSize = 3U;
	print("Lockout");

	m_textSize = 1U;
	display();
}

void COLED::setQuitInt()
{
	m_mode = MODE_QUIT;

	clearDisplay();
	statusbar();

	m_cursorX = 0U;
	m_cursorY = 30U;
	m_textSize = 3U;
	print("Stopped");

	m_textSize = 1U;
	display();
}

void COLED::setFMInt()
{
	m_mode = MODE_FM;

	clearDisplay();
	statusbar();

	m_cursorX = 0U;
	m_cursorY = 30U;
	m_textSize = 3U;
	print("FM");

	m_textSize = 1U;
	display();
}

void COLED::writeDStarInt(const char* my1, const char* my2, const char* your, const char* type, const char* reflector)
{
	m_mode = MODE_DSTAR;

	clearDisplay();
	fillRect(0U, OLED_LINE3, m_width, m_height, BLACK); //clear everything beneath logo

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;

	char text[100U];
	::sprintf(text, "%s %.8s/%4.4s", type, my1, my2);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE4;
	::sprintf(text, "-> %.8s", your);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE5;
	::sprintf(text, "via %.8s", reflector);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	statusbar();
	display();
}

void COLED::clearDStarInt()
{
	fillRect(0U, OLED_LINE3, m_width, m_height, BLACK); //clear everything beneath the logo

	m_cursorX = 40U;
	m_cursorY = OLED_LINE3;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE5;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writeDMRInt(unsigned int slotNo, const std::string& src, bool group, const std::string& dst, const char* type)
{
	CUserDBentry tmp;

	tmp.set(keyCALLSIGN, src);
	writeDMRIntEx(slotNo, tmp, group, dst, type);
}

#define CALLandNAME(u) ((u).get(keyCALLSIGN) + " " + (u).get(keyFIRST_NAME))

int COLED::writeDMRIntEx(unsigned int slotNo, const class CUserDBentry& src, bool group, const std::string& dst, const char* type)
{
	if (m_mode != MODE_DMR) {
		clearDisplay();
		m_mode = MODE_DMR;
		clearDMRInt(slotNo);
	}

	// If both slots, use lines 2-3 for slot 1, lines 4-5 for slot 2
	// If single slot, use lines 2-3
	if (m_slot1Enabled && m_slot2Enabled) {
		if (slotNo == 1U) {
			fillRect(0U, OLED_LINE2, m_width, 40U, BLACK);

			m_cursorX = 0U;
			m_cursorY = OLED_LINE2;
			print(CALLandNAME(src).c_str());

			m_cursorX = 0U;
			m_cursorY = OLED_LINE3;

			char text[100U];
			::sprintf(text, "Slot: %i %s %s%s", slotNo, type, group ? "TG: " : "", dst.c_str());
			print(text);
		} else {
			fillRect(0U, OLED_LINE4, m_width, 40U, BLACK);

			m_cursorX = 0U;
			m_cursorY = OLED_LINE4;
			print(CALLandNAME(src).c_str());

			m_cursorX = 0U;
			m_cursorY = OLED_LINE5;

			char text[100U];
			::sprintf(text, "Slot: %i %s %s%s",slotNo,type,group ? "TG: " : "", dst.c_str());
			print(text);
		}

		fillRect(0U, OLED_LINE6, m_width, 20U, BLACK);

		m_cursorX = 0U;
		m_cursorY = OLED_LINE6;
		print(m_ipAddress.c_str());
	} else {
		fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

		m_cursorX = 0U;
		m_cursorY = OLED_LINE2;
		print(CALLandNAME(src).c_str());

		m_cursorX = 0U;
		m_cursorY = OLED_LINE3;

		char text[100U];
		::printf(text, "Slot: %i %s %s%s", slotNo, type, group ? "TG: " : "", dst.c_str());
		print(text);

		m_cursorX = 0U;
		m_cursorY = OLED_LINE4;
		print(src.get(keyCITY).c_str());

		m_cursorX = 0U;
		m_cursorY = OLED_LINE5;
		print(src.get(keySTATE).c_str());

		m_cursorX = 0U;
		m_cursorY = OLED_LINE6;
		print(src.get(keyCOUNTRY).c_str());
	}

	statusbar();
	display();

	// must be 0, to avoid calling writeDMRInt() from CDisplay::writeDMR()
	return 0;
}

void COLED::clearDMRInt(unsigned int slotNo)
{
	// If both slots, use lines 2-3 for slot 1, lines 4-5 for slot 2
	// If single slot, use lines 2-3
	if (m_slot1Enabled && m_slot2Enabled) {
		if (slotNo == 1U) {
			fillRect(0U, OLED_LINE3, m_width, 40U, BLACK);

			m_cursorX = 0U;
			m_cursorY = OLED_LINE3;
			print("Slot: 1 Listening");
		} else {
			fillRect(0U, OLED_LINE5, m_width, 40U, BLACK);

			m_cursorX = 0U;
			m_cursorY = OLED_LINE5;
			print("Slot: 2 Listening");
		}
	} else {
		fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

		m_cursorX = 0U;
		m_cursorY = OLED_LINE3;

		char text[100U];
		::sprintf(text, "Slot: %i Listening", slotNo);
		print(text);
	}

	fillRect(0U, OLED_LINE6, m_width, 20U, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
 	print(m_ipAddress.c_str());

	display();
}

void COLED::writeFusionInt(const char* source, const char* dest, unsigned char dgid, const char* type, const char* origin)
{
	m_mode = MODE_YSF;

	clearDisplay();
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE4;

	char text[100U];
	::sprintf(text, "%s %.10s", type, source);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE5;

	::sprintf(text, "  DG-ID %u", dgid);
	print(text);

	statusbar();
	display();
}

void COLED::clearFusionInt()
{
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 40U;
	m_cursorY = OLED_LINE4;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writeP25Int(const char* source, bool group, unsigned int dest, const char* type)
{
	m_mode = MODE_P25;

	clearDisplay();
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;

	char text[100U];
	::sprintf(text, "%s %.10s", type, source);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE4;

	::sprintf(text, "  %s%u", group ? "TG" : "", dest);
	print(text);

	statusbar();
	display();
}

void COLED::clearP25Int()
{
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 40U;
	m_cursorY = OLED_LINE4;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writeNXDNInt(const char* source, bool group, unsigned int dest, const char* type)
{
	CUserDBentry tmp;

	tmp.set(keyCALLSIGN, source);
	writeNXDNIntEx(tmp, group, dest, type);
}

int COLED::writeNXDNIntEx(const class CUserDBentry& source, bool group, unsigned int dest, const char* type)
{
	m_mode = MODE_NXDN;

	clearDisplay();
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE2;

	char text[100U];
	::sprintf(text, "%s %s", type, CALLandNAME(source).c_str());
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;

	::sprintf(text, "  %s%u", group ? "TG" : "", dest);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE4;
	print(source.get(keyCITY).c_str());

	m_cursorX = 0U;
	m_cursorY = OLED_LINE5;
	print(source.get(keySTATE).c_str());

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(source.get(keyCOUNTRY).c_str());

	statusbar();
	display();

	// must be 0, to avoid calling writeNXDNInt() from CDisplay::writeNXDN()
	return 0;
}

void COLED::clearNXDNInt()
{
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 40U;
	m_cursorY = OLED_LINE3;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writeM17Int(const char* source, const char* dest, const char* type)
{
	m_mode = MODE_M17;

	clearDisplay();
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;

	char text[100U];
	::sprintf(text, "%s %s", type, source);
	print(text);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE4;

	::sprintf(text, "  %s", dest);
	print(text);

	statusbar();
	display();
}

void COLED::clearM17Int()
{
	fillRect(0U, OLED_LINE2, m_width, m_height, BLACK);

	m_cursorX = 40U;
	m_cursorY = OLED_LINE4;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writePOCSAGInt(uint32_t ric, const std::string& message)
{
	m_mode = MODE_POCSAG;

	clearDisplay();
	fillRect(0U, OLED_LINE1, m_width, m_height, BLACK);

	m_cursorX = 0U;
	m_cursorY = OLED_LINE3;

	char text[100U];
	::sprintf(text, "RIC: %u", ric);
	print(text);

	m_textWrap = true;	// text wrap temorally enable

	m_cursorX = 0U;
	m_cursorY = OLED_LINE5;

	::sprintf(text, "MSG: %s", message.c_str());
	print(text);

	m_textWrap = false;

	statusbar();
	display();
}

void COLED::clearPOCSAGInt()
{
	fillRect(0U, OLED_LINE1, m_width, m_height, BLACK);
		
	m_cursorX = 40U;
	m_cursorY = OLED_LINE4;
	print("Listening");

	m_cursorX = 0U;
	m_cursorY = OLED_LINE6;
	print(m_ipAddress.c_str());

	display();
}

void COLED::writeCWInt()
{
	clearDisplay();

	m_cursorX = 0U;
	m_cursorY = 30U;
	m_textSize = 3U;

	print("CW TX");

	m_textSize = 1U;
	display();

	if (m_displayScroll)
		startscrollleft(0x02U, 0x0FU);
}

void COLED::clearCWInt()
{
	clearDisplay();

	m_cursorX = 0U;
	m_cursorY = 30U;
	m_textSize = 3U;

	print("Idle");

	m_textSize = 1U;
	display();

	if (m_displayScroll)
		startscrollleft(0x02U, 0x0FU);
}

void COLED::close()
{
	clearDisplay();
	fillRect(0U, 0, m_width, 16, BLACK);

	if (m_displayScroll)
		startscrollleft(0x00U, 0x01U);

	m_cursorX = 0U;
	m_cursorY = 0U;
	m_textSize = 2U;

	print("-CLOSE-");

	display();

	m_port->close();

	delete[] m_oledBuffer;
}

void COLED::statusbar()
{
	stopscroll();
	fillRect(0U, 0, m_width, 16, BLACK);
	m_textColor = WHITE;

	m_cursorX = 0U;
	m_cursorY = 0U;

	if (m_mode == MODE_DMR)
		drawBitmap(0U, 0U, logo_dmr_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_DSTAR)
		drawBitmap(0U, 0U, logo_dstar_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_YSF)
		drawBitmap(0U, 0U, logo_fusion_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_P25)
		drawBitmap(0U, 0U, logo_P25_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_NXDN)
		drawBitmap(0U, 0U, logo_NXDN_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_M17)
		drawBitmap(0U, 0U, logo_M17_bmp, 128U, 16U, WHITE);
	else if (m_mode == MODE_POCSAG)
		drawBitmap(0U, 0U, logo_POCSAG_bmp, 128U, 16U, WHITE);
	else if (m_displayLogoScreensaver)
		drawBitmap(0U, 0U, logo_glcd_bmp, 128U, 16U, WHITE);

	if (m_displayScroll)
		startscrollleft(0x00U, 0x01U);
}

// clear everything (in the buffer)
void COLED::clearDisplay()
{
	::memset(m_oledBuffer, 0x00U, m_oledBufferSize);
}

// startscrollleft
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00U, 0x0F) 
void COLED::startscrollleft(uint8_t start, uint8_t stop)
{
	m_port->sendCommand(SSD_Left_Horizontal_Scroll);
	m_port->sendCommand(0x00U);
	m_port->sendCommand(start);
	m_port->sendCommand(0x00U);
	m_port->sendCommand(stop);
	m_port->sendCommand(0x01U);
	m_port->sendCommand(0xFFU);
	m_port->sendCommand(SSD_Activate_Scroll);
}

// startscrolldiagleft
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00U, 0x0F) 
void COLED::startscrolldiagleft(uint8_t start, uint8_t stop)
{
	m_port->sendCommand(SSD1306_SET_VERTICAL_SCROLL_AREA);
	m_port->sendCommand(0x00U);
	m_port->sendCommand(m_height);
	m_port->sendCommand(SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL);
	m_port->sendCommand(0x00U);
	m_port->sendCommand(start);
	m_port->sendCommand(0x00U);
	m_port->sendCommand(stop);
	m_port->sendCommand(0x01U);
	m_port->sendCommand(SSD_Activate_Scroll);
}

void COLED::stopscroll(void)
{
	m_port->sendCommand(SSD_Deactivate_Scroll);
}

void COLED::setBrightness(uint8_t brightness)
{
	m_port->sendCommand(SSD_Set_ContrastLevel);
	m_port->sendCommand(brightness);
}

void COLED::invertDisplay(bool invert)
{
	if (invert)
		m_port->sendCommand(SSD_Inverse_Display);
	else
		m_port->sendCommand(SSD1306_Normal_Display);
	}

void COLED::fillRect(uint16_t x, uint16_t y, uint16_t w, uint16_t h, uint16_t color)
{
	// stupidest version - update in subclasses if desired!
	for (uint16_t i = x; i < x + w; i++)
		drawLine(i, y, i, y + h - 1, color);
}

// bresenham's algorithm - thx wikpedia
void COLED::drawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color)
{
	int16_t steep = abs(y1 - y0) > abs(x1 - x0);

	if (steep) {
		uint16_t temp = x0;
		x0 = y0;
		y0 = temp;

		temp = x1;
		x1 = y1;
		y1 = temp;
	}

	if (x0 > x1) {
		uint16_t temp = x0;
		x0 = x1;
		x1 = temp;

		temp = y0;
		y0 = y1;
		y1 = temp;
	}

	int16_t dx, dy;
	dx = x1 - x0;
	dy = abs(y1 - y0);

	int16_t err = dx / 2;
	int16_t ystep;

	if (y0 < y1)
		ystep = 1;
	else
		ystep = -1;

	for (; x0 <= x1; x0++) {
		if (steep)
			drawPixel(y0, x0, color);
		else
			drawPixel(x0, y0, color);
		err -= dy;

		if (err < 0) {
			y0 += ystep;
			err += dx;
		}
	}
}

void COLED::drawBitmap(uint16_t x, uint16_t y, const uint8_t* bitmap, uint16_t w, uint16_t h, uint16_t color)
{
	uint16_t byteWidth = (w + 7U) / 8U;

	for (uint16_t j = 0U; j < h; j++) {
		for (uint16_t i = 0U; i < w; i++) {
			if (*(bitmap + j * byteWidth + i / 8) & (128 >> (i & 7)))
				drawPixel(x + i, y + j, color);
		}
	}
}

// the most basic function, set a single pixel
void COLED::drawPixel(uint16_t x, uint16_t y, uint16_t color)
{
	if ((x < 0) || (x >= m_width) || (y < 0) || (y >= m_height))
		return;

	if (m_displayType == OLED_SEEED_I2C_96x96) {
		// Get where to do the change in the buffer
		uint8_t* p = m_oledBuffer + (x + (y / 2) * m_width);

		// Get old value to not touch the other nible
		uint8_t c = *p;

		// We are on High nible ?
		if (((y / 2) & 1) == 1) {
			c &= 0x0F;
			c |= (color == WHITE ? m_grayH : 0x00) << 4;
		} else {
			c &= 0xF0;
			c |= (color == WHITE ? m_grayL : 0x00);
		}

		// set new nible value leaving the other untouched
		*p = c;
	} else {
		// Get where to do the change in the buffer
		uint8_t* p = m_oledBuffer + (x + (y / 8) * m_width);

		// x is which column
		if (color == WHITE)
			*p |= _BV((y % 8));
		else
			*p &= ~_BV((y % 8));
	}
}

// the print function
void COLED::print(const char* text)
{
	const char* p = text;

	while (*p != 0x00U)
		write(uint8_t(*p++));
}

void COLED::display()
{
	if (m_displayType == OLED_SEEED_I2C_96x96) {
		m_port->sendCommand(SSD1327_Set_Row_Address,    0x00U, 0x5FU);
		m_port->sendCommand(SSD1327_Set_Column_Address, 0x08U, 0x37U);
	} else {
		m_port->sendCommand(SSD1306_Set_Lower_Column_Start_Address | 0x0); // low col = 0
		m_port->sendCommand(SSD1306_Set_Higher_Column_Start_Address | 0x0); // hi col = 0
		m_port->sendCommand(SSD1306_Set_Start_Line | 0x0); // line #0
	}

	// pointer to OLED data buffer
	uint8_t* p = m_oledBuffer;

	if (m_displayType == OLED_ADAFRUIT_SPI_128x32 ||
		m_displayType == OLED_ADAFRUIT_SPI_128x64) {
		m_port->setDataMode();

		// Send all data to OLED
		for (uint16_t i = 0U; i < m_oledBufferSize; i++)
			m_port->writeData(*p++);

		// I wonder why we have to do this (check datasheet)
		if (m_height == 32U) {
			for (uint16_t i = 0U; i < m_oledBufferSize; i++)
				m_port->writeData(0x00U);
		}
	} else {
		uint8_t buff[17U];

		// Setup D/C to switch to data mode
		buff[0] = SSD_Data_Mode;

		if (m_displayType == OLED_SH1106_I2C_128x64) {
			for (uint8_t k = 0U; k < 8U; k++) {
				m_port->sendCommand(0xB0U + k);//set page addressSSD_Data_Mode;
				m_port->sendCommand(0x02U);//set lower column address
				m_port->sendCommand(0x10U);//set higher column address

				for (uint8_t i = 0U; i < 8U; i++) {
					for (uint8_t x = 1U; x <= 16U; x++)
						buff[x] = *p++;

					m_port->writeData(buff, 17U);
				}
			}
		} else {
			// loop trough all OLED buffer and 
			// send a bunch of 16 data byte in one xmission
			for (uint16_t i = 0U; i < m_oledBufferSize; i += 16U) {
				for (uint8_t x = 1U; x <= 16U; x++)
					buff[x] = *p++;

				m_port->writeData(buff, 17U);
			}
		}
	}
}

size_t COLED::write(uint8_t c)
{
	if (c == '\n') {
		m_cursorY += m_textSize * 8U;
		m_cursorX = 0U;
	} else if (c == '\r') {
		// skip em
	} else {
		drawChar(m_cursorX, m_cursorY, c, m_textColor, m_textBGColor, m_textSize);
		m_cursorX += m_textSize * 6U;

		if (m_textWrap && (m_cursorX > (m_width - m_textSize * 6U))) {
			m_cursorY += m_textSize * 8U;
			m_cursorX = 0U;
		}
	}

	return 1;
}

// draw a character
void COLED::drawChar(uint16_t x, uint16_t y, unsigned char c, uint16_t color, uint16_t bg, uint8_t size)
{
	if ((x >= m_width) || // Clip right
		(y >= m_height) || // Clip bottom
		((x + 5 * size - 1) < 0) || // Clip left
		((y + 8 * size - 1) < 0))   // Clip top
		return;

	for (uint8_t i = 0U; i < 6U; i++) {
		uint8_t line;
		if (i == 5U)
			line = 0x00U;
		else
			line = FONT[(c * 5) + i];

		for (uint8_t j = 0U; j < 8U; j++) {
			if (line & 0x1) {
				if (size == 1U) // default size
					drawPixel(x + i, y + j, color);
				else  // big size
					fillRect(x + (i * size), y + (j * size), size, size, color);
			} else if (bg != color) {
				if (size == 1U) // default size
					drawPixel(x + i, y + j, bg);
				else
					// big size
					fillRect(x + i * size, y + j * size, size, size, bg);
			}

			line >>= 1;
		}
	}
}

void COLED::begin()
{
	uint8_t vccType = SSD_Internal_Vcc;
	if (m_displayType == OLED_SEEED_I2C_128x64)
		vccType = SSD_External_Vcc;

	// depends on OLED type configuration
	uint8_t multiplex;
	uint8_t compins;
	uint8_t contrast;
	if (m_height == 32U) {
		multiplex = 0x1FU;
		compins   = 0x02U;
		contrast  = 0x8FU;
	} else {
		if (m_displayType == OLED_SEEED_I2C_96x96) {
			multiplex = 0x5FU;
			compins   = 0x12U;
			contrast  = 0x53U;
		} else {
			// So 128x64
			multiplex = 0x3FU;
			compins = 0x12U;

			if (m_displayType == OLED_SH1106_I2C_128x64)
				contrast = 0x80U;
			else
				contrast = (vccType == SSD_External_Vcc ? 0x9FU : 0xCFU);
		}
	}

	uint8_t chargepump;
	uint8_t precharge;
	if (vccType == SSD_External_Vcc) {
		chargepump = 0x10U;
		precharge  = 0x22U;
	} else {
		chargepump = 0x14U;
		precharge  = 0xF1U;
	}

	if (m_displayType == OLED_SEEED_I2C_96x96)
		m_port->sendCommand(SSD1327_Set_Command_Lock, 0x12U); // Unlock OLED driver IC MCU interface from entering command. i.e: Accept commands

	m_port->sendCommand(SSD_Display_Off);
	m_port->sendCommand(SSD_Set_Muliplex_Ratio, multiplex);

	if (m_displayType == OLED_SEEED_I2C_96x96) {
		m_port->sendCommand(SSD1327_Set_Display_Clock_Div, 0x01U);
		m_port->sendCommand(SSD1327_Set_Display_Start_Line, 0U);
		m_port->sendCommand(SSD1327_Set_Display_Offset, 96U);
		m_port->sendCommand(SSD_Set_Segment_Remap, 0x46U);

		m_port->sendCommand(0xABU); // set vdd internal
		m_port->sendCommand(0x01U); //

		m_port->sendCommand(0xB1U); // Set Phase Length
		m_port->sendCommand(0X51U); //

		m_port->sendCommand(0xB9U); //

		m_port->sendCommand(0xBCU); // set pre_charge voltage/VCOMH
		m_port->sendCommand(0x08U); // (0x08);

		m_port->sendCommand(0xBEU); // set VCOMH
		m_port->sendCommand(0X07U); // (0x07);

		m_port->sendCommand(0xB6U); // Set second pre-charge period
		m_port->sendCommand(0x01U); //

		m_port->sendCommand(0xD5U); // enable second precharge and enternal vsl
		m_port->sendCommand(0X62U); // (0x62);

		// Set Normal Display Mode
		m_port->sendCommand(SSD1327_Normal_Display);

		// Row Address
		// Start 0 End 95
		m_port->sendCommand(SSD1327_Set_Row_Address, 0U, 95U);

		// Column Address
		// Start from 8th Column of driver IC. This is 0th Column for OLED 
		// End at  (8 + 47)th column. Each Column has 2 pixels(segments)
		m_port->sendCommand(SSD1327_Set_Column_Address, 8U, 0x37U);

		// Map to horizontal mode
		m_port->sendCommand(0xA0U); // remap to
		m_port->sendCommand(0x46U); // Vertical mode

		// Init gray level for text. Default:Brightest White
		m_grayH = 0xF0U;
		m_grayL = 0x0FU;
	} else if (m_displayType == OLED_SH1106_I2C_128x64) {
		m_port->sendCommand(SSD1306_Set_Lower_Column_Start_Address | 0x02U); /*set lower column address*/
		m_port->sendCommand(SSD1306_Set_Higher_Column_Start_Address);     /*set higher column address*/
		m_port->sendCommand(SSD1306_Set_Start_Line);                      /*set display start line*/
		m_port->sendCommand(SH1106_Set_Page_Address);    /*set page address*/
		m_port->sendCommand(SSD_Set_Segment_Remap | 0x01); /*set segment remap*/
		m_port->sendCommand(SSD1306_Normal_Display);     /*normal / reverse*/
		m_port->sendCommand(0xADU);    /*set charge pump enable*/
		m_port->sendCommand(0x8BU);    /*external VCC   */
		m_port->sendCommand(0x30U);    /*0X30---0X33  set VPP   9V liangdu!!!!*/
		m_port->sendCommand(SSD1306_Set_Com_Output_Scan_Direction_Remap);    /*Com scan direction*/
		m_port->sendCommand(SSD1306_Set_Display_Offset);    /*set display offset*/
		m_port->sendCommand(0x00U);   /*   0x20  */
		m_port->sendCommand(SSD1306_Set_Display_Clock_Div);    /*set osc division*/
		m_port->sendCommand(0x80U);
		m_port->sendCommand(SSD1306_Set_Precharge_Period);    /*set pre-charge period*/
		m_port->sendCommand(0x1FU);    /*0x22*/
		m_port->sendCommand(SSD1306_Set_Com_Pins);    /*set COM pins*/
		m_port->sendCommand(0x12U);
		m_port->sendCommand(SSD1306_Set_Vcomh_Deselect_Level);    /*set vcomh*/
		m_port->sendCommand(0x40U);
	} else {
		m_port->sendCommand(SSD1306_Charge_Pump_Setting, chargepump);
		m_port->sendCommand(SSD1306_Set_Memory_Mode, 0x00U);              // 0x20 0x0 act like ks0108
		m_port->sendCommand(SSD1306_Set_Display_Clock_Div, 0x80U);      // 0xD5 + the suggested ratio 0x80
		m_port->sendCommand(SSD1306_Set_Display_Offset, 0x00U);        // no offset
		m_port->sendCommand(SSD1306_Set_Start_Line | 0x0U);            // line #0
		// use this two commands to flip display
		m_port->sendCommand(SSD_Set_Segment_Remap | 0x1U);
		m_port->sendCommand(SSD1306_Set_Com_Output_Scan_Direction_Remap);

		m_port->sendCommand(SSD1306_Set_Com_Pins, compins);
		m_port->sendCommand(SSD1306_Set_Precharge_Period, precharge);
		m_port->sendCommand(SSD1306_Set_Vcomh_Deselect_Level, 0x40U); // 0x40 -> unknown value in datasheet
		m_port->sendCommand(SSD1306_Entire_Display_Resume);
		m_port->sendCommand(SSD1306_Normal_Display);         // 0xA6

		// Reset to default value in case of 
		// no reset pin available on OLED, 
		m_port->sendCommand(SSD_Set_Column_Address, 0U, 127U);
		m_port->sendCommand(SSD_Set_Page_Address, 0U, 7U);
	}

	m_port->sendCommand(SSD_Set_ContrastLevel, contrast);

	stopscroll();

	// Empty uninitialized buffer
	clearDisplay();

	// turn on oled panel
	m_port->sendCommand(SSD_Display_On);

	// wait 100ms
	CThread::sleep(100U);
}