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

// Define servo objects
Servo servo1;
Servo servo2;

// Define LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Adjust the address if needed

// Define pin assignments
const int button1Pin = PA2;
const int button2Pin = PA3;
const int servo1Pin = PA0;
const int servo2Pin = PA1;
const int trigPin = PA4;
const int echoPin = PA5;
const int buzzerPin = PA6;

// Define angles
const int defaultAngle = 0;
const int pressedAngle = 90;

void setup() {
  // Attach servos
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);

  // Set initial servo positions
  servo1.write(defaultAngle);
  servo2.write(defaultAngle);

  // Set button pins as input
  pinMode(button1Pin, INPUT_PULLUP); // Use pull-up to simulate button presses
  pinMode(button2Pin, INPUT_PULLUP);

  // Set ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Set buzzer pin as output
  pinMode(buzzerPin, OUTPUT);

  // Initialize Serial Monitor
  Serial.begin(9600);

  // Initialize LCD
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("System Ready");
  delay(2000);
  lcd.clear();
}

void loop() {
  // Read button states
  int button1State = !digitalRead(button1Pin); // Inverted due to pull-up
  int button2State = !digitalRead(button2Pin); // Inverted due to pull-up

  // Control servo 1
  if (button1State) {
    servo1.write(pressedAngle);
    Serial.println("Servo 1: 90 degrees");
  } else {
    servo1.write(defaultAngle);
    Serial.println("Servo 1: 0 degrees");
  }

  // Control servo 2
  if (button2State) {
    servo2.write(pressedAngle);
    Serial.println("Servo 2: 90 degrees");
  } else {
    servo2.write(defaultAngle);
    Serial.println("Servo 2: 0 degrees");
  }

  // Ultrasonic sensor measurement
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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

  // Check distance and trigger alert
  if (distance > 0 && distance < 10) { // Adjust threshold as needed
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Anda Menang!");
    digitalWrite(buzzerPin, HIGH);
    Serial.println("Ultrasonic: Anda Menang!");
  } else {
    lcd.clear();
    digitalWrite(buzzerPin, LOW);
    Serial.println("Ultrasonic: No detection");
  }

  // Small delay to stabilize readings
  delay(100);
}