/*******************************************************************************
* File Name          : ssd1306.c
* Author             : lxyppc
* Version            : V1.0
* Date               : 10-01-21
* Description        : ssd1306 operations
*                      the SSH1101A is compatible with ssd1306
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "stm32l100c_discovery.h"
#include "bsp.h"
#include "ssd1306.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define   SSD1306_PAGE_NUMBER           8
#define   SSD1306_COLUMN_NUMBER         128
#define   SSD1306_COLUMN_MARGIN_START   2
#define   SSD1306_COLUMN_MARGIN_END     2
#define   SSD1306_X_PIXEL   128
#define   SSD1306_Y_PIXEL   32

/* Private macro -------------------------------------------------------------*/
#define   ssd1306_Buffer    (_SSD1306_Buffer + SSD1306_COLUMN_MARGIN_START)

/* Private variables ---------------------------------------------------------*/
static  uint8_t  _SSD1306_Buffer[SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER + SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END] = {0};
static  uint8_t  pageIndex = 0;
static  uint8_t  iS_SSD_On = 0;
static  uint8_t  pre_on = 0;
static  uint8_t  curContrast = 0xCC;
static  uint8_t  lastContrast = 0xCC;

/* Private function prototypes -----------------------------------------------*/
void  WriteCommand(unsigned char command);
void  WriteData(unsigned char data);
void  OnPageTransferDone(void);
unsigned long ssd1306_OFF(void);
unsigned long ssd1306_ON(void);
unsigned char* ssd1306_GetBuffer()
{
  return ssd1306_Buffer;
}
/*******************************************************************************
* Function Name  : WriteCommand
* Description    : Write command to the ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void WriteCommand(unsigned char command)
{
  SSD_A0_Low();
  SPI_SendByte(command);
  SPI_Wait();
}

/*******************************************************************************
* Function Name  : WriteData
* Description    : Write data to the ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void WriteData(unsigned char data)
{
  SSD_A0_High();
  SPI_SendByte(data);
  SPI_Wait();
}

/*******************************************************************************
* Function Name  : ssd1306_Init
* Description    : Initialize the ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void ssd1306_Init(void)
{

  /* Generate a reset */
  SSD_Reset_Low();
  uint32_t i;
  for(i=5000; i>1; i--) 
  SSD_Reset_High();

  WriteCommand(0xAE);
  WriteCommand(0xD5);
  WriteCommand(0x80);
  WriteCommand(0xA8);
  WriteCommand(0x1F);
  WriteCommand(0xD3);
  WriteCommand(0x00);
  WriteCommand(0x40 | 0x00); // line #0
  WriteCommand(0x8D);
  WriteCommand(0x14); //10 or 14 if not externalvcc
  WriteCommand(0x20);
  WriteCommand(0x00);
  WriteCommand(0xA0 | 0x1); // segremap
  WriteCommand(0xC8); // comscandep
  WriteCommand(0xDA); // setcompins 
  WriteCommand(0x02);
  WriteCommand(0x81); // contrast
  WriteCommand(0x0F); // contrast value. 8f is a good one.
  WriteCommand(0xD9);
  WriteCommand(0xF1); //22 or F1 if not externalvcc
  WriteCommand(0xDB);
  WriteCommand(0x40);
  WriteCommand(0xA4); // dispalyallon_resume
  WriteCommand(0xA6); // normaldisplay

  WriteCommand(0xAF); // display on 
}

void ssd1306_block_write(void)
{
  // Set col start addr to 0
  WriteCommand(0x21); 
  WriteCommand(0x00); 
  // Set col end addr to width - 1
  WriteCommand(127);

  // Set page addr
  WriteCommand(0x22);
  WriteCommand(0x00); // start page addr
  WriteCommand(4); // end page addr (height / 8)

  // Write data
  uint32_t i = 0;
  for(i=0;i<512;i++)
    WriteData(ssd1306_Buffer[i]);
}


