/*
 * MIT License

Copyright (c) 2017 DeeplyEmbedded

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

 * SSD1306_OLED.c
 *
 *  Created on  : Sep 26, 2017
 *  Author      : Vinay Divakar
 *  Description : SSD1306 OLED Driver, Graphics API's.
 *  Website     : www.deeplyembedded.org
 */

/* Lib Includes */
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <unistd.h>

#include "ssd1306.hpp"

/* MACROS */
#define PGM_READ_BYTE(addr) (*TO_CONST_UCHAR_STR(addr))
#define PGM_READ_WORD(addr) (*reinterpret_cast<const unsigned long*>(addr))
#define PGM_READ_DWORD(addr) (*reinterpret_cast<const unsigned long*>(addr))
#define PGM_READ_POINTER(addr) (reinterpret_cast<void*>PGM_READ_WORD(addr))


const unsigned char ssd1306::font[] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x5B, 0x4F, 0x5B, 0x3E,
    0x3E, 0x6B, 0x4F, 0x6B, 0x3E, 0x1C, 0x3E, 0x7C, 0x3E, 0x1C,
    0x18, 0x3C, 0x7E, 0x3C, 0x18, 0x1C, 0x57, 0x7D, 0x57, 0x1C,
    0x1C, 0x5E, 0x7F, 0x5E, 0x1C, 0x00, 0x18, 0x3C, 0x18, 0x00,
    0xFF, 0xE7, 0xC3, 0xE7, 0xFF, 0x00, 0x18, 0x24, 0x18, 0x00,
    0xFF, 0xE7, 0xDB, 0xE7, 0xFF, 0x30, 0x48, 0x3A, 0x06, 0x0E,
    0x26, 0x29, 0x79, 0x29, 0x26, 0x40, 0x7F, 0x05, 0x05, 0x07,
    0x40, 0x7F, 0x05, 0x25, 0x3F, 0x5A, 0x3C, 0xE7, 0x3C, 0x5A,
    0x7F, 0x3E, 0x1C, 0x1C, 0x08, 0x08, 0x1C, 0x1C, 0x3E, 0x7F,
    0x14, 0x22, 0x7F, 0x22, 0x14, 0x5F, 0x5F, 0x00, 0x5F, 0x5F,
    0x06, 0x09, 0x7F, 0x01, 0x7F, 0x00, 0x66, 0x89, 0x95, 0x6A,
    0x60, 0x60, 0x60, 0x60, 0x60, 0x94, 0xA2, 0xFF, 0xA2, 0x94,
    0x08, 0x04, 0x7E, 0x04, 0x08, 0x10, 0x20, 0x7E, 0x20, 0x10,
    0x08, 0x08, 0x2A, 0x1C, 0x08, 0x08, 0x1C, 0x2A, 0x08, 0x08,
    0x1E, 0x10, 0x10, 0x10, 0x10, 0x0C, 0x1E, 0x0C, 0x1E, 0x0C,
    0x30, 0x38, 0x3E, 0x38, 0x30, 0x06, 0x0E, 0x3E, 0x0E, 0x06,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x5F, 0x00, 0x00,
    0x00, 0x07, 0x00, 0x07, 0x00, 0x14, 0x7F, 0x14, 0x7F, 0x14,
    0x24, 0x2A, 0x7F, 0x2A, 0x12, 0x23, 0x13, 0x08, 0x64, 0x62,
    0x36, 0x49, 0x56, 0x20, 0x50, 0x00, 0x08, 0x07, 0x03, 0x00,
    0x00, 0x1C, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 0x1C, 0x00,
    0x2A, 0x1C, 0x7F, 0x1C, 0x2A, 0x08, 0x08, 0x3E, 0x08, 0x08,
    0x00, 0x80, 0x70, 0x30, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08,
    0x00, 0x00, 0x60, 0x60, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02,
    0x3E, 0x51, 0x49, 0x45, 0x3E, 0x00, 0x42, 0x7F, 0x40, 0x00,
    0x72, 0x49, 0x49, 0x49, 0x46, 0x21, 0x41, 0x49, 0x4D, 0x33,
    0x18, 0x14, 0x12, 0x7F, 0x10, 0x27, 0x45, 0x45, 0x45, 0x39,
    0x3C, 0x4A, 0x49, 0x49, 0x31, 0x41, 0x21, 0x11, 0x09, 0x07,
    0x36, 0x49, 0x49, 0x49, 0x36, 0x46, 0x49, 0x49, 0x29, 0x1E,
    0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x40, 0x34, 0x00, 0x00,
    0x00, 0x08, 0x14, 0x22, 0x41, 0x14, 0x14, 0x14, 0x14, 0x14,
    0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 0x01, 0x59, 0x09, 0x06,
    0x3E, 0x41, 0x5D, 0x59, 0x4E, 0x7C, 0x12, 0x11, 0x12, 0x7C,
    0x7F, 0x49, 0x49, 0x49, 0x36, 0x3E, 0x41, 0x41, 0x41, 0x22,
    0x7F, 0x41, 0x41, 0x41, 0x3E, 0x7F, 0x49, 0x49, 0x49, 0x41,
    0x7F, 0x09, 0x09, 0x09, 0x01, 0x3E, 0x41, 0x41, 0x51, 0x73,
    0x7F, 0x08, 0x08, 0x08, 0x7F, 0x00, 0x41, 0x7F, 0x41, 0x00,
    0x20, 0x40, 0x41, 0x3F, 0x01, 0x7F, 0x08, 0x14, 0x22, 0x41,
    0x7F, 0x40, 0x40, 0x40, 0x40, 0x7F, 0x02, 0x1C, 0x02, 0x7F,
    0x7F, 0x04, 0x08, 0x10, 0x7F, 0x3E, 0x41, 0x41, 0x41, 0x3E,
    0x7F, 0x09, 0x09, 0x09, 0x06, 0x3E, 0x41, 0x51, 0x21, 0x5E,
    0x7F, 0x09, 0x19, 0x29, 0x46, 0x26, 0x49, 0x49, 0x49, 0x32,
    0x03, 0x01, 0x7F, 0x01, 0x03, 0x3F, 0x40, 0x40, 0x40, 0x3F,
    0x1F, 0x20, 0x40, 0x20, 0x1F, 0x3F, 0x40, 0x38, 0x40, 0x3F,
    0x63, 0x14, 0x08, 0x14, 0x63, 0x03, 0x04, 0x78, 0x04, 0x03,
    0x61, 0x59, 0x49, 0x4D, 0x43, 0x00, 0x7F, 0x41, 0x41, 0x41,
    0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x41, 0x41, 0x41, 0x7F,
    0x04, 0x02, 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40,
    0x00, 0x03, 0x07, 0x08, 0x00, 0x20, 0x54, 0x54, 0x78, 0x40,
    0x7F, 0x28, 0x44, 0x44, 0x38, 0x38, 0x44, 0x44, 0x44, 0x28,
    0x38, 0x44, 0x44, 0x28, 0x7F, 0x38, 0x54, 0x54, 0x54, 0x18,
    0x00, 0x08, 0x7E, 0x09, 0x02, 0x18, 0xA4, 0xA4, 0x9C, 0x78,
    0x7F, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7D, 0x40, 0x00,
    0x20, 0x40, 0x40, 0x3D, 0x00, 0x7F, 0x10, 0x28, 0x44, 0x00,
    0x00, 0x41, 0x7F, 0x40, 0x00, 0x7C, 0x04, 0x78, 0x04, 0x78,
    0x7C, 0x08, 0x04, 0x04, 0x78, 0x38, 0x44, 0x44, 0x44, 0x38,
    0xFC, 0x18, 0x24, 0x24, 0x18, 0x18, 0x24, 0x24, 0x18, 0xFC,
    0x7C, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x24,
    0x04, 0x04, 0x3F, 0x44, 0x24, 0x3C, 0x40, 0x40, 0x20, 0x7C,
    0x1C, 0x20, 0x40, 0x20, 0x1C, 0x3C, 0x40, 0x30, 0x40, 0x3C,
    0x44, 0x28, 0x10, 0x28, 0x44, 0x4C, 0x90, 0x90, 0x90, 0x7C,
    0x44, 0x64, 0x54, 0x4C, 0x44, 0x00, 0x08, 0x36, 0x41, 0x00,
    0x00, 0x00, 0x77, 0x00, 0x00, 0x00, 0x41, 0x36, 0x08, 0x00,
    0x02, 0x01, 0x02, 0x04, 0x02, 0x3C, 0x26, 0x23, 0x26, 0x3C,
    0x1E, 0xA1, 0xA1, 0x61, 0x12, 0x3A, 0x40, 0x40, 0x20, 0x7A,
    0x38, 0x54, 0x54, 0x55, 0x59, 0x21, 0x55, 0x55, 0x79, 0x41,
    0x22, 0x54, 0x54, 0x78, 0x42, 0x21, 0x55, 0x54, 0x78, 0x40,
    0x20, 0x54, 0x55, 0x79, 0x40, 0x0C, 0x1E, 0x52, 0x72, 0x12,
    0x39, 0x55, 0x55, 0x55, 0x59, 0x39, 0x54, 0x54, 0x54, 0x59,
    0x39, 0x55, 0x54, 0x54, 0x58, 0x00, 0x00, 0x45, 0x7C, 0x41,
    0x00, 0x02, 0x45, 0x7D, 0x42, 0x00, 0x01, 0x45, 0x7C, 0x40,
    0x7D, 0x12, 0x11, 0x12, 0x7D, 0xF0, 0x28, 0x25, 0x28, 0xF0,
    0x7C, 0x54, 0x55, 0x45, 0x00, 0x20, 0x54, 0x54, 0x7C, 0x54,
    0x7C, 0x0A, 0x09, 0x7F, 0x49, 0x32, 0x49, 0x49, 0x49, 0x32,
    0x3A, 0x44, 0x44, 0x44, 0x3A, 0x32, 0x4A, 0x48, 0x48, 0x30,
    0x3A, 0x41, 0x41, 0x21, 0x7A, 0x3A, 0x42, 0x40, 0x20, 0x78,
    0x00, 0x9D, 0xA0, 0xA0, 0x7D, 0x3D, 0x42, 0x42, 0x42, 0x3D,
    0x3D, 0x40, 0x40, 0x40, 0x3D, 0x3C, 0x24, 0xFF, 0x24, 0x24,
    0x48, 0x7E, 0x49, 0x43, 0x66, 0x2B, 0x2F, 0xFC, 0x2F, 0x2B,
    0xFF, 0x09, 0x29, 0xF6, 0x20, 0xC0, 0x88, 0x7E, 0x09, 0x03,
    0x20, 0x54, 0x54, 0x79, 0x41, 0x00, 0x00, 0x44, 0x7D, 0x41,
    0x30, 0x48, 0x48, 0x4A, 0x32, 0x38, 0x40, 0x40, 0x22, 0x7A,
    0x00, 0x7A, 0x0A, 0x0A, 0x72, 0x7D, 0x0D, 0x19, 0x31, 0x7D,
    0x26, 0x29, 0x29, 0x2F, 0x28, 0x26, 0x29, 0x29, 0x29, 0x26,
    0x30, 0x48, 0x4D, 0x40, 0x20, 0x38, 0x08, 0x08, 0x08, 0x08,
    0x08, 0x08, 0x08, 0x08, 0x38, 0x2F, 0x10, 0xC8, 0xAC, 0xBA,
    0x2F, 0x10, 0x28, 0x34, 0xFA, 0x00, 0x00, 0x7B, 0x00, 0x00,
    0x08, 0x14, 0x2A, 0x14, 0x22, 0x22, 0x14, 0x2A, 0x14, 0x08,
    0x55, 0x00, 0x55, 0x00, 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA,
    0xFF, 0x55, 0xFF, 0x55, 0xFF, 0x00, 0x00, 0x00, 0xFF, 0x00,
    0x10, 0x10, 0x10, 0xFF, 0x00, 0x14, 0x14, 0x14, 0xFF, 0x00,
    0x10, 0x10, 0xFF, 0x00, 0xFF, 0x10, 0x10, 0xF0, 0x10, 0xF0,
    0x14, 0x14, 0x14, 0xFC, 0x00, 0x14, 0x14, 0xF7, 0x00, 0xFF,
    0x00, 0x00, 0xFF, 0x00, 0xFF, 0x14, 0x14, 0xF4, 0x04, 0xFC,
    0x14, 0x14, 0x17, 0x10, 0x1F, 0x10, 0x10, 0x1F, 0x10, 0x1F,
    0x14, 0x14, 0x14, 0x1F, 0x00, 0x10, 0x10, 0x10, 0xF0, 0x00,
    0x00, 0x00, 0x00, 0x1F, 0x10, 0x10, 0x10, 0x10, 0x1F, 0x10,
    0x10, 0x10, 0x10, 0xF0, 0x10, 0x00, 0x00, 0x00, 0xFF, 0x10,
    0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0xFF, 0x10,
    0x00, 0x00, 0x00, 0xFF, 0x14, 0x00, 0x00, 0xFF, 0x00, 0xFF,
    0x00, 0x00, 0x1F, 0x10, 0x17, 0x00, 0x00, 0xFC, 0x04, 0xF4,
    0x14, 0x14, 0x17, 0x10, 0x17, 0x14, 0x14, 0xF4, 0x04, 0xF4,
    0x00, 0x00, 0xFF, 0x00, 0xF7, 0x14, 0x14, 0x14, 0x14, 0x14,
    0x14, 0x14, 0xF7, 0x00, 0xF7, 0x14, 0x14, 0x14, 0x17, 0x14,
    0x10, 0x10, 0x1F, 0x10, 0x1F, 0x14, 0x14, 0x14, 0xF4, 0x14,
    0x10, 0x10, 0xF0, 0x10, 0xF0, 0x00, 0x00, 0x1F, 0x10, 0x1F,
    0x00, 0x00, 0x00, 0x1F, 0x14, 0x00, 0x00, 0x00, 0xFC, 0x14,
    0x00, 0x00, 0xF0, 0x10, 0xF0, 0x10, 0x10, 0xFF, 0x10, 0xFF,
    0x14, 0x14, 0x14, 0xFF, 0x14, 0x10, 0x10, 0x10, 0x1F, 0x00,
    0x00, 0x00, 0x00, 0xF0, 0x10, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
    0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xFF, 0xFF, 0xFF, 0x00, 0x00,
    0x00, 0x00, 0x00, 0xFF, 0xFF, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F,
    0x38, 0x44, 0x44, 0x38, 0x44, 0xFC, 0x4A, 0x4A, 0x4A, 0x34,
    0x7E, 0x02, 0x02, 0x06, 0x06, 0x02, 0x7E, 0x02, 0x7E, 0x02,
    0x63, 0x55, 0x49, 0x41, 0x63, 0x38, 0x44, 0x44, 0x3C, 0x04,
    0x40, 0x7E, 0x20, 0x1E, 0x20, 0x06, 0x02, 0x7E, 0x02, 0x02,
    0x99, 0xA5, 0xE7, 0xA5, 0x99, 0x1C, 0x2A, 0x49, 0x2A, 0x1C,
    0x4C, 0x72, 0x01, 0x72, 0x4C, 0x30, 0x4A, 0x4D, 0x4D, 0x30,
    0x30, 0x48, 0x78, 0x48, 0x30, 0xBC, 0x62, 0x5A, 0x46, 0x3D,
    0x3E, 0x49, 0x49, 0x49, 0x00, 0x7E, 0x01, 0x01, 0x01, 0x7E,
    0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x44, 0x44, 0x5F, 0x44, 0x44,
    0x40, 0x51, 0x4A, 0x44, 0x40, 0x40, 0x44, 0x4A, 0x51, 0x40,
    0x00, 0x00, 0xFF, 0x01, 0x03, 0xE0, 0x80, 0xFF, 0x00, 0x00,
    0x08, 0x08, 0x6B, 0x6B, 0x08, 0x36, 0x12, 0x36, 0x24, 0x36,
    0x06, 0x0F, 0x09, 0x0F, 0x06, 0x00, 0x00, 0x18, 0x18, 0x00,
    0x00, 0x00, 0x10, 0x10, 0x00, 0x30, 0x40, 0xFF, 0x01, 0x01,
    0x00, 0x1F, 0x01, 0x01, 0x1E, 0x00, 0x19, 0x1D, 0x17, 0x12,
    0x00, 0x3C, 0x3C, 0x3C, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00
};

