#include <Servo.h>

Servo rotationServo;
Servo shootingServo;
const int trigPin = 9; // Connect GY-42 trig pin to Arduino pin 9
const int echoPin = 10; // Connect GY-42 echo pin to Arduino pin 10
const int buzzerPin = 8; // Connect buzzer to Arduino pin 8

int angles[25]; // Array to store sensor values for each 5 degrees rotation
int previousAngleValue = 0;

void setup() {
  rotationServo.attach(6); // Attach rotation servo to pin 6
  shootingServo.attach(7); // Attach shooting servo to pin 7
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT); // Set buzzer pin as output
  
  Serial.begin(9600); // Initialize serial communication for debugging
  delay(1000);
  
  // Beep twice after setup is complete
  beep(0.4, 2);
  
  // Rotate the rotation servo motor 120 degrees
  for (int angle = 120; angle >= 0; angle -= 5) {
    rotationServo.write(angle);
    delay(500);
    int distance = measureDistance();
    angles[angle / 5] = distance;
    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print(" degrees, Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
  }
}

void loop() {
  // Rotate the servo motor from 0 to 120 degrees
  for (int angle = 0; angle <= 120; angle += 5) {
    rotationServo.write(angle);
    delay(500);
    int distance = measureDistance();
    // Compare the current sensor value with the previous one with a margin of error of 10cm
    if (abs(distance - angles[angle / 5]) > 10) {
      // Trigger the shooting servo motor to move 10 degrees for 2 seconds
      shootingServo.write(180); // Move shooting servo to shoot
      beep(0.5, 3); // Beep three times when shooting
      delay(2000);
      shootingServo.write(90); // Reset shooting servo position
      delay(500);
      // Continue rotating the rotation servo motor
      rotationServo.write(angle);
      delay(500);
      angles[angle / 5] = distance; // Update the sensor value in the array
    }
  }
  
  // Rotate the servo motor back from 120 to 0 degrees
  for (int angle = 120; angle >= 0; angle -= 5) {
    rotationServo.write(angle);
    delay(500);
    int distance = measureDistance();
    // Compare the current sensor value with the previous one with a margin of error of 10cm
    if (abs(distance - angles[angle / 5]) > 10) {
      // Trigger the shooting servo motor to move 10 degrees for 2 seconds
      shootingServo.write(180); // Move shooting servo to shoot
      beep(0.5, 3); // Beep three times when shooting
      delay(2000);
      shootingServo.write(90); // Reset shooting servo position
      delay(500);
      // Continue rotating the rotation servo motor
      rotationServo.write(angle);
      delay(500);
      angles[angle / 5] = distance; // Update the sensor value in the array
    }
  }
}

int measureDistance() {
  // Measure distance using GY-42 ultrasonic sensor module
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  int duration = pulseIn(echoPin, HIGH);
  // Calculate the distance in centimeters
  int distance = duration * 0.034 / 2;
  
  return distance;
}

void beep(float duration, int times) {
  for (int i = 0; i < times; i++) {
    digitalWrite(buzzerPin, HIGH);
    delay(duration * 1000);
    digitalWrite(buzzerPin, LOW);
    delay(100); // Pause between beeps
  }
}