/*******************************************************************************
* Function Name  : ssd1306_TurnOff
* Description    : Turn off the ssd1306 controller
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_TurnOff(void)
{
  pre_on = 0;
  return iS_SSD_On;
}

/*******************************************************************************
* Function Name  : ssd1306_TurnOn
* Description    : Turn off the ssd1306 controller
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_TurnOn(void)
{
  pre_on = 1;
  return iS_SSD_On;
}

/*******************************************************************************
* Function Name  : ssd1306_SetContrast
* Description    : Set the ssd1306 contrast
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned char ssd1306_SetContrast(unsigned char contrast)
{
  curContrast = contrast;
  return lastContrast;
}

/*******************************************************************************
* Function Name  : ssd1306_GetContrast
* Description    : Get the ssd1306 contrast
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned char ssd1306_GetContrast()
{
  return lastContrast;
}

/*******************************************************************************
* Function Name  : ssd1306_OFF
* Description    : Turn off the ssd1306 controller
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_OFF(void)
{
  if(iS_SSD_On){
#ifdef  DEBUG_UI
    uint32_t i = 0;
    for(i=0;i<ssd1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;i++){
      ssd1306_Buffer[i] = 0;
    }
#else
    // Turn off the display
    WriteCommand(0xae);
    
    // Set Charge pump
    WriteCommand(0x8D); /* Set Charge pump */
    WriteCommand(0x10); /* 0x14=ON, 0x10=Off */
#endif
    iS_SSD_On = 0;
  }
  return iS_SSD_On;
}


/*******************************************************************************
* Function Name  : ssd1306_ON
* Description    : Turn on the ssd1306 controller
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_ON(void)
{
  if(!iS_SSD_On){
#ifdef  DEBUG_UI
#else
  #ifdef    DEBUG_BOARD
  #else
    // Set Charge pump
    WriteCommand(0x8D); /* Set Charge pump */
    WriteCommand(0x14); /* 0x14=ON, 0x10=Off */
  #endif
    // Turn on the display
    WriteCommand(0xaf);
#endif
    iS_SSD_On = 1;
  }
  return iS_SSD_On;
}


/*******************************************************************************
* Function Name  : ssd1306_IsOn
* Description    : Check whether the ssd1306 is on or off
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_IsOn(void)
{
  return iS_SSD_On;
}

/*******************************************************************************
* Function Name  : OnPageTransferDone
* Description    : Called when each page transfer done
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void  OnPageTransferDone(void)
{
  if(pageIndex == SSD1306_PAGE_NUMBER){
    pageIndex = 0;
    return;
  }
#ifdef    DEBUG_BOARD
  WriteCommand(0xb0 + pageIndex);
  if(pageIndex == 0){
    WriteCommand(0x00);
    WriteCommand(0x10);
  }
  SSD_A0_High();
  DMA1_Channel5->CCR &= ((uint32_t)0xFFFFFFFE);
  DMA1_Channel5->CNDTR = ssd1306_COLUMN_NUMBER+SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END;
  DMA1_Channel5->CMAR = (uint32_t)(ssd1306_Buffer+SSD1306_COLUMN_NUMBER*pageIndex - SSD1306_COLUMN_MARGIN_START);
  DMA_SSD_1306->CCR |= ((uint32_t)0x00000001);
  pageIndex++;
#else
  SSD_A0_High();
  DMA_SSD_1306->CCR &= ((uint32_t)0xFFFFFFFE);
  DMA_SSD_1306->CNDTR = SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;//+SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END;
  DMA_SSD_1306->CMAR = (uint32_t)(ssd1306_Buffer);//+SSD1306_COLUMN_NUMBER*pageIndex);
  //DMA_Cmd(DMA_SSD_1306, ENABLE);
  DMA_SSD_1306->CCR |= ((uint32_t)0x00000001);
//  pageIndex++;
  pageIndex = SSD1306_PAGE_NUMBER;
#endif
}

/*******************************************************************************
* Function Name  : DMA1_Channel5_IRQHandler
* Description    : This function handles DMA1 Channel 5 interrupt request.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void DMA_Handler_SSD_1306(void) //DMA1_Channel5_IRQHandler(void)
{
  if(DMA_GetITStatus(DMA1_IT_TC5)){
    DMA_ClearITPendingBit(DMA1_IT_GL5);
    OnPageTransferDone();
  }
}

/*******************************************************************************
* Function Name  : StartPageTransfer
* Description    : Start a new page transfer
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void StartPageTransfer(void)
{
  pageIndex = 0;
  if(pre_on){
    if(iS_SSD_On == 0){
      pre_on = ssd1306_ON();
    }
  }else{
    if(iS_SSD_On){
      pre_on = ssd1306_OFF();
    }
  }
  if( curContrast != lastContrast ){
    lastContrast = curContrast;
    WriteCommand(0x81); /* Set Contrast Control */
    WriteCommand(lastContrast); /* 0 ~ 255 0x1f*/
  }
  
  if(iS_SSD_On){
    OnPageTransferDone();
  }
}

