#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int SW_A = 2;
const int joyX_A = 3;
const int joyY_A = 2;

const int SW_B = 3;
const int joyX_B = 1;
const int joyY_B = 0;

boolean isInMenu = true;
int selectedIndex = 0;

const String gamemodes [] = { "Classic", "Gun Game", "Option 3", "Option 4" };
const String current_gamemode = "";
const int sizeOfGamemodes = sizeof(gamemodes)/sizeof(gamemodes[0]);

/*
* Connections
* SSD1306 OLED | Arduino Uno
* ---------------------------
*     VCC      |    +5V       (Vcc/power/+ve)
*     GND      |    GND       (Ground/-ve/0v)
*     SCL      |    A5        (Serial Clock Line)
*     SDA      |    A4        (Serial Data Line)
*/

const int SCREEN_WIDTH = 128;
const int SCREEN_HEIGHT = 64;
const int PLAYING_FIELD_HEIGHT = SCREEN_HEIGHT - 10;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

uint8_t current_ball_x = SCREEN_WIDTH / 2, current_ball_y = PLAYING_FIELD_HEIGHT / 2;
uint8_t reset_ball_x = SCREEN_WIDTH / 2, reset_ball_y = PLAYING_FIELD_HEIGHT / 2;

uint8_t ball_dir_x = 1, ball_dir_y = 1;

uint8_t PADDLE_HEIGHT = 10;
int PADDLE_POSITION_X_DELTA = 10;

int PADDLE_POSITION_X_P1 = 0 + PADDLE_POSITION_X_DELTA;
int PADDLE_POSITION_X_P2 = SCREEN_WIDTH - PADDLE_POSITION_X_DELTA;

int PADDLE_POSITION_Y_P1 = (PLAYING_FIELD_HEIGHT / 2) - (PADDLE_HEIGHT / 2);
int PADDLE_POSITION_Y_P2 = (PLAYING_FIELD_HEIGHT / 2) - (PADDLE_HEIGHT / 2);

int PLAYER1_SCORE = 0;
int PLAYER2_SCORE = 0;

void setup() {
  Serial.begin(9600);

  setup_buttons();

  setup_display();
}

void setup_buttons() {
  
  pinMode(SW_A, INPUT_PULLUP);
  pinMode(SW_B, INPUT_PULLUP);

}

void setup_display() {
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    while(true);
  }
}

void handleMenu(const int confirm, const int cancel) {

  const float voltage_player1 = map(analogRead(joyY_A), 0, 1023, -100, 100);
  
  display.clearDisplay();

  draw_headline();
  handle_options(voltage_player1);
  if (confirm == 0) {
    
    current_gamemode = gamemodes[selectedIndex];
    isInMenu = false;

    display.clearDisplay();

    drawCourt();
    display.display();
  
  } else {
    
    display.display();
    delay(100);
  }

  PLAYER1_SCORE = 0;
  PLAYER2_SCORE = 0;
}

void handle_options(const float voltage_player1) {

  if (voltage_player1 <= -50) {
    selectedIndex -= 1;
  }
  if (voltage_player1 >= 50) {
    selectedIndex += 1;
  }

  for (int i = 0; i < sizeOfGamemodes; i++) {
    display.println(gamemodes[i] + (selectedIndex == i ? " <--" : ""));
  }
}

void draw_headline() {

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(15, 0);
  display.println("Welcome to Pong !");

  display.drawLine(0, 12, 128, 12, WHITE);

  display.println("");

}

void loop() {

  const int confirm = digitalRead(SW_A);
  const int cancel = digitalRead(SW_B);

  if (isInMenu) {

    handleMenu(confirm, cancel);

  } else {

    if (cancel == 0) {
      isInMenu = true; 
    }

    updatePaddles();
    updateBall();

    drawScore();

    display.display();
    
  }
}

