#include "ssd1306.h"
#include "font.h"
//#include "font8x8.h"

#include "ssd1306_i2c.h"

// @const List of init commands with arguments by Adafruit
// @link https://github.com/adafruit/Adafruit_SSD1306
const uint8_t INIT_SSD1306_ADAFRUIT[] = {
  17,                                                             // number of initializers
  SSD1306_DISPLAY_OFF, 0,                                         // 0xAE / Set Display OFF
  SSD1306_SET_OSC_FREQ, 1, 0x80,                                  // 0xD5 / 0x80 => D=1; DCLK = Fosc / D <=> DCLK = Fosc
  SSD1306_SET_MUX_RATIO, 1, 0x1F,                                 // 0xA8 / 0x3F (64MUX) for 128 x 64 version
                                                                  //      / 0x1F (32MUX) for 128 x 32 version
  SSD1306_DISPLAY_OFFSET, 1, 0x00,                                // 0xD3
  SSD1306_SET_START_LINE, 0,                                      // 0x40
  SSD1306_SET_CHAR_REG, 1, 0x14,                                  // 0x8D / Enable charge pump during display on
  SSD1306_MEMORY_ADDR_MODE, 1, 0x00,                              // 0x20 / Set Memory Addressing Mode
                                                                  // 0x00 / Horizontal Addressing Mode
                                                                  // 0x01 / Vertical Addressing Mode
                                                                  // 0x02 /  Page Addressing Mode (RESET)
  SSD1306_SEG_REMAP_OP, 0,                                        // 0xA0 / remap 0xA1
  SSD1306_COM_SCAN_DIR_OP, 0,                                     // 0xC8
  SSD1306_COM_PIN_CONF, 0, 0x02,                                  // 0xDA / 0x12 - Disable COM Left/Right remap, Alternative COM pin configuration
                                                                  //        0x12 - for 128 x 64 version
                                                                  //        0x02 - for 128 x 32 version
  SSD1306_SET_CONTRAST, 1, 0x8F,                                  // 0x81 / 0x8F - reset value (max 0xFF)
  SSD1306_SET_PRECHARGE, 1, 0xc2,                                 // 0xD9 / higher value less blinking
                                                                  //        0xC2, 1st phase = 2 DCLK,  2nd phase = 13 DCLK
  SSD1306_VCOM_DESELECT, 1, 0x40,                                 // 0xDB / Set V COMH Deselect, reset value 0x22 = 0,77xUcc
  SSD1306_DIS_ENT_DISP_ON, 0,                                     // 0xA4
  SSD1306_DIS_NORMAL, 0,                                          // 0xA6
  SSD1306_DEACT_SCROLL, 0,                                        // 0x2E
  SSD1306_DISPLAY_ON, 0                                           // 0xAF / Set Display ON  
};

// @const uint8_t - List of init commands according to datasheet SSD1306
const uint8_t INIT_SSD1306[] = {
  17,                                                             // number of initializers
  SSD1306_DISPLAY_OFF, 0,                                         // 0xAE = Set Display OFF
  SSD1306_SET_MUX_RATIO, 1, 0x1F,                                 // 0xA8 - 0x3F for 128 x 64 version (64MUX)
                                                                  //      - 0x1F for 128 x 32 version (32MUX)
  SSD1306_MEMORY_ADDR_MODE, 1, 0x00,                              // 0x20 = Set Memory Addressing Mode
                                                                  // 0x00 - Horizontal Addressing Mode
                                                                  // 0x01 - Vertical Addressing Mode
                                                                  // 0x02 - Page Addressing Mode (RESET)
  SSD1306_SET_START_LINE, 0,                                      // 0x40
  SSD1306_DISPLAY_OFFSET, 1, 0x00,                                // 0xD3
  SSD1306_SEG_REMAP_OP, 0,                                        // 0xA0 / remap 0xA1
  SSD1306_COM_SCAN_DIR_OP, 0,                                     // 0xC0 / remap 0xC8
  SSD1306_COM_PIN_CONF, 1, 0x02,                                  // 0xDA, 0x12 - Disable COM Left/Right remap, Alternative COM pin configuration
                                                                  //       0x12 - for 128 x 64 version
                                                                  //       0x02 - for 128 x 32 version
  SSD1306_SET_CONTRAST, 1, 0x7F,                                  // 0x81, 0x7F - reset value (max 0xFF)
  SSD1306_DIS_ENT_DISP_ON, 0,                                     // 0xA4
  SSD1306_DIS_NORMAL, 0,                                          // 0xA6
  SSD1306_SET_OSC_FREQ, 1, 0x80,                                  // 0xD5, 0x80 => D=1; DCLK = Fosc / D <=> DCLK = Fosc
  SSD1306_SET_PRECHARGE, 1, 0xc2,                                 // 0xD9, higher value less blinking
                                                                  // 0xC2, 1st phase = 2 DCLK,  2nd phase = 13 DCLK
  SSD1306_VCOM_DESELECT, 1, 0x20,                                 // Set V COMH Deselect, reset value 0x22 = 0,77xUcc
  SSD1306_SET_CHAR_REG, 1, 0x14,                                  // 0x8D, Enable charge pump during display on
  SSD1306_DEACT_SCROLL, 0,                                        // 0x2E
  SSD1306_DISPLAY_ON, 0                                           // 0xAF = Set Display ON
};

unsigned int _counter;
// @var array Chache memory Lcd 8 * 128 = 1024
static char cacheMemLcd[CACHE_SIZE_MEM];

/**
 * +------------------------------------------------------------------------------------+
 * |== PRIVATE FUNCTIONS ===============================================================|
 * +------------------------------------------------------------------------------------+
 */

/**
 * @brief   SSD1306 Init
 *
 * @param   uint8_t address
 *
 * @return  uint8_t
 */
