#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servoP1;
Servo servoP2;

// -------- Pins --------
const int joyP1 = A0;
const int joyP2 = A1;

const int servoP1Pin = 9;
const int servoP2Pin = 10;

const int trigP1 = 4;   // P1 goal sensor
const int echoP1 = 5;

const int trigP2 = 6;   // P2 goal sensor
const int echoP2 = 7;

const int ledP1 = 11;
const int ledP2 = 12;

const int startButton = 2;
const int buzzer = 8;   // ✅ NEW

// -------- Game Variables --------
int scoreP1 = 0;
int scoreP2 = 0;

bool gameStarted = false;

// ================= SETUP =================
void setup() {
  servoP1.write(90);
  servoP2.write(90);
  pinMode(trigP1, OUTPUT);
  pinMode(echoP1, INPUT);
  pinMode(trigP2, OUTPUT);
  pinMode(echoP2, INPUT);

  pinMode(ledP1, OUTPUT);
  pinMode(ledP2, OUTPUT);

  pinMode(startButton, INPUT_PULLUP);
  pinMode(buzzer, OUTPUT);  // ✅ NEW

  servoP1.attach(servoP1Pin);
  servoP2.attach(servoP2Pin);

  lcd.init();
  lcd.backlight();

  showStartScreen();
}

// ================= LOOP =================
void loop() {

  // Button always available
  if (digitalRead(startButton) == LOW) {
    delay(200);   // debounce
    startGame();
  }

  if (gameStarted) {
    movePaddles();
    checkGoals();
  }
}


// ================= FUNCTIONS =================

// Move servos based on joystick
void movePaddles() {

  int joy1 = analogRead(joyP1);
  int joy2 = analogRead(joyP2);

  int angle1 = map(joy1, 0, 1023, 0, 90);
  int angle2 = map(joy2, 0, 1023, 0, 90);

  servoP1.write(angle1);
  servoP2.write(angle2);
}

// Read ultrasonic distance
long readDistance(int trigPin, int echoPin) {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;

  return distance;
}

// Check for goals
void checkGoals() {

  long distP1 = readDistance(trigP1, echoP1);
  long distP2 = readDistance(trigP2, echoP2);

  // If puck enters P2 goal → P1 scores
  if (distP2 > 0 && distP2 < 5) {

    scoreP1++;
    digitalWrite(ledP1, HIGH);

    tone(buzzer, 1000, 150);  // ✅ GOAL SOUND

    showGoal("P1 SCORES!");
    delay(2000);

    digitalWrite(ledP1, LOW);
    updateScore();
  }

  // If puck enters P1 goal → P2 scores
  if (distP1 > 0 && distP1 < 5) {

    scoreP2++;
    digitalWrite(ledP2, HIGH);

    tone(buzzer, 1000, 150);  // ✅ GOAL SOUND

    showGoal("P2 SCORES!");
    delay(2000);

    digitalWrite(ledP2, LOW);
    updateScore();
  }

  checkWinner();
}

// Show start screen
void showStartScreen() {

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("ROBOTIC HOCKEY");
  lcd.setCursor(0, 1);
  lcd.print("Press Start");
}

// Start game
void startGame() {

  scoreP1 = 0;
  scoreP2 = 0;
  gameStarted = true;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Starting...");

  // ✅ Countdown beeps
  for (int i = 0; i < 3; i++) {
    tone(buzzer, 800, 150);
    delay(300);
  }

  delay(500);
  updateScore();
}

// Update score display
void updateScore() {

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("P1:");
  lcd.print(scoreP1);
  lcd.print("  P2:");
  lcd.print(scoreP2);

  lcd.setCursor(0, 1);
  lcd.print("First to 5");
}

// Show goal message
void showGoal(String message) {

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("GOAL!");
  lcd.setCursor(0, 1);
  lcd.print(message);
}

// Check winner
void checkWinner() {

  if (scoreP1 == 5) {
    showWinner("PLAYER 1 WINS!");
  }

  if (scoreP2 == 5) {
    showWinner("PLAYER 2 WINS!");
  }
}

// Show winner screen
void showWinner(String message) {

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(message);

  lcd.setCursor(0, 1);
  lcd.print(scoreP1);
  lcd.print(" - ");
  lcd.print(scoreP2);

  // ✅ Victory sound
  for (int i = 0; i < 3; i++) {
    tone(buzzer, 1200, 200);
    delay(300);
  }

  delay(3000);

  gameStarted = false;
  showStartScreen();
}