/****************************************************************
 * Function Name : clearDisplay
 * Description   : Clear the display memory buffer
 * Returns       : NONE.
 * Params        : NONE.
 ****************************************************************/
void ssd1306::clear_display()
{
    memset(screen_, 0x00, DISPLAY_BUFF_SIZE);
}

/****************************************************************
 * Function Name : display_Init_seq
 * Description   : Performs SSD1306 OLED Initialization Sequence
 * Returns       : NONE.
 * Params        : NONE.
 ****************************************************************/
void ssd1306::display_init_seq()
{
    /* Add the reset code, If needed */

    /* Send display OFF command */
    cmd(SSD1306_DISPLAY_OFF);
    /* Set display clock frequency */
    cmd(SSD1306_SET_DISP_CLK);
    /* Send display CLK command parameter */
    cmd(SSD1306_DISPCLK_DIV);
    /* Set display multiplex */
    cmd(SSD1306_SET_MULTIPLEX);
    /* Send display MULT command parameter */
    cmd(SSD1306_MULT_DAT);
    /* Set display OFFSET */
    cmd(SSD1306_SET_DISP_OFFSET);
    /* Send display OFFSET command parameter */
    cmd(SSD1306_DISP_OFFSET_VAL);
    /* Set display START LINE - Check this command if something weird happens */
    cmd(SSD1306_SET_DISP_START_LINE);
    /* Enable CHARGEPUMP*/
    cmd(SSD1306_CONFIG_CHARGE_PUMP);
    /* Send display CHARGEPUMP command parameter */
    cmd(SSD1306_CHARGE_PUMP_EN);
    /* Set display MEMORYMODE */
    cmd(SSD1306_SET_MEM_ADDR_MODE);
    /* Send display HORIZONTAL MEMORY ADDR MODE command parameter */
    cmd(SSD1306_HOR_MM);
    /* Set display SEG_REMAP */
    cmd(SSD1306_SEG_REMAP);
    /* Set display DIR */
    cmd(SSD1306_SET_COMSCANDEC);
    /* Set display COM */
    cmd(SSD1306_SET_COMPINS);
    /* Send display COM command parameter */
    cmd(SSD1306_CONFIG_COM_PINS);
    /* Set display CONTRAST */
    cmd(SSD1306_SET_CONTRAST);
    /* Send display CONTRAST command parameter */
    cmd(SSD1306_CONTRAST_VAL);
    /* Set display PRECHARGE */
    cmd(SSD1306_SET_PRECHARGE);
    /* Send display PRECHARGE command parameter */
    cmd(SSD1306_PRECHARGE_VAL);
    /* Set display VCOMH */
    cmd(SSD1306_SET_VCOMDETECT);
    /* Send display VCOMH command parameter */
    cmd(SSD1306_VCOMH_VAL);
    /* Set display ALL-ON */
    cmd(SSD1306_DISPLAYALLON_RESUME);
    /* Set display to NORMAL-DISPLAY */
    cmd(SSD1306_NORMAL_DISPLAY);
    /* Set display to DEACTIVATE_SCROLL */
    cmd(SSD1306_DEACTIVATE_SCROLL);
    /* Set display to TURN-ON */
    cmd(SSD1306_DISPLAYON);
}