uint8_t SSD1306_Init (uint8_t address)
{ 
    const uint8_t * list = INIT_SSD1306;    
    uint8_t status = INIT_STATUS;                                   // init status
    uint8_t arguments;
    uint8_t commands =  *list++;
    uint8_t writedata;

    

    status = SSD1306_Send_StartAndSLAW (address);
    if (SSD1306_SUCCESS != status) {
        return status;
    }

    while (commands--) {
        // Command
        // -----------------------------------------------------------------------------------
        writedata = *list++;
        status = SSD1306_Send_Command (writedata);
        if (SSD1306_SUCCESS != status){
            return status;
        }
        // Arguments
        // -----------------------------------------------------------------------------------
        //arguments = pgm_read_byte (list++);
        arguments = *list++;;
        
        while (arguments--) {
            writedata = *list++;
            status = SSD1306_Send_Command (writedata);  // argument

            if (SSD1306_SUCCESS != status) {
                return status;
            }
        }
    }
    TWI_Stop ();

    return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Send Start and SLAW request
 *
 * @param   uint8_t
 *
 * @return  uint8_t
 */
uint8_t SSD1306_Send_StartAndSLAW (uint8_t address)
{
    uint8_t status = INIT_STATUS;

    status = TWI_MT_Start ();
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // TWI: send SLAW
    // -------------------------------------------------------------------------------------
    status = TWI_MT_Send_SLAW (address);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }  
    return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Send command
 *
 * @param   uint8_t command
 *
 * @return  uint8_t
 */
uint8_t SSD1306_Send_Command (uint8_t command)
{

    uint8_t status = INIT_STATUS;

    // send control byte
    // -------------------------------------------------------------------------------------   
    status = TWI_MT_Send_Data (SSD1306_COMMAND);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // send command
    // -------------------------------------------------------------------------------------   
    status = TWI_MT_Send_Data (command);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }

    return SSD1306_SUCCESS;
}

/**
 * +------------------------------------------------------------------------------------+
 * |== PUBLIC FUNCTIONS ================================================================|
 * +------------------------------------------------------------------------------------+
 */

/**
 * @brief   SSD1306 Normal colors
 *
 * @param   uint8_t address
 *
 * @return  uint8_t
 */
