//#define SOFTWARE_I2C
#define HARDWARE_I2C_USE_PA0809

#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

uint8_t OLED_BUFF[1024] = {0};
uint8_t OLED_DIRT[1024] = {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();
}
bool OLEDdata_is_on = false;
void OLEDbegin_data(){
  if (OLEDdata_is_on) return;
  WIRE.beginTransmission(I2C_ADDRESS);
  WIRE.write(0x40);
  OLEDdata_is_on = true;
}
void OLEDput_data(uint8_t d){
  WIRE.write(d);
}
void OLEDend_data(){
  if (!OLEDdata_is_on) return;
  WIRE.endTransmission();
  OLEDdata_is_on = false;
}


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
  
  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);
  }
  
  
  for (int i = 0; i < 1024; i++){
    OLED_BUFF[i] = 0;
    OLED_DIRT[i] = 0;
  }
}

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

void OLEDpixel(int x, int y){
  if (x < 0 || x >= 128) return;
  if (y < 0 || y >= 64 ) return;
  y = 63-y;
  int row = y / 8;
  OLED_DIRT[ row * 128 + x] |= (1 << (y % 8));
}

void OLEDunpixel(int x, int y){
  if (x < 0 || x >= 128) return;
  if (y < 0 || y >= 64 ) return;
  y = 63-y;
  int row = y / 8;
  OLED_DIRT[ row * 128 + x] &= ~(1 << (y % 8));
}

void OLEDclear(){
  for (int i = 0; i < 1024; i++){
    OLED_DIRT[i] = 0;
  }
}

void OLEDflush(){
  for (int i = 0; i < 8; i++){
    int j0 = -1;
    for (int j = 0; j < 128; j++){
      int idx = i*128+j;
      uint8_t d = OLED_DIRT[idx];
      if (d != OLED_BUFF[idx]){
        if (j0 == -1){
          j0 = j;
          OLEDgoto(i,j);
          OLEDbegin_data();
        }
//        OLEDdata(d);
        OLEDput_data(d);
        OLED_BUFF[idx] = d;
      }else{
        OLEDend_data();
        j0 = -1;
      }
    }
  }
}


#define LINE_STEP(sx0,sx1,sy0,sy1,x) (((sx0) == (sx1)) ? (sy1) : ((int)round((float)(sy0)+(float)((x)-(sx0))*(float)((sy1)-(sy0))/(float)((sx1)-(sx0)))) )

void OLEDline(float x0 ,float y0 ,float x1 ,float y1){
  int sx0 = (int)round(x0);
  int sy0 = (int)round(y0);
  int sx1 = (int)round(x1);
  int sy1 = (int)round(y1);
  int dx  = sx1 > sx0 ? 1 : -1; // x increment
  int dy  = sy1 > sy0 ? 1 : -1; // y increment
  if (abs(sx0 - sx1) >= abs(sy0 - sy1)){ // slope < 1 ?
    int x = sx0;
    while (x != sx1+dx){
      int y = LINE_STEP(sx0,sx1,sy0,sy1,x);
      OLEDpixel(x,y);
      x += dx;
    }
  }else{
    int y = sy0;
    while (y != sy1+dy){
      int x = LINE_STEP(sy0,sy1,sx0,sx1,y);
      OLEDpixel(x,y);
      y += dy;
    }
  }
}

#define PT_IN_PL(x,y,x0,y0,x1,y1) ((((x)-(x0))*((y1)-(y0)) - ((y)-(y0))*((x1)-(x0)))<=0)
#define PT_IN_TRI(x,y,x0,y0,x1,y1,x2,y2) ( PT_IN_PL(x,y,x0,y0,x1,y1) && PT_IN_PL(x,y,x1,y1,x2,y2) && PT_IN_PL(x,y,x2,y2,x0,y0) )

void OLEDtriangle(float x0 ,float y0 ,float x1 ,float y1 ,float x2 ,float y2, char* pttn, int pttn_w, int pttn_h){
  if (!PT_IN_PL(x0,y0,x1,y1,x2,y2)){
    float x_tmp = x1;
    float y_tmp = y1;
    x1 = x2;
    y1 = y2;
    x2 = x_tmp;
    y2 = y_tmp;
  }
  int xmin = (int)floor(fmin(fmin(x0,x1),x2));
  int xmax = (int) ceil(fmax(fmax(x0,x1),x2));
  int ymin = (int)floor(fmin(fmin(y0,y1),y2));
  int ymax = (int) ceil(fmax(fmax(y0,y1),y2));

  int y; for (y=ymin; y <= ymax; y++){
    int x; for (x=xmin; x <= xmax; x++){

      if (PT_IN_TRI((float)x,(float)y,x0,y0,x1,y1,x2,y2)){
        bool v = '1' ==pttn[(y % pttn_h) * pttn_w + (x % pttn_w)];
        if (v) {
          OLEDpixel(x,y);
        }else{
          OLEDunpixel(x,y);
        }
      }
    }
  }
}


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
  
  WIRE.begin();

  delay(1000);
  hello_oled();

  Serial.begin(9600);
  
}


void hello_oled(){

  OLEDrestart();
  
  OLEDtriangle(60,10, 100,30, 50,40, "1000", 2,2);
  OLEDtriangle(10,40, 30,60, 5,55, "1", 1,1);
  OLEDline(50,10,120,60);
  OLEDline(10,30,120,50);

  OLEDflush();
  
}


void loop() {

}