/*******************************************************************************
  Display related functions
 ******************************************************************************/
/*******************************************************************************
* Function Name  : ssd1306_DrawBlock
* Description    : Draw a block with specify data to ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_DrawBlock(
  Pos_t x,
  Pos_t y,
  Pos_t cx,
  Pos_t cy,
  const unsigned char* data)
{
  if(x >= SSD1306_X_PIXEL)return 0;
  if(y >= SSD1306_Y_PIXEL)return 0;
  if(x+cx > SSD1306_X_PIXEL ) cx = SSD1306_X_PIXEL - x ;
  if(y+cy > SSD1306_Y_PIXEL ) cy = SSD1306_Y_PIXEL - y ;
  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
  unsigned char offset1 = y%8;
  unsigned char offset2 = (cy+y)%8;
  if(data){
    unsigned char mask1 = (1<<offset1)-1;
    unsigned char mask2 = ~((1<<offset2)-1);
    cy = (cy+offset1)/8;
   
    Pos_t i = 0;
    for(i=0;i<cx;i++){
      Pos_t j = 0;
      unsigned short tmp = *pStart & mask1;
      for(;j<cy;j++){
        tmp |= (((unsigned short)data[j*cx])<<offset1);
        *(pStart + j*SSD1306_COLUMN_NUMBER) = tmp;
        tmp>>=8;
      }
      if(offset2){
        tmp |= ((((unsigned short)data[j*cx])<<offset1) & (~mask2))
            | *(pStart + j*SSD1306_COLUMN_NUMBER) & mask2;
        *(pStart + j*SSD1306_COLUMN_NUMBER) = tmp;
      }
      data++;
      pStart++;
    }
  }else{
    unsigned char mask1 = ~((1<<offset1)-1);
    unsigned char mask2 = ((1<<offset2)-1);
    cy = (cy+offset1)/8;

    Pos_t i = 0;
    for(i=0;i<cx;i++){
      Pos_t j = 1;
      *pStart ^= mask1;
      for(;j<cy;j++){
        *(pStart + j*SSD1306_COLUMN_NUMBER) ^= 0xFF;
      }
      if(offset2){
        *(pStart + j*SSD1306_COLUMN_NUMBER) ^= mask2;
      }
      pStart++;
    }
  }
  
  
//  if(!offset){
//    for(Pos_t i=0;i<cx;i++){
//      unsigned char tmp;
//      Pos_t j = 0;
//      for(;j<cy/8;j++){
//        tmp = data[i+j*cx];
//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
//      }
//      if(cy&0x7){
//        unsigned char mask1 = (1<<(cy&7)) - 1;
//        unsigned char mask2 = 0xFF - mask1;
//        tmp = (data[i+j*cx] & mask1)
//            | (*(pStart + j*ssd1306_COLUMN_NUMBER + i + x) & mask2);
//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
//      }
//    }
//  }else{
//    for(Pos_t i=0;i<cx;i++){
//      unsigned short tmp;
//      for(Pos_t j=0;j<cy/8;j++){
//        if(j == 0){
//          // First line
//          tmp = *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) & mask;
//        }
//        tmp |= (((unsigned short)data[i+j*cx])<<offset);
//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
//        tmp>>=8;
//        if(j == (cy/8)-1){
//          // Last line
//          *(pStart + (j+1)*ssd1306_COLUMN_NUMBER + i + x) &= ~mask;
//          *(pStart + (j+1)*ssd1306_COLUMN_NUMBER + i + x) |= tmp;
//        }
//      }
//    }
//  }
  
  return 0;
}

/*******************************************************************************
* Function Name  : ssd1306_DrawPoint
* Description    : Draw a point with specify data to ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_DrawPoint(
  Pos_t x,
  Pos_t y,
  Color_t color)
{
  if(x >= SSD1306_X_PIXEL)return 0;
  if(y >= SSD1306_Y_PIXEL)return 0;
  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
  unsigned char mask = 1<<(y%8);
  if(color){
    *pStart |= mask;
  }else{
    *pStart &= ~mask;
  }
  return 0;
}

/*******************************************************************************
* Function Name  : ssd1306_ReadPoint
* Description    : Read a point from ssd1306
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
unsigned long ssd1306_ReadPoint(
  Pos_t x,
  Pos_t y)
{
  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
  unsigned char mask = 1<<(y%8);
  return *pStart&mask ? 1 : 0;
}

/*******************************************************************************
* Function Name  : ssd1306_ClearScreen
* Description    : Clear the screen contents
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void  ssd1306_FillScreen(Color_t color)
{
  unsigned char mask = color ? 0xFF : 0;
  unsigned long i = 0;
  for(i=0;i<SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;i++){
    ssd1306_Buffer[i] = mask;
  }
}

const DeviceProp  ssd1306_Prop =
{
  .pfnDrawBlok = ssd1306_DrawBlock,
  .pfnDrawPoint = ssd1306_DrawPoint,
  .xPixel = 128,
  .yPixel = 64,
};

// Times New Roman font
const char fontData[240][5] =
{                                       // Refer to "Times New Roman" Font Database
                                        //   Basic Characters
    {0x00,0x00,0x00,0x00,0x00},         //   (  0)    - 0x0020 No-Break Space
    {0x00,0x00,0x4F,0x00,0x00},         //   (  1)  ! - 0x0021 Exclamation Mark
    {0x00,0x07,0x00,0x07,0x00},         //   (  2)  " - 0x0022 Quotation Mark
    {0x14,0x7F,0x14,0x7F,0x14},         //   (  3)  # - 0x0023 Number Sign
    {0x24,0x2A,0x7F,0x2A,0x12},         //   (  4)  $ - 0x0024 Dollar Sign
    {0x23,0x13,0x08,0x64,0x62},         //   (  5)  % - 0x0025 Percent Sign
    {0x36,0x49,0x55,0x22,0x50},         //   (  6)  & - 0x0026 Ampersand
    {0x00,0x05,0x03,0x00,0x00},         //   (  7)  ' - 0x0027 Apostrophe
    {0x00,0x1C,0x22,0x41,0x00},         //   (  8)  ( - 0x0028 Left Parenthesis
    {0x00,0x41,0x22,0x1C,0x00},         //   (  9)  ) - 0x0029 Right Parenthesis
    {0x14,0x08,0x3E,0x08,0x14},         //   ( 10)  * - 0x002A Asterisk
    {0x08,0x08,0x3E,0x08,0x08},         //   ( 11)  + - 0x002B Plus Sign
    {0x00,0x50,0x30,0x00,0x00},         //   ( 12)  , - 0x002C Comma
    {0x08,0x08,0x08,0x08,0x08},         //   ( 13)  - - 0x002D Hyphen-Minus
    {0x00,0x60,0x60,0x00,0x00},         //   ( 14)  . - 0x002E Full Stop
    {0x20,0x10,0x08,0x04,0x02},         //   ( 15)  / - 0x002F Solidus
    {0x3E,0x51,0x49,0x45,0x3E},         //   ( 16)  0 - 0x0030 Digit Zero
    {0x00,0x42,0x7F,0x40,0x00},         //   ( 17)  1 - 0x0031 Digit One
    {0x42,0x61,0x51,0x49,0x46},         //   ( 18)  2 - 0x0032 Digit Two
    {0x21,0x41,0x45,0x4B,0x31},         //   ( 19)  3 - 0x0033 Digit Three
    {0x18,0x14,0x12,0x7F,0x10},         //   ( 20)  4 - 0x0034 Digit Four
    {0x27,0x45,0x45,0x45,0x39},         //   ( 21)  5 - 0x0035 Digit Five
    {0x3C,0x4A,0x49,0x49,0x30},         //   ( 22)  6 - 0x0036 Digit Six
    {0x01,0x71,0x09,0x05,0x03},         //   ( 23)  7 - 0x0037 Digit Seven
    {0x36,0x49,0x49,0x49,0x36},         //   ( 24)  8 - 0x0038 Digit Eight
    {0x06,0x49,0x49,0x29,0x1E},         //   ( 25)  9 - 0x0039 Dight Nine
    {0x00,0x36,0x36,0x00,0x00},         //   ( 26)  : - 0x003A Colon
    {0x00,0x56,0x36,0x00,0x00},         //   ( 27)  ; - 0x003B Semicolon
    {0x08,0x14,0x22,0x41,0x00},         //   ( 28)  < - 0x003C Less-Than Sign
    {0x14,0x14,0x14,0x14,0x14},         //   ( 29)  = - 0x003D Equals Sign
    {0x00,0x41,0x22,0x14,0x08},         //   ( 30)  > - 0x003E Greater-Than Sign
    {0x02,0x01,0x51,0x09,0x06},         //   ( 31)  ? - 0x003F Question Mark
    {0x32,0x49,0x79,0x41,0x3E},         //   ( 32)  @ - 0x0040 Commercial At
    {0x7E,0x11,0x11,0x11,0x7E},         //   ( 33)  A - 0x0041 Latin Capital Letter A
    {0x7F,0x49,0x49,0x49,0x36},         //   ( 34)  B - 0x0042 Latin Capital Letter B
    {0x3E,0x41,0x41,0x41,0x22},         //   ( 35)  C - 0x0043 Latin Capital Letter C
    {0x7F,0x41,0x41,0x22,0x1C},         //   ( 36)  D - 0x0044 Latin Capital Letter D
    {0x7F,0x49,0x49,0x49,0x41},         //   ( 37)  E - 0x0045 Latin Capital Letter E
    {0x7F,0x09,0x09,0x09,0x01},         //   ( 38)  F - 0x0046 Latin Capital Letter F
    {0x3E,0x41,0x49,0x49,0x7A},         //   ( 39)  G - 0x0047 Latin Capital Letter G
    {0x7F,0x08,0x08,0x08,0x7F},         //   ( 40)  H - 0x0048 Latin Capital Letter H
    {0x00,0x41,0x7F,0x41,0x00},         //   ( 41)  I - 0x0049 Latin Capital Letter I
    {0x20,0x40,0x41,0x3F,0x01},         //   ( 42)  J - 0x004A Latin Capital Letter J
    {0x7F,0x08,0x14,0x22,0x41},         //   ( 43)  K - 0x004B Latin Capital Letter K
    {0x7F,0x40,0x40,0x40,0x40},         //   ( 44)  L - 0x004C Latin Capital Letter L
    {0x7F,0x02,0x0C,0x02,0x7F},         //   ( 45)  M - 0x004D Latin Capital Letter M
    {0x7F,0x04,0x08,0x10,0x7F},         //   ( 46)  N - 0x004E Latin Capital Letter N
    {0x3E,0x41,0x41,0x41,0x3E},         //   ( 47)  O - 0x004F Latin Capital Letter O
    {0x7F,0x09,0x09,0x09,0x06},         //   ( 48)  P - 0x0050 Latin Capital Letter P
    {0x3E,0x41,0x51,0x21,0x5E},         //   ( 49)  Q - 0x0051 Latin Capital Letter Q
    {0x7F,0x09,0x19,0x29,0x46},         //   ( 50)  R - 0x0052 Latin Capital Letter R
    {0x46,0x49,0x49,0x49,0x31},         //   ( 51)  S - 0x0053 Latin Capital Letter S
    {0x01,0x01,0x7F,0x01,0x01},         //   ( 52)  T - 0x0054 Latin Capital Letter T
    {0x3F,0x40,0x40,0x40,0x3F},         //   ( 53)  U - 0x0055 Latin Capital Letter U
    {0x1F,0x20,0x40,0x20,0x1F},         //   ( 54)  V - 0x0056 Latin Capital Letter V
    {0x3F,0x40,0x38,0x40,0x3F},         //   ( 55)  W - 0x0057 Latin Capital Letter W
    {0x63,0x14,0x08,0x14,0x63},         //   ( 56)  X - 0x0058 Latin Capital Letter X
    {0x07,0x08,0x70,0x08,0x07},         //   ( 57)  Y - 0x0059 Latin Capital Letter Y
    {0x61,0x51,0x49,0x45,0x43},         //   ( 58)  Z - 0x005A Latin Capital Letter Z
    {0x00,0x7F,0x41,0x41,0x00},         //   ( 59)  [ - 0x005B Left Square Bracket
    {0x02,0x04,0x08,0x10,0x20},         //   ( 60)  \ - 0x005C Reverse Solidus
    {0x00,0x41,0x41,0x7F,0x00},         //   ( 61)  ] - 0x005D Right Square Bracket
    {0x04,0x02,0x01,0x02,0x04},         //   ( 62)  ^ - 0x005E Circumflex Accent
    {0x40,0x40,0x40,0x40,0x40},         //   ( 63)  _ - 0x005F Low Line
    {0x01,0x02,0x04,0x00,0x00},         //   ( 64)  ` - 0x0060 Grave Accent
    {0x20,0x54,0x54,0x54,0x78},         //   ( 65)  a - 0x0061 Latin Small Letter A
    {0x7F,0x48,0x44,0x44,0x38},         //   ( 66)  b - 0x0062 Latin Small Letter B
    {0x38,0x44,0x44,0x44,0x20},         //   ( 67)  c - 0x0063 Latin Small Letter C
    {0x38,0x44,0x44,0x48,0x7F},         //   ( 68)  d - 0x0064 Latin Small Letter D
    {0x38,0x54,0x54,0x54,0x18},         //   ( 69)  e - 0x0065 Latin Small Letter E
    {0x08,0x7E,0x09,0x01,0x02},         //   ( 70)  f - 0x0066 Latin Small Letter F
    {0x06,0x49,0x49,0x49,0x3F},         //   ( 71)  g - 0x0067 Latin Small Letter G
    {0x7F,0x08,0x04,0x04,0x78},         //   ( 72)  h - 0x0068 Latin Small Letter H
    {0x00,0x44,0x7D,0x40,0x00},         //   ( 73)  i - 0x0069 Latin Small Letter I
    {0x20,0x40,0x44,0x3D,0x00},         //   ( 74)  j - 0x006A Latin Small Letter J
    {0x7F,0x10,0x28,0x44,0x00},         //   ( 75)  k - 0x006B Latin Small Letter K
    {0x00,0x41,0x7F,0x40,0x00},         //   ( 76)  l - 0x006C Latin Small Letter L
    {0x7C,0x04,0x18,0x04,0x7C},         //   ( 77)  m - 0x006D Latin Small Letter M
    {0x7C,0x08,0x04,0x04,0x78},         //   ( 78)  n - 0x006E Latin Small Letter N
    {0x38,0x44,0x44,0x44,0x38},         //   ( 79)  o - 0x006F Latin Small Letter O
    {0x7C,0x14,0x14,0x14,0x08},         //   ( 80)  p - 0x0070 Latin Small Letter P
    {0x08,0x14,0x14,0x18,0x7C},         //   ( 81)  q - 0x0071 Latin Small Letter Q
    {0x7C,0x08,0x04,0x04,0x08},         //   ( 82)  r - 0x0072 Latin Small Letter R
    {0x48,0x54,0x54,0x54,0x20},         //   ( 83)  s - 0x0073 Latin Small Letter S
    {0x04,0x3F,0x44,0x40,0x20},         //   ( 84)  t - 0x0074 Latin Small Letter T
    {0x3C,0x40,0x40,0x20,0x7C},         //   ( 85)  u - 0x0075 Latin Small Letter U
    {0x1C,0x20,0x40,0x20,0x1C},         //   ( 86)  v - 0x0076 Latin Small Letter V
    {0x3C,0x40,0x30,0x40,0x3C},         //   ( 87)  w - 0x0077 Latin Small Letter W
    {0x44,0x28,0x10,0x28,0x44},         //   ( 88)  x - 0x0078 Latin Small Letter X
    {0x0C,0x50,0x50,0x50,0x3C},         //   ( 89)  y - 0x0079 Latin Small Letter Y
    {0x44,0x64,0x54,0x4C,0x44},         //   ( 90)  z - 0x007A Latin Small Letter Z
    {0x00,0x08,0x36,0x41,0x00},         //   ( 91)  { - 0x007B Left Curly Bracket
    {0x00,0x00,0x7F,0x00,0x00},         //   ( 92)  | - 0x007C Vertical Line
    {0x00,0x41,0x36,0x08,0x00},         //   ( 93)  } - 0x007D Right Curly Bracket
    {0x02,0x01,0x02,0x04,0x02},         //   ( 94)  ~ - 0x007E Tilde
    {0x08,0x14,0x2A,0x14,0x22},         //   ( 95) << - 0x00AB Left-Pointing Double Angle Quotation Mark
    {0x00,0x02,0x05,0x02,0x00},         //   ( 96)    - 0x00B0 Degree Sign
    {0x44,0x44,0x5F,0x44,0x44},         //   ( 97) +- - 0x00B1 Plus-Minus Sign
    {0x7E,0x20,0x20,0x10,0x3E},         //   ( 98)  u - 0x00B5 Micro Sign
    {0x22,0x14,0x2A,0x14,0x08},         //   ( 99) >> - 0x00BB Right-Pointing Double Angle Quotation Mark
    {0x30,0x48,0x45,0x40,0x20},         //   (100)  ? - 0x00BF Inverted Question Mark
    {0x22,0x14,0x08,0x14,0x22},         //   (101)  x - 0x00D7 Multiplcation Sign
    {0x08,0x08,0x2A,0x08,0x08},         //   (102)  + - 0x00F7 Division Sign
    {0x18,0x14,0x08,0x14,0x0C},         //   (103)    - 0x221E Infinity
    {0x44,0x4A,0x4A,0x51,0x51},         //   (104)  < - 0x2264 Less-Than or Equal to
    {0x51,0x51,0x4A,0x4A,0x44},         //   (105)  > - 0x2265 Greater-Than or Equal to
    {0x54,0x14,0x64,0x08,0x70},         //   (106)  .: - RF Symbol
    {0x70,0x7C,0x72,0x7C,0x70},         //   (107)  ^ - Lock symbol
    {0x70,0x5C,0x52,0x54,0x70},         //   (108)  / - Unlock symbol
    {0x0C,0x1E,0x3C,0x1E,0x0C},         //   (109)  <3 - Heart Symbol
    {0x18,0x22,0xFF,0x12,0x0C},         //   (110)  U - USB Symbol
};


void setStartPage(unsigned char d)
{
    WriteCommand(0xB0|d);       // Set Page Start Address for Page Addressing Mode
                                // Default => 0xB0 (0x00)
}
/* Below are functions used to configure the OLED */
void setStartColumn(unsigned char d)
{
    WriteCommand(0x00+d%16);    // Set Lower Column Start Address for Page Addressing Mode
    WriteCommand(0x10+d/16);    // Set Higher Column Start Address for Page Addressing Mode
                                // Default => 0x10
}