uint8_t SSD1306_NormalScreen (uint8_t address)
{
    uint8_t status = INIT_STATUS;

    // TWI: start & SLAW
    // -------------------------------------------------------------------------------------
    status = SSD1306_Send_StartAndSLAW (address);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // send command
    // -------------------------------------------------------------------------------------   
    status = SSD1306_Send_Command (SSD1306_DIS_NORMAL);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // TWI: Stop
    // -------------------------------------------------------------------------------------
    TWI_Stop ();

    return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Inverse colors
 *
 * @param   uint8_t address
 *
 * @return  uint8_t
 */
uint8_t SSD1306_InverseScreen (uint8_t address)
{
    uint8_t status = INIT_STATUS;

    // TWI: start & SLAW
    // -------------------------------------------------------------------------------------
    status = SSD1306_Send_StartAndSLAW (address);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // send command
    // -------------------------------------------------------------------------------------   
    status = SSD1306_Send_Command (SSD1306_DIS_INVERSE);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // TWI: Stop
    // -------------------------------------------------------------------------------------
    TWI_Stop ();
    

    return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Update screen
 *
 * @param   uint8_t address
 *
 * @return  uint8_t
 */
uint8_t SSD1306_UpdateScreen (uint8_t address)
{

    uint8_t status = INIT_STATUS;
    uint16_t i = 0;

    // TWI: start & SLAW
    // -------------------------------------------------------------------------------------
    status = SSD1306_Send_StartAndSLAW (address);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    // control byte data stream
    // -------------------------------------------------------------------------------------   
    status = TWI_MT_Send_Data (SSD1306_DATA_STREAM);
    if (SSD1306_SUCCESS != status) 
    {
        TWI_Stop();
        return status;
    }
    //  send cache memory lcd
    // -------------------------------------------------------------------------------------
    while (i < CACHE_SIZE_MEM) {
        status = TWI_MT_Send_Data (cacheMemLcd[i++]);                 // send data
        if (SSD1306_SUCCESS != status) 
        {
            TWI_Stop();
            return status;
        }
    }
    // stop TWI
    // -------------------------------------------------------------------------------------
    TWI_Stop ();
  

    return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Clear screen
 *
 * @param   void
 *
 * @return  void
 */
void SSD1306_ClearScreen (void)
{
    memset (cacheMemLcd, 0x00, CACHE_SIZE_MEM);                     // null cache memory lcd
}

/**
 * @brief   SSD1306 Set position
 *
 * @param   uint8_t column -> 0 ... 127 
 * @param   uint8_t page -> 0 ... 7 or 3 
 *
 * @return  void
 */
void SSD1306_SetPosition (uint8_t x, uint8_t y) 
{
    _counter = x + (y << 7);                                        // update counter
}

/**
 * @brief   SSD1306 Update text poisition - this ensure that character will not be divided at the end of row, 
 *          the whole character will be depicted on the new row
 *
 * @param   void
 *
 * @return  uint8_t
 */
uint8_t SSD1306_UpdatePosition (void) 
{
  uint8_t y = _counter >> 7;                                      // y / 8
  uint8_t x = _counter - (y << 7);                                // y % 8
  uint8_t x_new = x + CHARS_COLS_LENGTH + 1;                      // x + character length + 1
  
  if (x_new > END_COLUMN_ADDR) {                                  // check position
    if (y > END_PAGE_ADDR) {                                      // if more than allowable number of pages
      return SSD1306_ERROR;                                       // return out of range
    } else if (y < (END_PAGE_ADDR-1)) {                           // if x reach the end but page in range
      _counter = ((++y) << 7);                                    // update
    }
  }
 
  return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Draw character
 *
 * @param   char character
 *
 * @return  uint8_t
 */
uint8_t SSD1306_DrawChar (char character)
{
  uint8_t i = 0;

  if (SSD1306_UpdatePosition () == SSD1306_ERROR) {
    return SSD1306_ERROR;
  }
  while (i < CHARS_COLS_LENGTH) {
    cacheMemLcd[_counter++] = FONTS[character-32][i++];
  }
  _counter++;

  return SSD1306_SUCCESS;
}

/**
 * @brief   SSD1306 Draw String
 *
 * @param   char * string
 *
 * @return  void
 */
void SSD1306_DrawString (char *str)
{
  int i = 0;
  while (str[i] != '\0') {
    SSD1306_DrawChar (str[i++]);
  }
}

/**
 * @brief   Draw pixel
 *
 * @param   uint8_t x -> 0 ... MAX_X
 * @param   uint8_t y -> 0 ... MAX_Y
 *
 * @return  uint8_t
 */
uint8_t SSD1306_DrawPixel (uint8_t x, uint8_t y)
{
  uint8_t page = 0;
  uint8_t pixel = 0;
  
  if ((x > MAX_X) || (y > MAX_Y)) {                               // if out of range
    return SSD1306_ERROR;                                         // out of range
  }
  page = y >> 3;                                                  // find page (y / 8)
  pixel = 1 << (y - (page << 3));                                 // which pixel (y % 8)
  _counter = x + (page << 7);                                     // update counter
  cacheMemLcd[_counter++] |= pixel;                               // save pixel

  return SSD1306_SUCCESS;
}

/**
 * @brief   Draw line by Bresenham algoritm
 *  
 * @param   uint8_t x start position / 0 <= cols <= MAX_X-1
 * @param   uint8_t x end position   / 0 <= cols <= MAX_X-1
 * @param   uint8_t y start position / 0 <= rows <= MAX_Y-1 
 * @param   uint8_t y end position   / 0 <= rows <= MAX_Y-1
 *
 * @return  uint8_t
 */
uint8_t SSD1306_DrawLine (uint8_t x1, uint8_t x2, uint8_t y1, uint8_t y2)
{
  int16_t D;                                                      // determinant
  int16_t delta_x, delta_y;                                       // deltas
  int16_t trace_x = 1, trace_y = 1;                               // steps

  delta_x = x2 - x1;                                              // delta x
  delta_y = y2 - y1;                                              // delta y
  
  if (delta_x < 0) {                                              // check if x2 > x1
    delta_x = -delta_x;                                           // negate delta x
    trace_x = -trace_x;                                           // negate step x
  }
  
  if (delta_y < 0) {                                              // check if y2 > y1
    delta_y = -delta_y;                                           // negate detla y
    trace_y = -trace_y;                                           // negate step y
  }

  // Bresenham condition for m < 1 (dy < dx)
  // -------------------------------------------------------------------------------------
  if (delta_y < delta_x) {
    D = (delta_y << 1) - delta_x;                                 // calculate determinant
    SSD1306_DrawPixel (x1, y1);                                   // draw first pixel
    while (x1 != x2) {                                            // check if x1 equal x2
      x1 += trace_x;                                              // update x1
      if (D >= 0) {                                               // check if determinant is positive
        y1 += trace_y;                                            // update y1
        D -= 2*delta_x;                                           // update determinant
      }
      D += 2*delta_y;                                             // update deteminant
      SSD1306_DrawPixel (x1, y1);                                 // draw next pixel
    }
  // for m > 1 (dy > dx)    
  // -------------------------------------------------------------------------------------
  } else {
    D = delta_y - (delta_x << 1);                                 // calculate determinant
    SSD1306_DrawPixel (x1, y1);                                   // draw first pixel
    while (y1 != y2) {                                            // check if y2 equal y1
      y1 += trace_y;                                              // update y1
      if (D <= 0) {                                               // check if determinant is positive
        x1 += trace_x;                                            // update y1
        D += 2*delta_y;                                           // update determinant
      }
      D -= 2*delta_x;                                             // update deteminant
      SSD1306_DrawPixel (x1, y1);                                 // draw next pixel
    }
  }

  return SSD1306_SUCCESS;
}


#if 0

const uint8_t BasicFont[][8] = {
	{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x00,0x5F,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x00,0x07,0x00,0x07,0x00,0x00,0x00},
	{0x00,0x3C,0x3C,0x3C,0x3C,0x3C,0x3C,0x00},
	{0x00,0x24,0x2A,0x7F,0x2A,0x12,0x00,0x00},
	{0x00,0x23,0x13,0x08,0x64,0x62,0x00,0x00},
	{0x00,0x36,0x49,0x55,0x22,0x50,0x00,0x00},
	{0x00,0x00,0x05,0x03,0x00,0x00,0x00,0x00},
	{0x00,0x1C,0x22,0x41,0x00,0x00,0x00,0x00},
	{0x00,0x41,0x22,0x1C,0x00,0x00,0x00,0x00},
	{0x00,0x08,0x2A,0x1C,0x2A,0x08,0x00,0x00},
	{0x00,0x08,0x08,0x3E,0x08,0x08,0x00,0x00},
	{0x00,0xA0,0x60,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x08,0x08,0x08,0x08,0x08,0x00,0x00},
	{0x00,0x60,0x60,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x20,0x10,0x08,0x04,0x02,0x00,0x00},
	{0x00,0x3E,0x51,0x49,0x45,0x3E,0x00,0x00},
	{0x00,0x00,0x42,0x7F,0x40,0x00,0x00,0x00},
	{0x00,0x62,0x51,0x49,0x49,0x46,0x00,0x00},
	{0x00,0x22,0x41,0x49,0x49,0x36,0x00,0x00},
	{0x00,0x18,0x14,0x12,0x7F,0x10,0x00,0x00},
	{0x00,0x27,0x45,0x45,0x45,0x39,0x00,0x00},
	{0x00,0x3C,0x4A,0x49,0x49,0x30,0x00,0x00},
	{0x00,0x01,0x71,0x09,0x05,0x03,0x00,0x00},
	{0x00,0x36,0x49,0x49,0x49,0x36,0x00,0x00},
	{0x00,0x06,0x49,0x49,0x29,0x1E,0x00,0x00},
	{0x00,0x00,0x36,0x36,0x00,0x00,0x00,0x00},
	{0x00,0x00,0xAC,0x6C,0x00,0x00,0x00,0x00},
	{0x00,0x08,0x14,0x22,0x41,0x00,0x00,0x00},
	{0x00,0x14,0x14,0x14,0x14,0x14,0x00,0x00},
	{0x00,0x41,0x22,0x14,0x08,0x00,0x00,0x00},
	{0x00,0x02,0x01,0x51,0x09,0x06,0x00,0x00},
	{0x00,0x32,0x49,0x79,0x41,0x3E,0x00,0x00},
	{0x00,0x7E,0x09,0x09,0x09,0x7E,0x00,0x00},
	{0x00,0x7F,0x49,0x49,0x49,0x36,0x00,0x00},
	{0x00,0x3E,0x41,0x41,0x41,0x22,0x00,0x00},
	{0x00,0x7F,0x41,0x41,0x22,0x1C,0x00,0x00},
	{0x00,0x7F,0x49,0x49,0x49,0x41,0x00,0x00},
	{0x00,0x7F,0x09,0x09,0x09,0x01,0x00,0x00},
	{0x00,0x3E,0x41,0x41,0x51,0x72,0x00,0x00},
	{0x00,0x7F,0x08,0x08,0x08,0x7F,0x00,0x00},
	{0x00,0x41,0x7F,0x41,0x00,0x00,0x00,0x00},
	{0x00,0x20,0x40,0x41,0x3F,0x01,0x00,0x00},
    
	{0x00,0x7F,0x08,0x14,0x22,0x41,0x00,0x00},
    
	{0x00,0x7F,0x40,0x40,0x40,0x40,0x00,0x00},
	{0x00,0x7F,0x02,0x0C,0x02,0x7F,0x00,0x00},
	{0x00,0x7F,0x04,0x08,0x10,0x7F,0x00,0x00},
	{0x00,0x3E,0x41,0x41,0x41,0x3E,0x00,0x00},
	{0x00,0x7F,0x09,0x09,0x09,0x06,0x00,0x00},
	{0x00,0x3E,0x41,0x51,0x21,0x5E,0x00,0x00},
	{0x00,0x7F,0x09,0x19,0x29,0x46,0x00,0x00},
	{0x00,0x26,0x49,0x49,0x49,0x32,0x00,0x00},
	{0x00,0x01,0x01,0x7F,0x01,0x01,0x00,0x00},
	{0x00,0x3F,0x40,0x40,0x40,0x3F,0x00,0x00},
	{0x00,0x1F,0x20,0x40,0x20,0x1F,0x00,0x00},
	{0x00,0x3F,0x40,0x38,0x40,0x3F,0x00,0x00},
	{0x00,0x63,0x14,0x08,0x14,0x63,0x00,0x00},
	{0x00,0x03,0x04,0x78,0x04,0x03,0x00,0x00},
	{0x00,0x61,0x51,0x49,0x45,0x43,0x00,0x00},
	{0x00,0x7F,0x41,0x41,0x00,0x00,0x00,0x00},
	{0x00,0x02,0x04,0x08,0x10,0x20,0x00,0x00},
	{0x00,0x41,0x41,0x7F,0x00,0x00,0x00,0x00},
	{0x00,0x04,0x02,0x01,0x02,0x04,0x00,0x00},
	{0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00},
	{0x00,0x01,0x02,0x04,0x00,0x00,0x00,0x00},
	{0x00,0x20,0x54,0x54,0x54,0x78,0x00,0x00},
	{0x00,0x7F,0x48,0x44,0x44,0x38,0x00,0x00},
	{0x00,0x38,0x44,0x44,0x28,0x00,0x00,0x00},
	{0x00,0x38,0x44,0x44,0x48,0x7F,0x00,0x00},
	{0x00,0x38,0x54,0x54,0x54,0x18,0x00,0x00},
	{0x00,0x08,0x7E,0x09,0x02,0x00,0x00,0x00},
	{0x00,0x18,0xA4,0xA4,0xA4,0x7C,0x00,0x00},
	{0x00,0x7F,0x08,0x04,0x04,0x78,0x00,0x00},
	{0x00,0x00,0x7D,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x80,0x84,0x7D,0x00,0x00,0x00,0x00},
	{0x00,0x7F,0x10,0x28,0x44,0x00,0x00,0x00},
	{0x00,0x41,0x7F,0x40,0x00,0x00,0x00,0x00},
	{0x00,0x7C,0x04,0x18,0x04,0x78,0x00,0x00},
	{0x00,0x7C,0x08,0x04,0x7C,0x00,0x00,0x00},
	{0x00,0x38,0x44,0x44,0x38,0x00,0x00,0x00},
	{0x00,0xFC,0x24,0x24,0x18,0x00,0x00,0x00},
	{0x00,0x18,0x24,0x24,0xFC,0x00,0x00,0x00},
	{0x00,0x00,0x7C,0x08,0x04,0x00,0x00,0x00},
	{0x00,0x48,0x54,0x54,0x24,0x00,0x00,0x00},
	{0x00,0x04,0x7F,0x44,0x00,0x00,0x00,0x00},
	{0x00,0x3C,0x40,0x40,0x7C,0x00,0x00,0x00},
	{0x00,0x1C,0x20,0x40,0x20,0x1C,0x00,0x00},
	{0x00,0x3C,0x40,0x30,0x40,0x3C,0x00,0x00},
	{0x00,0x44,0x28,0x10,0x28,0x44,0x00,0x00},
	{0x00,0x1C,0xA0,0xA0,0x7C,0x00,0x00,0x00},
	{0x00,0x44,0x64,0x54,0x4C,0x44,0x00,0x00},
	{0x00,0x08,0x36,0x41,0x00,0x00,0x00,0x00},
	{0x00,0x00,0x7F,0x00,0x00,0x00,0x00,0x00},
	{0x00,0x41,0x36,0x08,0x00,0x00,0x00,0x00},
	{0x00,0x02,0x01,0x01,0x02,0x01,0x00,0x00},
	{0x00,0x02,0x05,0x05,0x02,0x00,0x00,0x00} 
};




#define SSD1306_128_64

#define i2c_addr 0x3C	//oled i2c address
//OLED configuration
#if defined SSD1306_128_64
#define SSD1306_LCDWIDTH                  128
#define SSD1306_LCDHEIGHT                 64
#endif
#if defined SSD1306_128_32
#define SSD1306_LCDWIDTH                  128
#define SSD1306_LCDHEIGHT                 32
#endif
#if defined SSD1306_96_16
#define SSD1306_LCDWIDTH                  96
#define SSD1306_LCDHEIGHT                 16
#endif

//OLED SSD1306 commands as given in datasheet of ssd 1306
#define SSD1306_SETCONTRAST 0x81
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_DISPLAYALLON 0xA5
#define SSD1306_NORMALDISPLAY 0xA6
#define SSD1306_INVERTDISPLAY 0xA7
#define SSD1306_DISPLAYOFF 0xAE
#define SSD1306_DISPLAYON 0xAF

#define SSD1306_SETDISPLAYOFFSET 0xD3
#define SSD1306_SETCOMPINS 0xDA

#define SSD1306_SETVCOMDETECT 0xDB

#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
#define SSD1306_SETPRECHARGE 0xD9

#define SSD1306_SETMULTIPLEX 0xA8

#define SSD1306_SETLOWCOLUMN 0x00
#define SSD1306_SETHIGHCOLUMN 0x10

#define SSD1306_SETSTARTLINE 0x40

#define SSD1306_MEMORYMODE 0x20
#define SSD1306_COLUMNADDR 0x21
#define SSD1306_PAGEADDR   0x22

#define SSD1306_COMSCANINC 0xC0
#define SSD1306_COMSCANDEC 0xC8

#define SSD1306_SEGREMAP 0xA0

#define SSD1306_CHARGEPUMP 0x8D

#define SSD1306_EXTERNALVCC 0x1
#define SSD1306_SWITCHCAPVCC 0x2

// Scrolling #defines
#define SSD1306_ACTIVATE_SCROLL 0x2F
#define SSD1306_DEACTIVATE_SCROLL 0x2E
#define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3
#define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26
#define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A


/***************************************************************************
funcion: void begin_oled_i2c_ssd1306_12864()
Description: This function initializes 124x64 OLED display.
***************************************************************************/
void begin_oled_i2c_ssd1306_12864()
{
	i2c_init();
	//initialize oled 128*64
	oled_command(SSD1306_DISPLAYOFF);
	oled_command(SSD1306_SETDISPLAYCLOCKDIV);
	oled_command(0x80);
	oled_command(SSD1306_SETMULTIPLEX);
	oled_command(64-1);//height-1
	oled_command(SSD1306_SETDISPLAYOFFSET);
	oled_command(0x00);//no offset
	oled_command(SSD1306_SETSTARTLINE| 0x00);//set start line
	oled_command(SSD1306_CHARGEPUMP);//charge pump
	oled_command(0x14);//dclk upto 14-15
	oled_command(SSD1306_MEMORYMODE);
	oled_command(0x00);//OLED RAM
	oled_command(SSD1306_SEGREMAP| 0x01);
	oled_command(SSD1306_COMSCANDEC);
	oled_command(SSD1306_SETCOMPINS);
	oled_command(0x12);
	oled_command(SSD1306_SETCONTRAST);
	oled_command(0xcf);
	oled_command(SSD1306_SETPRECHARGE);
	oled_command(0xf1);
	oled_command(SSD1306_SETVCOMDETECT);
	oled_command(0x40);
	oled_command(SSD1306_DISPLAYALLON_RESUME);
	oled_command(SSD1306_NORMALDISPLAY);
	oled_command(SSD1306_DISPLAYON);
	oled_command(SSD1306_COLUMNADDR);
	oled_command(0x00);
	oled_command(0x7f);
	oled_command(SSD1306_PAGEADDR);
	oled_command(0x00);
	oled_command(0x07);
}

/****************************************************************************
function: void oled_command(uint8_t data)
Description: Sends 'data' as command to OLED.
****************************************************************************/
void oled_command(unsigned char data)
{

	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x00);
	i2c_write(data);
	i2c_stop();
}

/****************************************************************************
function: void oled_data(uint8_t data)
Description: Sends 'data'  to OLED RAM for displaying data
****************************************************************************/
void oled_data(unsigned char data)
{
	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x40);
	i2c_write(data);
	i2c_stop();
}




#endif































#if 0


#include "oled.h"
#include "i2c.h"
#include "timer.h"







#if 0


void Oled_SendCommand(unsigned char command)
{    
    I2C_Start();	
	I2C_Write(OLED_ADDRESS_W);
	I2C_Write(OzOLED_COMMAND_MODE);
    I2C_Write(command);
	I2C_Stop();
}

void Oled_SendData(unsigned char data)
{
    I2C_Start();	
	I2C_Write(OLED_ADDRESS_W);
	I2C_Write(OzOLED_DATA_MODE);
    I2C_Write(data);
	I2C_Stop();
}




unsigned char Oled_PrintFloatXY(float float_num, unsigned char prec, unsigned char X, unsigned char Y)
{

    unsigned char num_int = 0;
    unsigned char num_frac = 0;
    unsigned char num_extra = 0;
    
    long d = float_num; // get the integer part
    float f = float_num - d; // get the fractional part

	if ( X < 128 )
		Oled_SetCursorXY(X, Y);

// prec - 6 maximum

	
	
	
	if (d == 0 && f < 0.0){
	
		Oled_PrintChar('-');
		num_extra++;
		Oled_PrintChar('0');
		num_extra++;
		f *= -1;
		
	}
	else if (d < 0 && f < 0.0){
	
		num_int = Oled_PrintNumber(d); // count how many digits in integer part
		f *= -1;
		
	}
	else{
	
		num_int = Oled_PrintNumber(d); // count how many digits in integer part
	
	}
	
	// only when fractional part > 0, we show decimal point
	if (f > 0.0){
	
		printChar('.');
		num_extra++;
	
		long f_shift = 1;
		
		if (num_int + prec > 8) 
			prec = 8 - num_int;
		
		for (unsigned char j=0; j<prec; j++){
			f_shift *= 10;
		}

		num_frac = Oled_PrintNumber((long)(f*f_shift)); // count how many digits in fractional part
		
	}
	
	return num_int + num_frac + num_extra;

}

#endif


#if 0


// ====================== LOW LEVEL =========================





void Oled_PrintBigNumber(const char *number, byte X, byte Y, byte numChar){
// big number pixels: 24 x 32

 // Y - page
	byte column = 0;
	byte count = 0;

	while(number[count] && count<numChar){
	
	
		setCursorXY(X, Y);
		
		for(byte i=0; i<96; i++) {
		
			// if character is not "0-9" or ':'
			if(number[count] < 48 || number[count] > 58)	
				sendData(0);
			else 				
				sendData(pgm_read_byte(&bigNumbers[number[count]-48][i]));
			
			
			if(column >= 23){
				column = 0;
				setCursorXY(X, ++Y);
			}
			else				
				column++;

		}
		
		count++;
		
		X = X + 3;
		Y = Y - 4;
		
	
	}

	
	
}


void Oled_PrawBitmap(const byte *bitmaparray, byte X, byte Y, byte width, byte height){

// max width = 16
// max height = 8

	setCursorXY( X, Y );
	
	byte column = 0; 
	for(int i=0; i<width*8*height; i++) {  

		sendData(pgm_read_byte(&bitmaparray[i]));
		
		if(++column == width*8) {
			column = 0;
			setCursorXY( X, ++Y );
		} 
	}

}



void OzOLED::clearDisplay()	{


	for(byte page=0; page<8; page++) {	
	
		setCursorXY(0, page);     
		for(byte column=0; column<128; column++){  //clear all columns
			sendData(0);    
		}

	}
	
	setCursorXY(0,0);  
	
}

/*
void OzOLED::clearPage(byte page)	{
	// clear page and set cursor at beginning of that page

	setCursorXY(0, page);    
	for(byte column=0; column<128; column++){  //clear all columns
		sendData(0x00);    
	}
	
}
*/


void OzOLED::setInverseDisplay(){

	sendCommand(OzOLED_CMD_INVERSE_DISPLAY);
	
}

void OzOLED::setNormalDisplay(){

	sendCommand(OzOLED_CMD_NORMAL_DISPLAY);
	
}

void OzOLED::setPowerOff(){

	sendCommand(OzOLED_CMD_DISPLAY_OFF);
	
}

void OzOLED::setPowerOn(){

	sendCommand(OzOLED_CMD_DISPLAY_ON);
	
}

void OzOLED::setBrightness(byte Brightness){

	sendCommand(OzOLED_CMD_SET_BRIGHTNESS);
	sendCommand(Brightness);
   
}

void OzOLED::setPageMode(){
	addressingMode = PAGE_ADDRESSING;
	sendCommand(0x20); 				//set addressing mode
	sendCommand(PAGE_ADDRESSING); 	//set page addressing mode
}

void OzOLED::setHorizontalMode(){
	addressingMode = HORIZONTAL_ADDRESSING;
	sendCommand(0x20); 				//set addressing mode
	sendCommand(HORIZONTAL_ADDRESSING); 	//set page addressing mode
}


// startscrollright
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// scrollRight(0x00, 0x0F)  - start - stop
void OzOLED::scrollRight(byte start, byte end, byte speed){

    sendCommand(OzOLED_RIGHT_SCROLL);  //Horizontal Scroll Setup
    sendCommand(0x00);	// dummy byte 
    sendCommand(start);	// start page address
    sendCommand(speed);	// set time interval between each scroll
    sendCommand(end);	// end page address
	
    sendCommand(0x01);  
    sendCommand(0xFF);
	
    sendCommand(0x2f);  //active scrolling
	
}


// 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(0x00, 0x0F)   - start - stop
void OzOLED::scrollLeft(byte start, byte end, byte speed){

    sendCommand(OzOLED_LEFT_SCROLL);  //Horizontal Scroll Setup
    sendCommand(0x00);	// dummy byte
    sendCommand(start);	// start page address
    sendCommand(speed);	// set time interval between each scroll
    sendCommand(end);	// end page address
	
    sendCommand(0x01);  
    sendCommand(0xFF);  
	
    sendCommand(0x2f);  //active scrolling
	
}

// startscrolldiagright
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F) 
void OzOLED::scrollDiagRight(){

        sendCommand(OzOLED_SET_VERTICAL_SCROLL_AREA);        
        sendCommand(0X00);
        sendCommand(OzOLED_Max_Y);
        sendCommand(OzOLED_VERTICAL_RIGHT_SCROLL); //Vertical and Horizontal Scroll Setup
        sendCommand(0X00); 	//dummy byte
        sendCommand(0x00);	 //define page0 as startpage address
        sendCommand(0X00);	//set time interval between each scroll ste as 6 frames
        sendCommand(0x07);	//define page7 as endpage address
        sendCommand(0X01);	//set vertical scrolling offset as 1 row
        sendCommand(OzOLED_CMD_ACTIVATE_SCROLL); //active scrolling
	
}

void OzOLED::scrollDiagLeft(){

        sendCommand(OzOLED_SET_VERTICAL_SCROLL_AREA);        
        sendCommand(0X00);
        sendCommand(OzOLED_Max_Y);
        sendCommand(OzOLED_VERTICAL_LEFT_SCROLL); //Vertical and Horizontal Scroll Setup
        sendCommand(0X00); //dummy byte
        sendCommand(0x00);	 //define page0 as startpage address
        sendCommand(0X00);	//set time interval between each scroll ste as 6 frames
        sendCommand(0x07);	//define page7 as endpage address
        sendCommand(0X01);	//set vertical scrolling offset as 1 row
        sendCommand(OzOLED_CMD_ACTIVATE_SCROLL); //active scrolling
	
}


void OzOLED::setActivateScroll(byte direction, byte startPage, byte endPage, byte scrollSpeed){


/*
This function is still not complete, we need more testing also.
Use the following defines for 'direction' :

 Scroll_Left			
 Scroll_Right			

For Scroll_vericle, still need to debug more... 

Use the following defines for 'scrollSpeed' :

 Scroll_2Frames		
 Scroll_3Frames
 Scroll_4Frames
 Scroll_5Frames	
 Scroll_25Frames
 Scroll_64Frames
 Scroll_128Frames
 Scroll_256Frames

*/


	if(direction == Scroll_Right) {
	
		//Scroll Right
		sendCommand(0x26);
		
	}
	else {
	
		//Scroll Left  
		sendCommand(0x27);

	}
	/*
	else if (direction == Scroll_Up ){
	
		//Scroll Up  
		sendCommand(0x29);
	
	}
	else{
	
		//Scroll Down  
		sendCommand(0x2A);
	
	}
	*/
	sendCommand(0x00);//dummy byte
	sendCommand(startPage);
	sendCommand(scrollSpeed);	
	sendCommand(endPage);		// for verticle scrolling, use 0x29 as command, endPage should = start page = 0
	
	/*
	if(direction == Scroll_Up) {
	
		sendCommand(0x01);

	}
	*/

	sendCommand(OzOLED_CMD_ACTIVATE_SCROLL);

}

void OzOLED::setDeactivateScroll(){

	sendCommand(OzOLED_CMD_DEACTIVATE_SCROLL);

}

#endif


#if 0
void Oled_SetPowerOff(void)
{	
    Oled_SendCommand(OzOLED_CMD_DISPLAY_OFF);    
}

void Oled_SetPowerOn(void)
{ 
    Oled_SendCommand(OzOLED_CMD_DISPLAY_ON); 
}

void Oled_SetNormalDisplay(void)
{   
    Oled_SendCommand(OzOLED_CMD_NORMAL_DISPLAY); 
}
void Oled_SetPageMode(void)
{
    Oled_SendCommand(0x20);              //set addressing mode   
    Oled_SendCommand(PAGE_ADDRESSING);   //set page addressing mode
}


void Oled_ClearDisplay(void)	
{
    unsigned char page;
    unsigned char column;

	for(page=0; page<8; page++) 
    {   
	
		Oled_SetCursorXY(0, page);     
		for(column=0; column<128; column++)
        {  
			Oled_SendData(0);    
		}

	}
	
	Oled_SetCursorXY(0,0);  
	
}


void Oled_Initialization(void)
{
    
    I2C_Start();	    
    I2C_Write(OLED_ADDRESS_W);  
    I2C_Write(OLED_CONTROL_BYTE_CMD_STREAM);
    I2C_Write(OLED_CMD_DISPLAY_OFF);
    I2C_Write(OLED_CMD_SET_MUX_RATIO);
    I2C_Write(0x3F);
    // Set the display offset to 0
    I2C_Write(OLED_CMD_SET_DISPLAY_OFFSET);
    I2C_Write(0x00);
    // Display start line to 0
    I2C_Write(OLED_CMD_SET_DISPLAY_START_LINE);

    // Mirror the x-axis. In case you set it up such that the pins are north.
    // Wire.write(0xA0); - in case pins are south - default
    I2C_Write(OLED_CMD_SET_SEGMENT_REMAP);

    // Mirror the y-axis. In case you set it up such that the pins are north.
    // Wire.write(0xC0); - in case pins are south - default
    I2C_Write(OLED_CMD_SET_COM_SCAN_MODE);

    //its working without lines below
    // Default - alternate COM pin map
    //Wire.write(OLED_CMD_SET_COM_PIN_MAP);
    //Wire.write(0x12);
    // set contrast
    //Wire.write(OLED_CMD_SET_CONTRAST);
    //Wire.write(0x7F);
    // Set display to enable rendering from GDDRAM (Graphic Display Data RAM)
    //Wire.write(OLED_CMD_DISPLAY_RAM);
    // Normal mode!
    //Wire.write(OLED_CMD_DISPLAY_NORMAL);
    // Default oscillator clock
    //Wire.write(OLED_CMD_SET_DISPLAY_CLK_DIV);
    //Wire.write(0x80);
    // Enable the charge pump
    I2C_Write(OLED_CMD_SET_CHARGE_PUMP);
    I2C_Write(0x14);

    //its working without lines below
    // Set precharge cycles to high cap type
    //Wire.write(OLED_CMD_SET_PRECHARGE);
    //Wire.write(0x22);
    // Set the V_COMH deselect volatage to max
    //Wire.write(OLED_CMD_SET_VCOMH_DESELCT);
    //Wire.write(0x30);
    // Horizonatal addressing mode - same as the KS108 GLCD
    //Wire.write(OLED_CMD_SET_MEMORY_ADDR_MODE);
    //Wire.write(0x00);
    // Turn the Display ON
    //Wire.write(OLED_CMD_DISPLAY_ON);

    // End the I2C comm with the SSD1306
    I2C_Stop();


	
    Oled_SetPowerOff(); 	//display off
    Delay_ms(100);
    Oled_SetPowerOn();	//display on
    Delay_ms(100); 
    Oled_SetNormalDisplay();  //default Set Normal Display
    
	Oled_SetPageMode();	// default addressing mode
	Oled_ClearDisplay();
	Oled_SetCursorXY(0,0);
}



#endif





#define SSD1306_128_64

#define i2c_addr 0x3C	//oled i2c address
//OLED configuration
#if defined SSD1306_128_64
#define SSD1306_LCDWIDTH                  128
#define SSD1306_LCDHEIGHT                 64
#endif
#if defined SSD1306_128_32
#define SSD1306_LCDWIDTH                  128
#define SSD1306_LCDHEIGHT                 32
#endif
#if defined SSD1306_96_16
#define SSD1306_LCDWIDTH                  96
#define SSD1306_LCDHEIGHT                 16
#endif

//OLED SSD1306 commands as given in datasheet of ssd 1306
#define SSD1306_SETCONTRAST 0x81
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_DISPLAYALLON 0xA5
#define SSD1306_NORMALDISPLAY 0xA6
#define SSD1306_INVERTDISPLAY 0xA7
#define SSD1306_DISPLAYOFF 0xAE
#define SSD1306_DISPLAYON 0xAF

#define SSD1306_SETDISPLAYOFFSET 0xD3
#define SSD1306_SETCOMPINS 0xDA

#define SSD1306_SETVCOMDETECT 0xDB

#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
#define SSD1306_SETPRECHARGE 0xD9

#define SSD1306_SETMULTIPLEX 0xA8

#define SSD1306_SETLOWCOLUMN 0x00
#define SSD1306_SETHIGHCOLUMN 0x10

#define SSD1306_SETSTARTLINE 0x40

#define SSD1306_MEMORYMODE 0x20
#define SSD1306_COLUMNADDR 0x21
#define SSD1306_PAGEADDR   0x22

#define SSD1306_COMSCANINC 0xC0
#define SSD1306_COMSCANDEC 0xC8

#define SSD1306_SEGREMAP 0xA0

#define SSD1306_CHARGEPUMP 0x8D

#define SSD1306_EXTERNALVCC 0x1
#define SSD1306_SWITCHCAPVCC 0x2

// Scrolling #defines
#define SSD1306_ACTIVATE_SCROLL 0x2F
#define SSD1306_DEACTIVATE_SCROLL 0x2E
#define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3
#define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26
#define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A


/***************************************************************************
funcion: void begin_oled_i2c_ssd1306_12864()
Description: This function initializes 124x64 OLED display.
***************************************************************************/
void begin_oled_i2c_ssd1306_12864()
{
	i2c_init();
	//initialize oled 128*64
	oled_command(SSD1306_DISPLAYOFF);
	oled_command(SSD1306_SETDISPLAYCLOCKDIV);
	oled_command(0x80);
	oled_command(SSD1306_SETMULTIPLEX);
	oled_command(64-1);//height-1
	oled_command(SSD1306_SETDISPLAYOFFSET);
	oled_command(0x00);//no offset
	oled_command(SSD1306_SETSTARTLINE| 0x00);//set start line
	oled_command(SSD1306_CHARGEPUMP);//charge pump
	oled_command(0x14);//dclk upto 14-15
	oled_command(SSD1306_MEMORYMODE);
	oled_command(0x00);//OLED RAM
	oled_command(SSD1306_SEGREMAP| 0x01);
	oled_command(SSD1306_COMSCANDEC);
	oled_command(SSD1306_SETCOMPINS);
	oled_command(0x12);
	oled_command(SSD1306_SETCONTRAST);
	oled_command(0xcf);
	oled_command(SSD1306_SETPRECHARGE);
	oled_command(0xf1);
	oled_command(SSD1306_SETVCOMDETECT);
	oled_command(0x40);
	oled_command(SSD1306_DISPLAYALLON_RESUME);
	oled_command(SSD1306_NORMALDISPLAY);
	oled_command(SSD1306_DISPLAYON);
	oled_command(SSD1306_COLUMNADDR);
	oled_command(0x00);
	oled_command(0x7f);
	oled_command(SSD1306_PAGEADDR);
	oled_command(0x00);
	oled_command(0x07);
}

/****************************************************************************
function: void oled_command(uint8_t data)
Description: Sends 'data' as command to OLED.
****************************************************************************/
void oled_command(unsigned char data)
{

	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x00);
	i2c_write(data);
	i2c_stop();
}