/****************************************************************
 * Function Name : transfer
 * Description   : Transfer the frame buffer onto the display
 * Returns       : NONE.
 * Params        : NONE.
 ****************************************************************/
void ssd1306::transfer()
{
    data();
    for (auto i = 0; i < 1024; ) {
        for (auto& byte : chunk_)
            byte = screen_[i++];
        if (spi_.write(chunk_, 16))
            exit(1);
        memset(chunk_, 0x00, 16);
    }
}

/****************************************************************
 * Function Name : Display
 * Description   : 1. Resets the column and page addresses.
 *                 2. Displays the contents of the memory buffer.
 * Returns       : NONE.
 * Params        : NONE.
 * Note          : Each new form can be preceded by a clearDisplay.
 ****************************************************************/
void ssd1306::display()
{
    init_col_pg_addrs(SSD1306_COL_START_ADDR, SSD1306_COL_END_ADDR,
        SSD1306_PG_START_ADDR, SSD1306_PG_END_ADDR);
    transfer();
}

/****************************************************************
 * Function Name : Init_Col_PG_addrs
 * Description   : Sets the column and page, start and
 *                 end addresses.
 * Returns       : NONE.
 * Params        : @col_start_addr: Column start address
 *                 @col_end_addr: Column end address
 *                 @pg_start_addr: Page start address
 *                 @pg_end_addr: Page end address
 ****************************************************************/
