//#define SOFTWARE_I2C
//#define HARDWARE_I2C_USE_PA0809

#define SER Serial2

#define I2C_ADDRESS 0x3C

#ifdef SOFTWARE_I2C
  #define SDA_PIN 8
  #define SCL_PIN 9
  #include <SoftWire.h>
  SoftWire WIRE(SDA_PIN, SCL_PIN);
  #define SOFTWARE_I2C_BUFFER_SIZE 129
  char ibuffer[SOFTWARE_I2C_BUFFER_SIZE];
#else
  #include <Wire.h>
  #ifdef HARDWARE_I2C_USE_PA0809
    TwoWire WIRE(&PERIPH_WIRE1,PIN_WIRE1_SDA, PIN_WIRE1_SCL);
  #else
    #define WIRE Wire
  #endif
#endif

static const uint8_t FONT[] = {0,0,0,0,125,0,96,0,96,127,20,127,50,107,38,39,8,115,62,107,42,0,96,0,28,34,65,65,34,28,107,62,107,8,62,8,2,3,0,8,8,8,3,3,0,3,28,96,62,65,62,33,127,1,35,69,57,34,73,54,60,4,127,122,73,78,62,73,38,64,79,112,54,73,54,50,73,62,0,20,0,2,20,0,8,20,34,20,20,20,34,20,8,32,77,48,63,65,125,127,80,127,127,73,54,62,65,34,127,65,62,127,73,65,127,72,72,62,65,46,127,8,127,65,127,65,2,1,126,127,8,119,127,1,1,127,62,127,127,64,127,127,65,127,127,72,48,62,69,63,127,72,55,50,73,38,64,127,64,127,1,127,124,3,124,127,14,127,119,8,119,112,15,112,71,73,113,127,65,65,96,28,3,65,65,127,32,64,32,1,1,1,64,32,0};

void OLEDcommand(uint8_t c) {
  WIRE.beginTransmission(I2C_ADDRESS);
  static uint8_t data[2];
  data[0] = 0;
  data[1] = c;
  WIRE.write(data,2);
  WIRE.endTransmission();
}

void OLEDcommands(uint8_t c1,uint8_t c2) {
  WIRE.beginTransmission(I2C_ADDRESS);
  static uint8_t data[3];
  data[0] = 0;
  data[1] = c1;
  data[2] = c2;
  WIRE.write(data,3);
  WIRE.endTransmission();
}
void OLEDdata(uint8_t d) {
   WIRE.beginTransmission(I2C_ADDRESS);
   static uint8_t data[2];
   data[0] = 0x40;
   data[1] = d;
   WIRE.write(data,2);
   WIRE.endTransmission();
}

void OLEDclear(){
  for (int j = 0; j < 8; ++j) {
    OLEDcommands(0x00,0x9);
    OLEDcommand(0xB0+j);
    for (int i = 0; i < 128; ++i) OLEDdata(0);
    OLEDgoto(j,65);
    for (int i = 0; i < 10; ++i) OLEDdata(0);
  }
}

void OLEDrestart(){
  OLEDcommand(0xae); // display off
  OLEDcommands(0xa8,0x3f); // set multiplex ratio, ratio 63
  OLEDcommands(0xd3,0x00); // set display offset, no offset
  OLEDcommand(0x40); // set display start line
  OLEDcommand(0xa0); // set segment remap col 127 to seg 0
  OLEDcommand(0xc8); // set COM output reverse
  OLEDcommands(0xda,0x12); // COM pin config, alt bottom to top
  OLEDcommands(0x81,0xff); // set contrast, max contrast
  OLEDcommand(0xa4); // resume to RAM display
  OLEDcommand(0xa6); // normal non-inverted display
  OLEDcommands(0xd5,0x80); // set clock divider, default
  OLEDcommands(0x8d,0x14); // set charge pump, enable
  OLEDcommands(0x20,0x02); // set memory mode, page addressing
  OLEDcommand(0xaf); // display on

  OLEDclear();


}

void OLEDgoto(uint8_t row,uint8_t col){
  OLEDcommands(0x00+(col & 0x0F),0x10+((col >> 4) & 0x0F));
  OLEDcommand(0xB0+row);
}


void OLED3x7string_direct(uint8_t row,uint8_t col,char str[]) {
  static uint8_t index,offset,pointer;
  static char chr;
  OLEDgoto(row,col);
  index = 0;
  while (1) {
     chr = str[index];
     if (chr >= 'a' && chr <= 'z'){
      chr -= ('a'-'A');
     }
     if (chr == '\0')
       break;
     pointer = chr-' ';
     for (offset = 0; offset < 3; ++offset) {
       OLEDdata(FONT[3*pointer+offset]);
     }
     OLEDdata(0);
     ++index;
   }
}



void setup() {

  #ifdef SOFTWARE_I2C
    WIRE.setDelay_us(1);
    WIRE.enablePullups();
    WIRE.setRxBuffer(ibuffer, SOFTWARE_I2C_BUFFER_SIZE);
    WIRE.setTxBuffer(ibuffer, SOFTWARE_I2C_BUFFER_SIZE);
  #endif

  pinMode(2, OUTPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  
  for (int i = 0; i < 3; i++){
    digitalWrite(2,HIGH);
    delay(300);
    digitalWrite(2,LOW);
    delay(300);
  }
  digitalWrite(2,HIGH);
  
  
  WIRE.begin();
  OLEDrestart();  
  SER.begin(9600);
  Serial.begin(9600);


  OLED3x7string_direct(7,4,"Ready.");
}

char buff[513];
int idx = 0;
int row = 1;

int BTN_CURR[2] = {HIGH,HIGH};
int BTN_PREV[2] = {HIGH,HIGH};
const int BTNS[2] = {5,6};

void btn_update(){
  for (int i = 0; i < 2; i++){
    BTN_CURR[i] = digitalRead(BTNS[i]);
    if (BTN_CURR[i] == LOW && BTN_PREV[i] == HIGH){
      btn_flip(true,i);
    }
    if (BTN_CURR[i] == HIGH && BTN_PREV[i] == LOW){
      btn_flip(false,i);
    }
    BTN_PREV[i] = BTN_CURR[i];
  }
}

void btn_flip(bool dir, int idx){
  if (dir){
    digitalWrite(2,LOW);
    OLEDrestart();
    row = 0;
    delay(500);
    digitalWrite(2,HIGH);
  } 
}

void loop() {
  if (SER.available() > 0) {
    digitalWrite(2,LOW);
    
    char b = SER.read();
    if (b == '\n'){
      if (buff[0] == 'I' && buff[1] == 'M' && buff[2] == 'G'){
        for (int i = 0; i < 4; i++){
          OLEDgoto(7-i, 90);
          for (int j = 0; j < 32; j++){
            OLEDdata( buff[i*32+j+3] );
          }
        }
        idx = 0;
      }else{
        buff[idx] = 0;
        if (row > 7){
          OLEDclear();
          row = 0;
        }
        
        OLED3x7string_direct(7-row,4,buff);
        row ++;
        idx = 0;
      }
    }else{
      buff[idx++] = b;
    }
    digitalWrite(2,HIGH);
  }
  btn_update();
  
}