#include <Arduino.h>
#include <Servo.h>

// Define pin numbers for components
#define TRIGGER_PIN 5
#define ECHO_PIN 6
#define HAND_MOTOR_PIN 3
#define CLIPPER_MOTOR_PIN 4

// Include the Servo library to control servo motors
Servo handMotor;
Servo clipperMotor;

// Define a class for the Ultrasonic Sensor
class UltrasonicSensor {
private:
  int triggerPin;
  int echoPin;

public:
  // Constructor to initialize the trigger and echo pins
  UltrasonicSensor(int triggerPin, int echoPin) : triggerPin(triggerPin), echoPin(echoPin) {
    // Set pin modes
    pinMode(triggerPin, OUTPUT);
    pinMode(echoPin, INPUT);
  }

  // Method to get distance measured by the ultrasonic sensor
  long getDistance() {
    // Send trigger signal
    digitalWrite(triggerPin, LOW);
    delayMicroseconds(2);
    digitalWrite(triggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(triggerPin, LOW);

    // Measure echo pulse and calculate distance
    return pulseIn(echoPin, HIGH) * 0.034 / 2; // Speed of sound is approximately 0.034 cm per microsecond
  }
};

// Setup function runs once when the microcontroller starts
void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Attach servo motors to their respective pins
  handMotor.attach(HAND_MOTOR_PIN);
  clipperMotor.attach(CLIPPER_MOTOR_PIN);
}

// Loop function runs repeatedly as long as the microcontroller is powered on
void loop() {
  // Create an instance of the UltrasonicSensor class with defined pins
  UltrasonicSensor ultrasonicSensor(TRIGGER_PIN, ECHO_PIN);
  
  // Get distance measured by the ultrasonic sensor
  long distance = ultrasonicSensor.getDistance();

  // Print the distance to the serial monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  // Check if an object is detected within 20cm
  if (distance <= 200) {
    // Object detected within 20cm, initiate grab sequence

    // Move hand motor downwards to grab the object
    handMotor.write(90);
    Serial.println("Hand motor moving downwards...");
    delay(4000); // Wait for 4 seconds

    // Close the clipper to grab the object securely
    clipperMotor.write(180);
    Serial.println("Clipper closing...");
    delay(1000); // Wait for 1 second

    // Object grabbed, wait for some time
    Serial.println("Object grabbed. Waiting...");
    delay(10000); // Wait for 10 seconds with the object grabbed

    // Release the object

    // Move hand motor downwards again
    handMotor.write(90);
    Serial.println("Hand motor moving downwards to release the object...");
    delay(1000); // Wait for 1 second

    // Open the clipper to release the object
    clipperMotor.write(0);
    Serial.println("Clipper opening...");
    delay(1000); // Wait for 1 second

    // Object released
    Serial.println("Object released.");
    delay(1000); // Wait for 2 seconds

    // Move hand motor back to the original position
    handMotor.write(45);
    Serial.println("Hand motor moving back to original position...");
    delay(2000); // Wait for 2 seconds
  } else {
    // No object detected within 20cm, remain idle
    Serial.println("No object detected within 20cm. Robot remains idle.");
    // Return hand and clipper motors to original position
    handMotor.write(45);     // original position
    clipperMotor.write(45);  // original position
  }

  // Adjust delay as needed
  delay(100);
}