#include "seeedOLED.h"

#include <cstring>

// 8x8 Font ASCII 32 - 127 Implemented
// Users can modify this to support more characters(glyphs)
// BasicFont is placed in code memory.

static const Uint8 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,0x14,0x7F,0x14,0x7F,0x14,0x00,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} 
};

SeeedOLED::SeeedOLED(I2C &i2c_if) : i2c(i2c_if){
  
}

void SeeedOLED::init(
    void (*wait_func)(const unsigned int &), const unsigned int &wait){
  sendCommand(Display_Off_Cmd);   //display off
  if(wait_func){wait_func(wait);}
  sendCommand(Display_On_Cmd);   //display on
  if(wait_func){wait_func(wait);}
  sendCommand(Normal_Display_Cmd);  //Set Normal Display (default)
  setPageMode();
}

void SeeedOLED::sendCommand(const Uint8 &command){
  Uint8 buf[] = {Command_Mode, command};
  i2c.write(i2c_address, (char *)buf, sizeof(buf));
}

void SeeedOLED::sendData(const Uint8 &data){
  Uint8 buf[] = {Data_Mode, data};
  i2c.write(i2c_address, (char *)buf, sizeof(buf));
}

template<std::size_t N>
static inline void send_command(I2C &i2c, const Uint8 (&command)[N]){
  Uint8 buf[N * 2];
  for(int i(0), j(0); i < N; ++i){
    buf[j++] = SeeedOLED::Command_Mode;
    buf[j++] = command[i];
  }
  i2c.write(SeeedOLED::i2c_address, (char *)buf, sizeof(buf));
}

template<std::size_t N>
static inline void send_data(I2C &i2c, const Uint8 (&data)[N]){
  Uint8 buf[N + 1];
  buf[0] = SeeedOLED::Data_Mode;
  std::memcpy(&buf[1], data, sizeof(data));
  i2c.write(SeeedOLED::i2c_address, (char *)buf, sizeof(buf));
}

void SeeedOLED::setBrightness(unsigned char brightness){
  const Uint8 command[] = {Set_Brightness_Cmd, brightness};
  send_command(i2c, command);
}

void SeeedOLED::setHorizontalMode(){
  addressingMode = HORIZONTAL_MODE;
  const Uint8 command[] = {
      0x20, // set addressing mode
      0x00}; // set horizontal addressing mode
  send_command(i2c, command);
}

void SeeedOLED::setPageMode(){
  addressingMode = PAGE_MODE;
  const Uint8 command[] = {
      0x20, // set addressing mode
      0x02}; // set page addressing mode
  send_command(i2c, command);
}

void SeeedOLED::setTextXY(const Uint8 &row, const Uint8 &column){
  const Uint8 command[] = {
      0xB0 + row, // set page address
      0x00 + (8 * column & 0x0F), // set column lower address
      0x10 + ((8 * column >> 4) & 0x0F)}; // set column higher address
  send_command(i2c, command);
}

void SeeedOLED::clearDisplay(){
  sendCommand(Display_Off_Cmd);   //display off
  for(int j(0); j < 8; ++j){  
    setTextXY(j, 0);    
    for(int i(0); i < 16; ++i){  //clear all columns
      putChar(' ');    
    }
  }
  sendCommand(Display_On_Cmd);   //display on
  setTextXY(0, 0);
}

static int char2font_index(const Uint8 &c){
  int index(c - 32);
  if(index < 0 || index > (127 - 32)){
    // Ignore non-printable ASCII characters.
    // This can be modified for multilingual font.
    index = ' ' - 32; // Space
  }
  return index;
}

void SeeedOLED::putChar(const Uint8 &c){
  send_data(i2c, basicFont[char2font_index(c)]);
}

void SeeedOLED::putString(const char *buf, const Uint8 &buf_length){
#if 1
  static const int font_width(sizeof(basicFont[0]) / sizeof(basicFont[0][0]));
  char buf2[1 + (font_width * 16)]; // max characters per a line is 16.
  if(buf_length == 0){return;}
  int j(0);
  buf2[j++] = Data_Mode;
  Uint8 length(buf_length > 16 ? 16 : buf_length);
  for(int i(0); i < length; ++i, j += font_width){
    std::memcpy(&buf2[j], &basicFont[char2font_index(buf[i])], font_width);
  }
  i2c.write(i2c_address, buf2, j);
#else
  for(unsigned int i(0); (i < buf_length); ++i){
    putChar(buf[i]);
  }
#endif
}

