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

#if defined(MODE_OLED)

#include "I2COLED.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 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};

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

  clear();
}

bool CI2COLED::init()
{
  bool ret = m_i2c.init();
  if (!ret)
    return false;

  // 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*/

  return true;
}

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

  switch (MMDVM_STATE(state)) {
    case STATE_DSTAR:
      write(3U, "D-Star");
      break;
    case STATE_DMR:
      write(3U, "DMR");
      break;
    case STATE_YSF:
      write(3U, "YSF");
      break;
    case STATE_P25:
      write(3U, "P25");
      break;
    case STATE_NXDN:
      write(3U, "NXDN");
      break;
    case STATE_FM:
      write(3U, "FM");
      break;
    case STATE_M17:
      write(3U, "M17");
      break;
    default:
      write(3U, "Idle");
      break;
  }

  display();
}

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

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

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

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

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

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

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

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

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

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

  buff[0U] = OLED_ADDRESS;
  buff[1U] = SSD_Data_Mode;

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

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

void CI2COLED::write(uint8_t scale, const char* text)
{
}

void CI2COLED::display()
{
}

#endif