void ssd1306::init_col_pg_addrs(unsigned char col_start_addr, unsigned char col_end_addr,
                       unsigned char pg_start_addr, unsigned char pg_end_addr)
{
    /* Send COLMN address setting command  */
    cmd(SSD1306_SET_COL_ADDR);
    /* Set COLMN start address */
    cmd(col_start_addr);
    /* Set COLMN end address */
    cmd(col_end_addr);
    /* Send PAGE address setting command  */
    cmd(SSD1306_PAGEADDR);
    /* Set PAGE start address */
    cmd(pg_start_addr);
    /* Set PAGE end address */
    cmd(pg_end_addr);
}

/****************************************************************
 * Function Name : setRotation
 * Description   : Set the display rotation
 * Returns       : NONE.
 * Params        : @x: Display rotation parameter
 ****************************************************************/
void ssd1306::set_rotation(unsigned char x)
{
    rotation_ = x & 3;
    switch(rotation_) {
    case 0:
    case 2:
        width_ = SSD1306_LCDWIDTH;
        height_ = SSD1306_LCDHEIGHT;
        break;
    case 1:
    case 3:
        width_ = SSD1306_LCDHEIGHT;
        height_ = SSD1306_LCDWIDTH;
        break;
    default:
        break;
    }
}

/****************************************************************
 * Function Name : startscrollright
 * Description   : Activate a right handed scroll for rows start
 *                 through stop
 * Returns       : NONE.
 * Params        : @start: Start location
 *                 @stop: Stop location
 * HINT.         : the display is 16 rows tall. To scroll the whole
 *                 display, run: display.scrollright(0x00, 0x0F)
 ****************************************************************/
void ssd1306::start_scroll_right(unsigned char start, unsigned char stop)
{
    /* Send SCROLL horizontal right command  */
    cmd(SSD1306_RIGHT_HORIZONTAL_SCROLL);
    cmd(0x00);
    cmd(start);
    cmd(0x00);
    cmd(stop);
    cmd(0x00);
    cmd(0xFF);

    /* Send SCROLL Activate command  */
    cmd(SSD1306_ACTIVATE_SCROLL);
}

/****************************************************************
 * Function Name : startscrollleft
 * Description   : Activate a left handed scroll for rows start
 *                 through stop
 * Returns       : NONE.
 * Params        : @start: Start location
 *                 @stop: Stop location
 * HINT.         : the display is 16 rows tall. To scroll the whole
 *                 display, run: display.scrollright(0x00, 0x0F)
 ****************************************************************/
void ssd1306::start_scroll_left(unsigned char start, unsigned char stop)
{
    /* Send SCROLL horizontal left command  */
    cmd(SSD1306_LEFT_HORIZONTAL_SCROLL);
    cmd(0x00);
    cmd(start);
    cmd(0x00);
    cmd(stop);
    cmd(0x00);
    cmd(0xFF);

    /* Send SCROLL Activate command  */
    cmd(SSD1306_ACTIVATE_SCROLL);
}

/****************************************************************
 * Function Name : startscrolldiagright
 * Description   : Activate a diagonal scroll for rows start
 *                 through stop
 * Returns       : NONE.
 * Params        : @start: Start location
 *                 @stop: Stop location
 * HINT.         : the display is 16 rows tall. To scroll the whole
 *                 display, run: display.scrollright(0x00, 0x0F)
 ****************************************************************/
void ssd1306::start_scroll_diag_right(unsigned char start, unsigned char stop)
{
    /* Send SCROLL diagonal right command  */
    cmd(SSD1306_SET_VERTICAL_SCROLL_AREA);
    cmd(0x00);
    cmd(SSD1306_LCDHEIGHT);
    cmd(SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL);
    cmd(0x00);
    cmd(start);
    cmd(0x00);
    cmd(stop);
    cmd(0x01);

    /* Send SCROLL Activate command  */
    cmd(SSD1306_ACTIVATE_SCROLL);
}

/****************************************************************
 * Function Name : startscrolldiagleft
 * Description   : Activate a diagonal scroll for rows start
 *                 through stop
 * Returns       : NONE.
 * Params        : @start: Start location
 *                 @stop: Stop location
 * HINT.         : the display is 16 rows tall. To scroll the whole
 *                 display, run: display.scrollright(0x00, 0x0F)
 ****************************************************************/
void ssd1306::start_scroll_diag_left(unsigned char start, unsigned char stop)
{
    /* Send SCROLL diagonal right command  */
    cmd(SSD1306_SET_VERTICAL_SCROLL_AREA);
    cmd(0x00);
    cmd(SSD1306_LCDHEIGHT);
    cmd(SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL);
    cmd(0x00);
    cmd(start);
    cmd(0x00);
    cmd(stop);
    cmd(0x01);

    /* Send SCROLL Activate command  */
    cmd(SSD1306_ACTIVATE_SCROLL);
}

/****************************************************************
 * Function Name : stopscroll
 * Description   : Stop scrolling
 * Returns       : NONE.
 * Params        : NONE.
 ****************************************************************/
void ssd1306::stop_scroll()
{
    cmd(SSD1306_DEACTIVATE_SCROLL);
}

/****************************************************************
 * Function Name : invertDisplay
 * Description   : Invert or Normalize the display
 * Returns       : NONE.
 * Params        : @i: 0x00 to Normal and 0x01 for Inverting
 ****************************************************************/
void ssd1306::invert_display(unsigned char i)
{
    cmd(i ? SSD1306_INVERTDISPLAY : SSD1306_NORMAL_DISPLAY);
}

/****************************************************************
 * Function Name : drawPixel
 * Description   : Draw a pixel
 * Returns       : -1 on error and 0 on success
 * Params        : @x: X - Co-ordinate
 *                 @y: Y - Co-ordinate
 *                 @color: Color
 ****************************************************************/
signed char ssd1306::draw_pixel(short x, short y, short color)
{
    /* Return if co-ordinates are out of display dimension's range */
    if (x < 0 || x >= width_ || y < 0 || y >= height_)
        return -1;
    switch (rotation_)
    {
    case 1:
        std::swap(x,y);
        x = width_ - x - 1;
        break;
    case 2:
        x = width_ - x - 1;
        y = height_ - y - 1;
        break;
    case 3:
        std::swap(x,y);
        y = height_ - y - 1;
        break;
    default:
        break;
    }

    /* x is the column */
    switch(color)
    {
    case WHITE:
        screen_[x + y/8 * SSD1306_LCDWIDTH] |= 1 << (y & 7);
        break;
    case BLACK:
        screen_[x + y/8 * SSD1306_LCDWIDTH] &= ~(1 << (y & 7));
        break;
    case INVERSE:
        screen_[x + y/8 * SSD1306_LCDWIDTH] ^= 1 << (y & 7);
        break;
    default:
        break;
    }
    return 0;
}