/****************************************************************************
function: void oled_data(uint8_t data)
Description: Sends 'data'  to OLED RAM for displaying data
****************************************************************************/
void oled_data(unsigned char data)
{
	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x40);
	i2c_write(data);
	i2c_stop();
}

/****************************************************************************
function: void oled_black_white()
Description: Sample function to check white line on screen
****************************************************************************/
void oled_black_white()
{
	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x40);
	for (int j=0; j<32;j++)
	{
	
	for(int i=0;i<16;i++)
	i2c_write(0);
	for(int i=0;i<16;i++)
	i2c_write(1);
	}
	i2c_stop();
}

/****************************************************************************
function: void dispaly_bmp_image(const char *image)
Description: A bitmap image '*image' to be displayed.
*image is array of all bits of bitmap images to be displayed.
****************************************************************************/
void dispaly_bmp_image(unsigned char __flash  *image)
{
	i2c_start(i2c_addr<<1 | I2C_WRITE);
	i2c_write(0x40);
	for (int j=0; j<1024;j++)
	{
		i2c_write(image[j]);
	}
	i2c_stop();
	oled_command(SSD1306_DISPLAYON);
}





void Oled_PrintCharXY(char C, unsigned char X, unsigned char Y)
{
    unsigned char i;
	if ( X < 128 )
	{
		Oled_SetCursorXY(X, Y);
	}

	//Ignore unused ASCII characters. Modified the range to support multilingual characters.
    if(C < 32 || C > 127)
		C='*'; //star - indicate characters that can't be displayed

	
    for(i=0; i<8; i++) {
	
       //read bytes from code memory
       oled_data(BasicFont[C-32][i]); //font array starts at 0, ASCII starts at 32. Hence the translation
	 
    }
}
void Oled_PrintChar(char C)
{
    unsigned char i;
    
    //Ignore unused ASCII characters. Modified the range to support multilingual characters.
    if(C < 32 || C > 127)
		C='*'; //star - indicate characters that can't be displayed

	
    for(i=0; i<8; i++) {
	
       //read bytes from code memory
       oled_data(BasicFont[C-32][i]); //font array starts at 0, ASCII starts at 32. Hence the translation
	 
    }
}