const uint8_t row[4][32] = { 


{0x00,0x00,0x01,0x03,0x07,0x0F,0x1E,0x3C,0x3C,0x7C,0x7C,0x7C,0xFC,0xFF,0xFF,0xFC,0xFC,0xFC,0xFC,0xFF,0x7F,0x7F,0x7F,0x3C,0x3C,0x1C,0x0C,0x06,0x03,0x01,0x00,0x00},

{0x0F,0x7F,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x3F,0x3F,0x7F,0xFF,0xFF,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x7F,0x0F},

{0xF0,0xFE,0xFF,0xFF,0xFF,0xC7,0x00,0x00,0x00,0x00,0x87,0xC7,0xC7,0xFF,0xFF,0x00,0x00,0x00,0x00,0x87,0x87,0xC7,0xC3,0x03,0x07,0x07,0x0F,0x7F,0xFF,0xFF,0xFE,0xF0},

{0x00,0x00,0x80,0xC0,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE,0xFE,0xFF,0xFF,0xFF,0x1F,0x1F,0x1F,0x1F,0xFF,0xFE,0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0xC0,0x80,0x00,0x00},




};

void ssd1306_test()
{
    uint8_t i = 0;
    setStartPage(3);
    setStartColumn(0);
    for(i = 0; i<32; i++)
    {
        WriteData(row[0][i]);
    }

    WriteData(0x00);

    setStartPage(2);
    setStartColumn(0);
    for(i = 0; i<32; i++)
    {
        WriteData(row[1][i]);
    }
    WriteData(0x00);

    setStartPage(1);
    setStartColumn(0);
    for(i = 0; i<32; i++)
    {
        WriteData(row[2][i]);
    }
    WriteData(0x00);

    setStartPage(0);
    setStartColumn(0);
    for(i = 0; i<32; i++)
    {
        WriteData(row[3][i]);
    }
    WriteData(0x00);
}

/* Print a single character from font.cpp */
void ssd1306_DrawChar(char ascii, unsigned char row, unsigned char xPos)
{
    char *srcPointer = 0;
    unsigned char i;

    srcPointer = &fontData[(ascii-32)][0];

    setStartPage(row);
    setStartColumn(xPos);

    for(i=0;i<5;i++)
    {
        WriteData(*srcPointer);
        srcPointer++;
    }
    WriteData(0x00);
}

void ssd1306_DrawString(const char *dataPtr, unsigned char row, unsigned char xPos)
{
    char *srcPointer;

    srcPointer = (char*)dataPtr;
    ssd1306_DrawChar(' ',row,xPos); // NBSP must be written first before the string start

    while(1)
    {
        ssd1306_DrawChar(*srcPointer,row,xPos);
        srcPointer++;
        xPos+=6;
        if(*srcPointer == 0) break;
    }
}