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

Servo servoX, servoY;

int servoAngleX = 0, servoAngleY = 0;
int receivedX = 0, receivedY = 0;
bool buttonPressed = false;
bool firstDataReceived = false;

int coordX = 0;
int coordY = 0;

const int increment = 10;
const int maxServoAngle = 180;
const float coordMultiplier = 1.5;

const int targetX = 600;
const int targetY = 600;
const int resetButtonPin = 6;

void receiveData(int byteCount) {
  if (byteCount >= 3) {
    receivedX = Wire.read();
    receivedY = Wire.read();
    buttonPressed = (Wire.read() != 0);
    firstDataReceived = true;
  }
}

void setup() {
  Wire.begin(8);             // Join I2C bus as slave with address 8
  Wire.onReceive(receiveData);

  pinMode(resetButtonPin, INPUT_PULLUP);

  servoX.attach(9);
  servoY.attach(10);
  servoX.write(servoAngleX);
  servoY.write(servoAngleY);
}

void loop() {
  if (!firstDataReceived) {
    // Waiting for data; do nothing
    return;
  }

  static int lastCoordX = 0;
  static int lastCoordY = 0;
  static bool gameOver = false;

  if (digitalRead(resetButtonPin) == LOW) {
    coordX = 0;
    coordY = 0;
    servoAngleX = 0;
    servoAngleY = 0;
    servoX.attach(9);
    servoY.attach(10);
    gameOver = false;
    delay(500);
  }

  if (gameOver) return;

  bool xActive = (receivedX > 130 || receivedX < 50);
  bool yActive = (receivedY > 130 || receivedY < 50);

  if (buttonPressed) {
    coordX += int(servoAngleX * coordMultiplier);
    coordY += int(servoAngleY * coordMultiplier);
    servoAngleX = 0;
    servoAngleY = 0;
    buttonPressed = false;
  } else {
    if (xActive && !yActive) {
      servoAngleX += increment;
      if (servoAngleX >= maxServoAngle) {
        coordX += int(servoAngleX * coordMultiplier);
        servoAngleX = 0;
      }
    } else if (yActive && !xActive) {
      servoAngleY += increment;
      if (servoAngleY >= maxServoAngle) {
        coordY += int(servoAngleY * coordMultiplier);
        servoAngleY = 0;
      }
    }
  }

  servoX.write(servoAngleX);
  servoY.write(servoAngleY);

  if (coordX != lastCoordX || coordY != lastCoordY) {
    Serial.print("X:");
    Serial.print(coordX);
    Serial.print(",Y:");
    Serial.println(coordY);
    lastCoordX = coordX;
    lastCoordY = coordY;
  }

  if (coordX >= targetX && coordY >= targetY) {
    Serial.println("GOAL REACHED!");
    servoX.detach();
    servoY.detach();
    gameOver = true;
  }

  delay(30);
}