void Oled_PrintString(const char *String, unsigned char X, unsigned char Y, unsigned char numChar)
{
    unsigned char count=0;

	if ( X < 128 )
	{
		Oled_SetCursorXY(X, Y);
	}

	
	
    while(String[count] && count<numChar)
    {
		Oled_PrintChar(String[count++]);  
	}

}


void Oled_SetCursorXY(unsigned char X, unsigned char Y)
{
	// Y - 1 unit = 1 page (8 pixel rows)
	// X - 1 unit = 8 pixel columns

    oled_command(0x00 + (8*X & 0x0F)); 		//set column lower address
    oled_command(0x10 + ((8*X>>4)&0x0F)); 	//set column higher address
	oled_command(0xB0 + Y); 					//set page address
	
}


unsigned char Oled_PrintNumberXY(long long_num, unsigned char X, unsigned char Y)
{

    unsigned char char_buffer[10] = "";
    unsigned char i = 0;
    unsigned char f = 0; // number of characters



	if ( X < 128 )
		Oled_SetCursorXY(X, Y);


	
	if (long_num < 0) {
	
		f++;
		Oled_PrintChar('-');
		long_num = -long_num;
	
	} 
	else if (long_num == 0) {
	
		f++;
		Oled_PrintChar('0');
		return f;
	
	} 

	while (long_num > 0) {
	
		char_buffer[i++] = long_num % 10;
		long_num /= 10;
	
	}

	f += i;
	for(; i > 0; i--) {

		Oled_PrintChar('0'+ char_buffer[i - 1]);

	}
	
	return f;

}

unsigned char Oled_PrintNumber(long long_num)
{
    unsigned char char_buffer[10] = "";
    unsigned char i = 0;
    unsigned char f = 0; // number of characters



	
	if (long_num < 0) {
	
		f++;
		Oled_PrintChar('-');
		long_num = -long_num;
	
	} 
	else if (long_num == 0) {
	
		f++;
		Oled_PrintChar('0');
		return f;
	
	} 

	while (long_num > 0) {
	
		char_buffer[i++] = long_num % 10;
		long_num /= 10;
	
	}

	f += i;
	for(; i > 0; i--) {

		Oled_PrintChar('0'+ char_buffer[i - 1]);

	}
	
	return f;
}


void Oled_ClearDisplay(void)	
{
    unsigned char page;
    unsigned char column;

	for(page=0; page<8; page++) 
    {   
	
		Oled_SetCursorXY(0, page);     
		for(column=0; column<128; column++)
        {  
			oled_data(0);    
		}

	}
	
	Oled_SetCursorXY(0,0);  
	
}
#endif