//#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

#include<time.h>

uint8_t OLED_BUFF[1024] = {0};
uint8_t OLED_DIRT[1024] = {0};
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};

// ball config
int ball_x0;
int ball_y0;
int ball_dx = 1;
int ball_dy = 1;
int ball_x;
int ball_y;
int ball_speed = 50;

// players config
#define CONTROLLER_W 4
int winner = 0;
int p_y0 = 32;
int p1_y= p_y0;
int p2_y= p_y0;
int d_p1 = 1;
int d_p2 = -1;

int randrange(int lower, int upper) {
  return (rand() % (upper - lower + 1)) + lower;
}

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;
      }
    }
  }
}

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);
  OLEDrestart();

  // randomize ball starting point
  srand(time(0)); // sets seed for rand function
  ball_x0 = randrange(0, 127);
  ball_y0 = randrange(0, 63);

  Serial.begin(9600);
  
}

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 == '\0')
       break;
     pointer = chr-' ';
     for (offset = 0; offset < 3; ++offset) {
       OLEDdata(FONT[3*pointer+offset]);
     }
     OLEDdata(0);
     ++index;
   }
}


void drawPlayerController(int player_loc, bool isPlayerOne) {
  int x_offset = 2;
  if (!isPlayerOne) {
    x_offset += 125;
  }

  for (int j = 0; j < 64; ++j) {
    OLEDunpixel(x_offset, j);
  }
  
  for (int i = 0; i < (2 * CONTROLLER_W + 1); ++i) {
    int loc = player_loc - CONTROLLER_W + i;
    OLEDpixel(x_offset, loc);
  }
  
}


void loop() {
  if (!winner) {
    // draw ball
    ball_x = ball_x0 + ball_dx;
    ball_y = ball_y0 + ball_dy;
    OLEDpixel(ball_x, ball_y);
    OLEDunpixel(ball_x0, ball_y0);

    // move players
    p1_y += d_p1;
    p2_y += d_p2;
    if (p1_y >= 64 - CONTROLLER_W || p1_y <= CONTROLLER_W) {
      d_p1 = -d_p1;
    }
    if (p2_y >= 64 - CONTROLLER_W || p2_y <= CONTROLLER_W) {
      d_p2 = -d_p2;
    }

    // draw players
    drawPlayerController(p1_y, true); // player one
    drawPlayerController(p2_y, false); // player one

    // ball boundaries
    if (ball_y >= 63) {
      ball_dy = -1;
    }
    if (ball_y <= 1) {
      ball_dy = 1;
    }
    if (ball_x >= 127) {
      ball_dx = -1;
    }
    if (ball_x <= 2) {
      ball_dx = 1;
    }

    // update ball iteration
    ball_x0 = ball_x;
    ball_y0 = ball_y;

    // display
    OLEDflush();
    delay(ball_speed);
  } else {
    OLED3x7string_direct(7,4,"PLAYER 1 WON");
  }
}