/*
 *   Copyright (C) 2020,2021 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 "Config.h"

#if defined(MODE_OLED)

#include "I2COLED.h"
#include "I2C3.h"

#include "Globals.h"

const uint8_t  OLED_ADDRESS = 0x78U;

const uint8_t  OLED_WIDTH  = 64U;
const uint8_t  OLED_HEIGHT = 32U;
const uint16_t OLED_BUFFER_SIZE = OLED_HEIGHT * OLED_WIDTH / 8U;

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 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 CHAR_HEIGHT = 5U;
const uint8_t CHAR_WIDTH  = 7U;

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

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



CI2COLED::CI2COLED() :
m_i2c(),
m_oledBuffer(NULL)
{
  m_oledBuffer = new uint8_t[OLED_BUFFER_SIZE];

  clear();
}

void CI2COLED::init()
{
  m_i2c.init();

  // Initialise the VG-6432TSWEG02 OLED display
  sendCommand(0xAEU);    /*display off*/

  sendCommand(0x00U);    /*set lower column address*/
  sendCommand(0x12U);    /*set higher column address*/

  sendCommand(0x00U);    /*set display start line*/

  sendCommand(0xB0U);    /*set page address*/

  sendCommand(0x81U, 0x41U);  /*contract control*/

  sendCommand(0xA1U);    /*set segment remap*/

  sendCommand(0xA6U);    /*normal / reverse*/

  sendCommand(0xA8U, 0x1FU);  /*multiplex ratio*/

  sendCommand(0xC8U);    /*Com scan direction*/

  sendCommand(0xD3U, 0x00U);  /*set display offset*/

  sendCommand(0xD5U, 0x80U);  /*set osc division*/

  sendCommand(0xD9U, 0x1FU);  /*set pre-charge period*/

  sendCommand(0xDAU, 0x12U);  /*set COM pins*/

  sendCommand(0xDBU, 0x40U);  /*set vcomh*/

  sendCommand(0x8DU, 0x10U);  /*set charge pump enable*/

  sendCommand(0xAFU);    /*display ON*/
}

void CI2COLED::setMode(int state)
{
  clear();

  switch (MMDVM_STATE(state)) {
    case STATE_IDLE:
      write("Idle");
      break;
    case STATE_DSTAR:
      write("D-Star");
      break;
    case STATE_DMR:
      write("DMR");
      break;
    case STATE_YSF:
      write("YSF");
      break;
    case STATE_P25:
      write("P25");
      break;
    case STATE_NXDN:
      write("NXDN");
      break;
    case STATE_FM:
      write("FM");
      break;
    case STATE_M17:
      write("M17");
      break;
    case STATE_POCSAG:
      write("POCSAG");
      break;
    case STATE_AX25:
      write("AX.25");
      break;
    default:
      write("Calibrate");
      break;
  }

  display();
}

void CI2COLED::sendCommand(uint8_t c0, uint8_t c1, uint8_t c2)
{
  uint8_t buff[4U];

  buff[0U] = SSD_Command_Mode;
  buff[1U] = c0;
  buff[2U] = c1;
  buff[3U] = c2;

  // Write Data on I2C
  m_i2c.write(OLED_ADDRESS, buff, 4U);
}

void CI2COLED::sendCommand(uint8_t c0, uint8_t c1)
{
  uint8_t buff[3U];

  buff[0U] = SSD_Command_Mode;
  buff[1U] = c0;
  buff[2U] = c1;

  // Write Data on I2C
  m_i2c.write(OLED_ADDRESS, buff, 3U);
}

void CI2COLED::sendCommand(uint8_t c)
{
  uint8_t buff[2U];

  buff[0U] = SSD_Command_Mode;
  buff[1U] = c;

  // Write Data on I2C
  m_i2c.write(OLED_ADDRESS, buff, 2U);
}

void CI2COLED::sendData(const uint8_t* data, uint16_t length)
{
  uint8_t buff[1U];

  buff[0U] = SSD_Data_Mode;

  // Write Data on I2C
  m_i2c.write(OLED_ADDRESS, buff, 1U);
  m_i2c.write(OLED_ADDRESS, data, length);
}

void CI2COLED::clear()
{
  ::memset(m_oledBuffer, 0x00U, OLED_BUFFER_SIZE);
}

void CI2COLED::write(const char* text)
{
  size_t len = ::strlen(text);

  uint16_t scale = OLED_WIDTH / (len * (CHAR_WIDTH + 1U));
  if (scale == 0U) scale = 1U;

  uint8_t charHeight = CHAR_HEIGHT * scale;
  uint8_t charWidth  = (CHAR_WIDTH + 1U) * scale;

  uint8_t x = (OLED_HEIGHT - charHeight) / 2U;
  uint8_t y = (OLED_WIDTH  - charWidth)  / 2U;

  for (size_t i = 0U; i < len; i++) {
    write(x, y, text[i], scale);
    y += charWidth;
  }
}

void CI2COLED::write(uint8_t x, uint8_t y, char c, uint8_t size)
{
  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);
        else  // big size
          fillRect(x + (i * size), y + (j * size), size, size);
      }

      line >>= 1;
    }
  }
}

void CI2COLED::fillRect(uint8_t x, uint8_t y, uint8_t width, uint8_t height)
{
  for (uint8_t i = x; i < (x + height - 1U); i++) {
    for (uint8_t j = y; j < (y + width - 1U); j++)
      drawPixel(i, j);
  }
}

void CI2COLED::drawPixel(uint8_t x, uint8_t y)
{
  // Get where to do the change in the buffer
  uint8_t* p = m_oledBuffer + (x + (y / 8U) * OLED_WIDTH);

  // x is which column
  *p |= _BV((y % 8));
}

void CI2COLED::display()
{
  sendCommand(SSD1306_Set_Lower_Column_Start_Address | 0x0); // low col = 0
  sendCommand(SSD1306_Set_Higher_Column_Start_Address | 0x0); // hi col = 0
  sendCommand(SSD1306_Set_Start_Line | 0x0); // line #0

  // loop trough all OLED buffer and 
  // send a bunch of 16 data byte in one xmission
  for (uint16_t i = 0U; i < OLED_BUFFER_SIZE; i += 16U)
    sendData(m_oledBuffer + i, 16U);
}

#endif