void updatePaddles() {
  display.drawFastVLine(PADDLE_POSITION_X_P1, PADDLE_POSITION_Y_P1, PADDLE_HEIGHT, BLACK);
  display.drawFastVLine(PADDLE_POSITION_X_P2, PADDLE_POSITION_Y_P2, PADDLE_HEIGHT, BLACK);

  int sensorValue_player1 = map(analogRead(joyY_A), 0, 1023, -100, 100);
  int sensorValue_player2 = map(analogRead(joyY_B), 0, 1023, -100, 100);

  const int new_paddle_position_p1 = PADDLE_POSITION_Y_P1 + int(sensorValue_player1/30);
  const int new_paddle_position_p2 = PADDLE_POSITION_Y_P2 + int(sensorValue_player2/30);
  
  if(new_paddle_position_p1 > 0 && new_paddle_position_p1 < PLAYING_FIELD_HEIGHT - PADDLE_HEIGHT) {
    PADDLE_POSITION_Y_P1 = new_paddle_position_p1;
  }
  if(new_paddle_position_p2 > 0 && new_paddle_position_p2 < PLAYING_FIELD_HEIGHT - PADDLE_HEIGHT) {
    PADDLE_POSITION_Y_P2 = new_paddle_position_p2;
  }

  display.drawFastVLine(PADDLE_POSITION_X_P1, PADDLE_POSITION_Y_P1, PADDLE_HEIGHT, WHITE);
  display.drawFastVLine(PADDLE_POSITION_X_P2, PADDLE_POSITION_Y_P2, PADDLE_HEIGHT, WHITE);
}



void updateBall() {
    display.drawPixel(current_ball_x, current_ball_y, BLACK);
    uint8_t new_ball_x = current_ball_x + ball_dir_x;
    uint8_t new_ball_y = current_ball_y + ball_dir_y;

// Bounce at player 1's wall (left)
// Player 2 (right) gets a point
    if (new_ball_x <= 0) {
        int reset_ball_y_offset = random(-15, 16); 
        ball_dir_x = -ball_dir_x;
        new_ball_x = reset_ball_x;
        reset_ball_y += reset_ball_y_offset;
        new_ball_y = reset_ball_y;
        PLAYER2_SCORE += 1;
        if (PLAYER2_SCORE >= 5) {
          displayWinner(2);
          return;
        }
    }

// Bounce at player 2's wall (right)
// Player 1 (left) gets a point
    if (new_ball_x >= SCREEN_WIDTH - 1) {
        int reset_ball_y_offset = random(-15, 16);
        ball_dir_x = -ball_dir_x;
        new_ball_x = reset_ball_x;
        reset_ball_y += reset_ball_y_offset;
        new_ball_y = reset_ball_y;
        PLAYER1_SCORE += 1;

        if (PLAYER1_SCORE >= 5) {
          displayWinner(2);
          return;
        }
    }

// bounce at player 1's paddle (left)
    if (new_ball_x == PADDLE_POSITION_X_P1 && PADDLE_POSITION_Y_P1 <= new_ball_y && new_ball_y <= PADDLE_POSITION_Y_P1 + PADDLE_HEIGHT){
        ball_dir_x = -ball_dir_x;
        new_ball_x += ball_dir_x;
    }

// bounce at player 2's paddle (right)
    if (new_ball_x == PADDLE_POSITION_X_P2 && PADDLE_POSITION_Y_P2 <= new_ball_y && new_ball_y <= PADDLE_POSITION_Y_P2 + PADDLE_HEIGHT){
        ball_dir_x = -ball_dir_x;
        new_ball_x += ball_dir_x;
    }


// bounce at the top or bottom of the screen
    if (new_ball_y == 0 || new_ball_y == 53) {
        ball_dir_y = -ball_dir_y;
        new_ball_y += ball_dir_y;
    }

    
    display.drawPixel(new_ball_x, new_ball_y, WHITE);
    current_ball_x = new_ball_x;
    current_ball_y = new_ball_y;
}

void drawCourt() 
{
    display.drawRect(0, 0, SCREEN_WIDTH, PLAYING_FIELD_HEIGHT, WHITE);
}

void drawScore() {
  const String score = String(PLAYER1_SCORE) + " : " + String(PLAYER2_SCORE);
  int scoreWidth = score.length() * 5;
  int xPosition = (128 - scoreWidth) / 2;

  display.fillRect(0, 54, 128, 10, BLACK);
  
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(xPosition, 56);

  display.println(score);
}


void displayWinner(int player) {
  const String winnerText = "Player " + String(player) + " Won!";
  int winnerTextWidth = winnerText.length() * 5;
  int xPosition = (128 - winnerTextWidth) / 2;

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(xPosition, SCREEN_HEIGHT / 2);


  display.println("Player " + String(player) + " Won!");
  display.display();
  delay(4000);  
  isInMenu = true;
}