void SeeedOLED::putString(const char *c_str){
#if 1
  putString(c_str, std::strlen(c_str));
#else
  for(unsigned int i(0); c_str[i]; ++i){
    putChar(c_str[i]);
  }
#endif
}

void SeeedOLED::drawBitmap(const Uint8 *bitmaparray, const int &bytes){
  mode_t localAddressMode(addressingMode);
  if(addressingMode != HORIZONTAL_MODE){
    // Bitmap is drawn in horizontal mode   
    setHorizontalMode();
  }

  for(int i(0); i < bytes; ++i){
    sendData(bitmaparray[i]);
  }

  if(localAddressMode == PAGE_MODE){
    // If pageMode was used earlier, restore it.
    setPageMode(); 
  }
}

void SeeedOLED::setHorizontalScrollProperties(
    const Uint8 &direction, 
    const Uint8 &startPage, const Uint8 &endPage, const Uint8 &scrollSpeed){
/*
 * Use the following defines for 'direction' :
 *   Scroll_Left
 *   Scroll_Right
 * 
 * Use the following defines for 'scrollSpeed' :
 *   Scroll_2Frames
 *   Scroll_3Frames
 *   Scroll_4Frames
 *   Scroll_5Frames
 *   Scroll_25Frames
 *   Scroll_64Frames
 *   Scroll_128Frames
 *   Scroll_256Frames
 */

  const Uint8 command[] = {
      (Scroll_Right == direction) ? 0x26 : 0x27, // set page address
      0x00, startPage, scrollSpeed, endPage, 0x00, 0xFF};
  send_command(i2c, command);
}

void SeeedOLED::activateScroll(){
  sendCommand(Activate_Scroll_Cmd);
}

void SeeedOLED::deactivateScroll(){
  sendCommand(Dectivate_Scroll_Cmd);
}

void SeeedOLED::setNormalDisplay(){
  sendCommand(Normal_Display_Cmd);
}

void SeeedOLED::setInverseDisplay(){
  sendCommand(Inverse_Display_Cmd);
}

const Uint8 SeeedOLED::i2c_address = 0x3c;

const Uint8 SeeedOLED::Max_X = 127; //128 Pixels
const Uint8 SeeedOLED::Max_Y = 63;  //64  Pixels

const Uint8 SeeedOLED::Command_Mode = 0x80;
const Uint8 SeeedOLED::Data_Mode = 0x40;
const Uint8 SeeedOLED::Display_Off_Cmd = 0xAE;
const Uint8 SeeedOLED::Display_On_Cmd = 0xAF;
const Uint8 SeeedOLED::Normal_Display_Cmd = 0xA6;
const Uint8 SeeedOLED::Inverse_Display_Cmd = 0xA7;
const Uint8 SeeedOLED::Activate_Scroll_Cmd = 0x2F;
const Uint8 SeeedOLED::Dectivate_Scroll_Cmd = 0x2E;
const Uint8 SeeedOLED::Set_Brightness_Cmd = 0x81;

const Uint8 SeeedOLED::Scroll_Left = 0x00;
const Uint8 SeeedOLED::Scroll_Right = 0x01;

const Uint8 SeeedOLED::Scroll_2Frames = 0x7;
const Uint8 SeeedOLED::Scroll_3Frames = 0x4;
const Uint8 SeeedOLED::Scroll_4Frames = 0x5;
const Uint8 SeeedOLED::Scroll_5Frames = 0x0;
const Uint8 SeeedOLED::Scroll_25Frames = 0x6;
const Uint8 SeeedOLED::Scroll_64Frames = 0x1;
const Uint8 SeeedOLED::Scroll_128Frames = 0x2;
const Uint8 SeeedOLED::Scroll_256Frames = 0x3;