/****************************************************************
 * Function Name : writeLine
 * Description   : Bresenham's algorithm
 * Returns       : NONE
 * Params        : @x0: X0 Co-ordinate
 *                 @y0: Y0 Co-ordinate
 *                 @x1: X1 Co-ordinate
 *                 @y1: Y1 Co-ordinate
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::write_line(short x0, short y0, short x1, short y1, short color)
{
    auto steep = abs(y1 - y0) > abs(x1 - x0);
    if (steep) {
        std::swap(x0, y0);
        std::swap(x1, y1);
    }

    if (x0 > x1) {
        std::swap(x0, x1);
        std::swap(y0, y1);
    }
    short dx = x1 - x0;
    short dy = abs(y1 - y0);

    short err = dx / 2;
    short ystep = y0 < y1 ? 1 : -1;

    for (; x0 <= x1; ++x0) {
        if (steep) {
            draw_pixel(y0, x0, color);
        } else {
            draw_pixel(x0, y0, color);
        }
        err -= dy;
        if (err < 0) {
            y0 += ystep;
            err += dx;
        }
    }
}

/* (x,y) is topmost point; if unsure, calling function
should sort endpoints or call writeLine() instead */
void ssd1306::draw_fast_v_line(short x, short y,short h, short color)
{
    write_line(x, y, x, y + h - 1, color);
}

/* (x,y) is topmost point; if unsure, calling function
should sort endpoints or call writeLine() instead */
void ssd1306::write_fast_v_line(short x, short y, short h, short color)
{
    draw_fast_v_line(x, y, h, color);
}

/* (x,y) is leftmost point; if unsure, calling function
 should sort endpoints or call writeLine() instead */
void ssd1306::draw_fast_h_line(short x, short y,short w, short color)
{
    write_line(x, y, x + w - 1, y, color);
}

// (x,y) is leftmost point; if unsure, calling function
// should sort endpoints or call writeLine() instead
void ssd1306::write_fast_h_line(short x, short y, short w, short color)
{
    draw_fast_h_line(x, y, w, color);
}

/****************************************************************
 * Function Name : drawCircleHelper
 * Description   : Draw a....
 * Returns       : NONE
 * Params        : @x: X Co-ordinate
 *                 @y: Y Co-ordinate
 *                 @w: Width
 *                 @h: height
 *                 @r: Corner radius
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_circle_helper(short x0, short y0, short r, unsigned char cornername, short color)
{
    short f     = 1 - r;
    short dd_f_x = 1;
    short dd_f_y = -2 * r;
    short x     = 0;
    short y     = r;

    while (x<y) {
        if (f >= 0) {
            y--;
            dd_f_y += 2;
            f     += dd_f_y;
        }
        x++;
        dd_f_x += 2;
        f     += dd_f_x;
        if (cornername & 0x4) {
            draw_pixel(x0 + x, y0 + y, color);
            draw_pixel(x0 + y, y0 + x, color);
        }
        if (cornername & 0x2) {
            draw_pixel(x0 + x, y0 - y, color);
            draw_pixel(x0 + y, y0 - x, color);
        }
        if (cornername & 0x8) {
            draw_pixel(x0 - y, y0 + x, color);
            draw_pixel(x0 - x, y0 + y, color);
        }
        if (cornername & 0x1) {
            draw_pixel(x0 - y, y0 - x, color);
            draw_pixel(x0 - x, y0 - y, color);
        }
    }
}

/****************************************************************
 * Function Name : drawLine
 * Description   : Draw line between two points
 * Returns       : NONE
 * Params        : @x0: X0 Starting X Co-ordinate
 *                 @y0: Y0 Starting Y Co-ordinate
 *                 @x1: X1 Ending X Co-ordinate
 *                 @y1: Y1 Ending Y Co-ordinate
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_line(short x0, short y0, short x1, short y1, short color)
{
    if (x0 == x1) {
        if (y0 > y1)
            std::swap(y0, y1);
        draw_fast_v_line(x0, y0, y1 - y0 + 1, color);
    } else if (y0 == y1) {
        if (x0 > x1)
            std::swap(x0, x1);
        draw_fast_h_line(x0, y0, x1 - x0 + 1, color);
    } else {
        write_line(x0, y0, x1, y1, color);
    }
}

/****************************************************************
 * Function Name : drawRect
 * Description   : Draw a rectangle
 * Returns       : NONE
 * Params        : @x: Corner X Co-ordinate
 *                 @y: Corner Y Co-ordinate
 *                 @w: Width in pixels
 *                 @h: Height in pixels
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_rect(short x, short y, short w, short h, short color)
{
    //startWrite();
    write_fast_h_line(x, y, w, color);
    write_fast_h_line(x, y+h-1, w, color);
    write_fast_v_line(x, y, h, color);
    write_fast_v_line(x+w-1, y, h, color);
    //endWrite();
}

/****************************************************************
 * Function Name : fillRect
 * Description   : Fill the rectangle
 * Returns       : NONE
 * Params        : @x: Starting X Co-ordinate
 *                 @y: Starting Y Co-ordinate
 *                 @w: Width in pixels
 *                 @h: Height in pixels
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::fill_rect(short x, short y, short w, short h, short color)
{
    //startWrite();
    for (auto i = x; i < x + w; ++i) {
        write_fast_v_line(i, y, h, color);
    }
    //endWrite();
}

/****************************************************************
 * Function Name : drawCircle
 * Description   : Draw a circle
 * Returns       : NONE
 * Params        : @x: Center X Co-ordinate
 *                 @y: Center Y Co-ordinate
 *                 @r: Radius in pixels
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_circle(short x0, short y0, short r, short color)
{
    short f = 1 - r;
    short dd_f_x = 1;
    short dd_f_y = -2 * r;
    short x = 0;
    short y = r;

    //startWrite();
    draw_pixel(x0    , y0 + r, color);
    draw_pixel(x0    , y0 - r, color);
    draw_pixel(x0 + r, y0    , color);
    draw_pixel(x0 - r, y0    , color);

    while (x < y) {
        if (f >= 0) {
            y--;
            dd_f_y += 2;
            f += dd_f_y;
        }
        x++;
        dd_f_x += 2;
        f += dd_f_x;

        draw_pixel(x0 + x, y0 + y, color);
        draw_pixel(x0 - x, y0 + y, color);
        draw_pixel(x0 + x, y0 - y, color);
        draw_pixel(x0 - x, y0 - y, color);
        draw_pixel(x0 + y, y0 + x, color);
        draw_pixel(x0 - y, y0 + x, color);
        draw_pixel(x0 + y, y0 - x, color);
        draw_pixel(x0 - y, y0 - x, color);
    }
}

/****************************************************************
 * Function Name : fillCircleHelper
 * Description   : Used to do circles and roundrects
 * Returns       : NONE
 * Params        : @x: Center X Co-ordinate
 *                 @y: Center Y Co-ordinate
 *                 @r: Radius in pixels
 *                 @cornername: Corner radius in pixels
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::fill_circle_helper(short x0, short y0, short r, unsigned char cornername, short delta, short color)
{

    short f      = 1 - r;
    short dd_f_x = 1;
    short dd_f_y = -2 * r;
    short x      = 0;
    short y      = r;

    while (x<y) {
        if (f >= 0) {
            y--;
            dd_f_y += 2;
            f     += dd_f_y;
        }
        x++;
        dd_f_x += 2;
        f += dd_f_x;

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

/****************************************************************
 * Function Name : fillCircle
 * Description   : Fill the circle
 * Returns       : NONE
 * Params        : @x0: Center X Co-ordinate
 *                 @y0: Center Y Co-ordinate
 *                 @r: Radius in pixels
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::fill_circle(short x0, short y0, short r, short color)
{
    write_fast_v_line(x0, y0-r, 2*r+1, color);
    fill_circle_helper(x0, y0, r, 3, 0, color);
}

/****************************************************************
 * Function Name : drawTriangle
 * Description   : Draw a triangle
 * Returns       : NONE
 * Params        : @x0: Corner-1 X Co-ordinate
 *                 @y0: Corner-1 Y Co-ordinate
 *                 @x1: Corner-2 X Co-ordinate
 *                 @y1: Corner-2 Y Co-ordinate
 *                 @x2: Corner-3 X Co-ordinate
 *                 @y2: Corner-3 Y Co-ordinate
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_triangle(short x0, short y0, short x1, short y1, short x2, short y2, short color)
{
    draw_line(x0, y0, x1, y1, color);
    draw_line(x1, y1, x2, y2, color);
    draw_line(x2, y2, x0, y0, color);
}

/****************************************************************
 * Function Name : fillTriangle
 * Description   : Fill a triangle
 * Returns       : NONE
 * Params        : @x0: Corner-1 X Co-ordinate
 *                 @y0: Corner-1 Y Co-ordinate
 *                 @x1: Corner-2 X Co-ordinate
 *                 @y1: Corner-2 Y Co-ordinate
 *                 @x2: Corner-3 X Co-ordinate
 *                 @y2: Corner-3 Y Co-ordinate
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::fill_triangle(short x0, short y0, short x1, short y1, short x2, short y2, short color)
{
    short a, b, y, last;

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

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

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

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

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

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

/****************************************************************
 * Function Name : drawRoundRect
 * Description   : Draw a rounded rectangle
 * Returns       : NONE
 * Params        : @x: X Co-ordinate
 *                 @y: Y Co-ordinate
 *                 @w: Width
 *                 @h: height
 *                 @r: Corner radius
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_round_rect(short x, short y, short w, short h, short r, short color)
{
    // smarter version
    write_fast_h_line(x + r    , y        , w - 2 * r, color); // Top
    write_fast_h_line(x + r    , y + h - 1, w - 2 * r, color); // Bottom
    write_fast_v_line(x        , y + r    , h - 2 * r, color); // Left
    write_fast_v_line(x + w - 1, y + r    , h - 2 * r, color); // Right
    // draw four corners
    draw_circle_helper(x + r        , y + r        , r, 1, color);
    draw_circle_helper(x + w - r - 1, y + r        , r, 2, color);
    draw_circle_helper(x + w - r - 1, y + h - r - 1, r, 4, color);
    draw_circle_helper(x + r        , y + h - r - 1, r, 8, color);
}

/****************************************************************
 * Function Name : fillRoundRect
 * Description   :  Fill a rounded rectangle
 * Returns       : NONE
 * Params        : @x: X Co-ordinate
 *                 @y: Y Co-ordinate
 *                 @w: Width
 *                 @h: height
 *                 @r: Corner radius
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::fill_round_rect(short x, short y, short w, short h, short r, short color)
{
    // smarter version
    fill_rect(x + r, y, w - 2 * r, h, color);

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

/*----------------------------------------------------------------------------
 * BITMAP API's
 ----------------------------------------------------------------------------*/

/****************************************************************
 * Function Name : drawBitmap
 * Description   : Draw a bitmap
 * Returns       : NONE
 * Params        : @x: X Co-ordinate
 *                 @y: Y Co-ordinate
 *                 @bitmap: bitmap to display
 *                 @w: Width
 *                 @h: height
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_bitmap(short x, short y, const unsigned char* bitmap, short w, short h, short color)
{
    unsigned char byte = 0;
    short byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

    for (auto j=0; j<h; j++, y++) {
        for (auto i=0; i<w; i++) {
            if (i & 7)
                byte <<= 1;
            else
                byte   = PGM_READ_BYTE(&bitmap[j * byteWidth + i / 8]);
            if (byte & 0x80)
                draw_pixel(x+i, y, color);
        }
    }
}

/*----------------------------------------------------------------------------
 * TEXT AND CHARACTER HANDLING API's
 ----------------------------------------------------------------------------*/

/****************************************************************
 * Function Name : setCursor
 * Description   : Sets the cursor on f(x,y)
 * Returns       : NONE.
 * Params        : @x - X-Cordinate
 *                 @y - Y-Cordinate
 ****************************************************************/
void ssd1306::set_cursor(short x, short y)
{
    cursor_x_ = x;
    cursor_y_ = y;
}

/****************************************************************
 * Function Name : getCursorX
 * Description   : Get cursor at X- Cordinate
 * Returns       : x cordinate value.
 ****************************************************************/
short ssd1306::get_cursor_x() const
{
    return cursor_x_;
}

/****************************************************************
 * Function Name : getCursorY
 * Description   : Get cursor at Y- Cordinate
 * Returns       : y cordinate value.
 ****************************************************************/
short ssd1306::get_cursor_y() const
{
    return cursor_y_;
}

/****************************************************************
 * Function Name : setTextSize
 * Description   : Set text size
 * Returns       : @s - font size
 ****************************************************************/
void ssd1306::set_text_size(unsigned char s)
{
    textsize_ = s ? s : 1;
}

/****************************************************************
 * Function Name : setTextColor
 * Description   : Set text color
 * Returns       : @c - Color
 ****************************************************************/
void ssd1306::set_text_color(short c)
{
    // For 'transparent' background, we'll set the bg
    // to the same as fg instead of using a flag
    textcolor_ = textbgcolor_ = c;
}

/****************************************************************
 * Function Name : setTextWrap
 * Description   : Wraps the text
 * Returns       : @w - enable or disbale wrap
 ****************************************************************/
void ssd1306::set_text_wrap(bool w)
{
    wrap_ = w;
}

/****************************************************************
 * Function Name : getRotation
 * Description   : Get the rotation value
 * Returns       : NONE.
 ****************************************************************/
unsigned char ssd1306::get_rotation() const
{
    return rotation_;
}

/****************************************************************
 * Function Name : drawBitmap
 * Description   : Draw a character
 * Returns       : NONE
 * Params        : @x: X Co-ordinate
 *                 @y: Y Co-ordinate
 *                 @c: Character
 *                 @size: Scaling factor
 *                 @bg: Background color
 *                 @color: Pixel color
 ****************************************************************/
void ssd1306::draw_char(short x, short y, unsigned char c, short color, short bg, unsigned char size)
{
    if (!gfx_font_) {
        // 'Classic' built-in font
        if ((x >= width_) || (y >= height_) || ((x + 6 * size - 1) < 0) || ((y + 8 * size - 1) < 0))
            return;

        // Handle 'classic' charset behavior
        if (!cp437_ && (c >= 176))
            c++;

        // Char bitmap = 5 columns
        for (char i=0; i<5; i++ ) {
            auto line = PGM_READ_BYTE(&font[c * 5 + i]);
            for (char j=0; j<8; j++, line >>= 1) {
                if (line & 1) {
                    if (size == 1)
                        draw_pixel(x+i, y+j, color);
                    else
                        fill_rect(x+i*size, y+j*size, size, size, color);
                } else if (bg != color) {
                    if (size == 1)
                        draw_pixel(x+i, y+j, bg);
                    else
                        fill_rect(x+i*size, y+j*size, size, size, bg);
                }
            }
        }

        // If opaque, draw vertical line for last column
        if (bg != color) {
            if (size == 1)
                write_fast_v_line(x+5, y, 8, bg);
            else
                fill_rect(x+5*size, y, size, 8*size, bg);
        }

    } else {
        // Character is assumed previously filtered by write() to eliminate
        // newlines, returns, non-printable characters, etc.  Calling
        // drawChar() directly with 'bad' characters of font may cause mayhem!

        c -= static_cast<unsigned char>(PGM_READ_BYTE(&gfx_font_->first));
        auto glyph = &(static_cast<gf_xglyph_t *>(PGM_READ_POINTER(&gfx_font_->glyph))[c]);
        auto bitmap = static_cast<unsigned char *>(PGM_READ_POINTER(&gfx_font_->bitmap));
        short bo = PGM_READ_WORD(&glyph->bitmap_offset);
        auto w  = PGM_READ_BYTE(&glyph->width);
        auto h  = PGM_READ_BYTE(&glyph->height);
        char xo = PGM_READ_BYTE(&glyph->x_offset);
        char yo = PGM_READ_BYTE(&glyph->y_offset);

        short xo16 = 0, yo16 = 0;
        if (size > 1) {
            xo16 = xo;
            yo16 = yo;
        }

        // Todo: Add character clipping here

        // NOTE: THERE IS NO 'BACKGROUND' COLOR OPTION ON CUSTOM FONTS.
        // THIS IS ON PURPOSE AND BY DESIGN.  The background color feature
        // has typically been used with the 'classic' font to overwrite old
        // screen contents with new data.  This ONLY works because the
        // characters are a uniform size; it's not a sensible thing to do with
        // proportionally-spaced fonts with glyphs of varying sizes (and that
        // may overlap).  To replace previously-drawn text when using a custom
        // font, use the getTextBounds() function to determine the smallest
        // rectangle encompassing a string, erase the area with fillRect(),
        // then draw new text.  This WILL unfortunately 'blink' the text, but
        // is unavoidable.  Drawing 'background' pixels will NOT fix this,
        // only creates a new set of problems.  Have an idea to work around
        // this (a canvas object type for MCUs that can afford the RAM and
        // displays supporting setAddrWindow() and pushColors()), but haven't
        // implemented this yet.
        unsigned char bits = 0, bit = 0;
        for (unsigned char yy = 0; yy < h; yy++) {
            for (unsigned char xx=0; xx<w; xx++) {
                if (!(bit++ & 7)) {
                    bits = PGM_READ_BYTE(&bitmap[bo++]);
                }
                if (bits & 0x80) {
                    if (size == 1) {
                        draw_pixel(x+xo+xx, y+yo+yy, color);
                    } else {
                        fill_rect(x+(xo16+xx)*size, y+(yo16+yy)*size,size, size, color);
                    }
                }
                bits <<= 1;
            }
        }
    } // End classic vs custom font
}

/****************************************************************
 * Function Name : write
 * Description   : Base function for text and character handling
 * Returns       : 1
 * Params        : @c: Character
 ****************************************************************/
short ssd1306::oled_write(unsigned char c)
{
    if (!gfx_font_) {
        // 'Classic' built-in font
        if (c == '\n') {
            // Newline?
            cursor_x_  = 0;                     // Reset x to zero,
            cursor_y_ += textsize_ * 8;          // advance y one line
        } else if (c != '\r') {
            // Ignore carriage returns
            if (wrap_ && cursor_x_ + textsize_ * 6 > width_) {
                // Off right?
                cursor_x_  = 0;                 // Reset x to zero,
                cursor_y_ += textsize_ * 8;      // advance y one line
            }
            draw_char(cursor_x_, cursor_y_, c, textcolor_, textbgcolor_, textsize_);
            cursor_x_ += textsize_ * 6;          // Advance x one char
        }

    } else {
        // Custom font
        if (c == '\n') {
            cursor_x_  = 0;
            cursor_y_ += static_cast<short>(textsize_) *
                static_cast<unsigned char>(PGM_READ_BYTE(&gfx_font_->y_advance));
        } else if (c != '\r') {
            auto first = PGM_READ_BYTE(&gfx_font_->first);
            if (c >= first && c <= static_cast<unsigned char>(PGM_READ_BYTE(&gfx_font_->last))) {
                auto glyph = &static_cast<gf_xglyph_t*>(PGM_READ_POINTER(&gfx_font_->glyph))[c - first];
                auto w     = PGM_READ_BYTE(&glyph->width);
                auto h     = PGM_READ_BYTE(&glyph->height);
                if (w > 0 && h > 0) {
                    // Is there an associated bitmap?
                    auto xo = static_cast<char>(PGM_READ_BYTE(&glyph->x_offset)); // sic
                    if (wrap_ && cursor_x_ + textsize_ * (xo + w) > width_)
                    {
                        cursor_x_  = 0;
                        cursor_y_ += static_cast<short>(textsize_) *static_cast<unsigned char>(PGM_READ_BYTE(&gfx_font_->y_advance));
                    }
                    draw_char(cursor_x_, cursor_y_, c, textcolor_, textbgcolor_, textsize_);
                }
                cursor_x_ += static_cast<unsigned char>(PGM_READ_BYTE(&glyph->x_advance)) * static_cast<short>(textsize_);
            }
        }
    }
    return 1;
}

/****************************************************************
 * Function Name : print
 * Description   : Base function for printing strings
 * Returns       : No. of characters printed
 * Params        : @buffer: Ptr to buffer containing the string
 *                 @size: Length of the string.
 ****************************************************************/
short ssd1306::print(const unsigned char *buffer, short size)
{
    short n = 0;
    while (size--) {
        if (oled_write(*buffer++))
            n++;
        else
            break;
    }
    return n;
}

ssd1306::ssd1306(
    unsigned bus, unsigned device,
    unsigned reset, unsigned dc) : spi_(bus, device), reset_(reset), dc_(dc)
{
    reset_.set_direction(gpio::output);
    reset_.set_value(gpio::low);
    usleep(50'000);
    reset_.set_value(gpio::high);
    dc_.set_direction(gpio::output);
    dc_.stream_open();
}

ssd1306::~ssd1306()
{
    dc_.stream_close();
}

/****************************************************************
 * Function Name : print_str
 * Description   : Print strings
 * Returns       : No. of characters printed
 * Params        : @strPtr: Ptr to buffer containing the string
 ****************************************************************/
short ssd1306::print_str(const char *strPtr)
{
    return print(TO_CONST_UCHAR_STR(strPtr), strlen(strPtr));
}

/****************************************************************
 * Function Name : println
 * Description   : Move to next line
 * Returns       : No. of characters printed
 * Params        : NONE.
 ****************************************************************/
short ssd1306::println()
{
    return print_str("\r\n");
}

/****************************************************************
 * Function Name : print_strln
 * Description   : Print strings and move to next line
 * Returns       : No. of characters printed
 * Params        : @strPtr: Ptr to buffer containing the string
 ****************************************************************/
short ssd1306::print_strln(const char *strPtr)
{
    auto n = print(TO_CONST_UCHAR_STR(strPtr), strlen(strPtr));
    n += print_str("\r\n");
    return n;
}

/*----------------------------------------------------------------------------
 * NUMBERS HANDLING API's
 ----------------------------------------------------------------------------*/

/****************************************************************
 * Function Name : printNumber
 * Description   : Base function to print unsigned numbers
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number(unsigned long n, unsigned char base)
{
    char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
    auto *str = &buf[sizeof(buf) - 1];

    *str = '\0';

    // prevent crash if called with base == 1
    if (base < 2)
        base = 10;
    do
    {
        auto m = n;
        n /= base;
        char c = m - base * n;
        *--str = c < 10 ? c + '0' : c + 'A' - 10;
    }
    while(n);
    //return oled_write((unsigned char)str);
    return print_str(str);
}

/****************************************************************
 * Function Name : printNumber_UL
 * Description   : Print unsigned long data types
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_ul(unsigned long n, int base)
{
    if (base == 0)
        return oled_write(n);
    return print_number(n, base);
}

/****************************************************************
 * Function Name : printNumber_UL_ln
 * Description   : Print unsigned long & advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_ul_ln(unsigned long num, int base)
{
    auto n = print_number(num, base);
    n += println();
    return n;
}

/****************************************************************
 * Function Name : printNumber_UI
 * Description   : Print unsigned int data types
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_ui(unsigned int n, int base)
{
    return print_number(static_cast<unsigned long>(n), base);
}

/****************************************************************
 * Function Name : printNumber_UI_ln
 * Description   : Print unsigned int & advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_ui_ln(unsigned int n, int base)
{
    auto a = print_number(static_cast<unsigned long>(n), base);
    a += println();
    return a;
}

/****************************************************************
 * Function Name : printNumber_UC
 * Description   : Print unsigned char data types
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_uc(unsigned char b, int base)
{
    return print_number(static_cast<unsigned long>(b), base);
}

/****************************************************************
 * Function Name : printNumber_UC_ln
 * Description   : Print unsigned char & advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_uc_ln(unsigned char b, int base)
{
    auto n = print_number(static_cast<unsigned long>(b), base);
    n += println();
    return n;
}

/****************************************************************
 * Function Name : printNumber_L
 * Description   : Print Long data types
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_l(long n, int base)
{
    if (base == 0) {
        return oled_write(n);
    }
    if (base == 10) {
        if (n < 0) {
            auto t = oled_write('-');
            n = -n;
            return print_number(n, 10) + t;
        }
        return print_number(n, 10);
    }
    return print_number(n, base);
}

/****************************************************************
 * Function Name : printNumber_UC_ln
 * Description   : Print long & advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_l_ln(long num, int base)
{
    auto n = print_number_l(num, base);
    n += println();
    return n;
}

/****************************************************************
 * Function Name : printNumber_I
 * Description   : Print int data types
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_i(int n, int base)
{
    return print_number_l(static_cast<long>(n), base);
}

/****************************************************************
 * Function Name : printNumber_I_ln
 * Description   : Print int & advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @base: Base e.g. HEX, BIN...
 ****************************************************************/
short ssd1306::print_number_i_ln(int n, int base)
{
    auto a = print_number_l(static_cast<long>(n), base);
    a += println();
    return a;
}

/****************************************************************
 * Function Name : printFloat
 * Description   : Print floating Pt. No's.
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @digits: Resolution
 ****************************************************************/
short ssd1306::print_float(double number, unsigned char digits)
{
    short n = 0;

    // Round correctly so that print(1.999, 2) prints as "2.00"
    auto rounding = 0.5;

    if (std::isnan(number))
        return print_str("nan");
    if (std::isinf(number))
        return print_str("inf");
    if (number > 4294967040.0)
        return print_str("ovf");  // constant determined empirically
    if (number < -4294967040.0)
        return print_str("ovf");  // constant determined empirically

    // Handle negative numbers
    if (number < 0.0) {
        n += oled_write('-');
        number = -number;
    }


    for (auto i = 0; i < digits; ++i)
        rounding /= 10.0;

    number += rounding;

    // Extract the integer part of the number and print it
    auto int_part = static_cast<unsigned long>(number);
    auto remainder = number - static_cast<double>(int_part);
    n += print_number_ul(int_part,DEC);

    // Print the decimal point, but only if there are digits beyond
    if (digits > 0) {
        n += print_str(".");
    }

    // Extract digits from the remainder one at a time
    while(digits-- > 0) {
        remainder *= 10.0;
        double to_print = static_cast<int>(remainder);
        n += print_number_i(to_print,DEC);
        remainder -= to_print;
    }
    return n;
}

/****************************************************************
 * Function Name : printFloat_ln
 * Description   : Print floating Pt. No and advance to next line
 * Returns       : No. of characters printed
 * Params        : @n: Number
 *                 @digits: Resolution
 ****************************************************************/
short ssd1306::print_float_ln(double num, int digits)
{
    auto n = print_float(num, digits);
    n += println();
    return n;
}