#include <Servo.h>
 
const int trigDiameterPin = 2;    // Digital pin for the diameter sensor trigger
const int echoDiameterPin = 3;    // Digital pin for the diameter sensor echo
 
const int trigHeightPin = 4;      // Digital pin for the height sensor trigger
const int echoHeightPin = 5;      // Digital pin for the height sensor echo
 
const int switchPin = 6;          // Digital pin for the push-button switch
 
Servo motor1;                      // Servo motor for motor 1
Servo motor2;                      // Servo motor for motor 2
Servo motor3;                      // Servo motor for motor 3
Servo motor4;                      // Servo motor for motor 4
 
// Define dimensions for each object (D = diameter, H = height)
const float object1D = 25.0;       // Diameter of object 1 in mm
const float object1H = 40.0;       // Height of object 1 in mm
 
const float object2D = 25.0;       // Diameter of object 2 in mm
const float object2H = 35.0;       // Height of object 2 in mm
 
const float object3D = 20.0;       // Diameter of object 3 in mm
const float object3H = 35.0;       // Height of object 3 in mm
 
const float object4D = 20.0;       // Diameter of object 4 in mm
const float object4H = 30.0;       // Height of object 4 in mm
 
// Tolerance value for measurements
const float tolerance = 2.0;       // Tolerance in mm
 
bool systemOn = false;             // Flag to indicate if the system is turned on
 
void setup() {
  pinMode(trigDiameterPin, OUTPUT);
  pinMode(echoDiameterPin, INPUT);
  
  pinMode(trigHeightPin, OUTPUT);
  pinMode(echoHeightPin, INPUT);
  
  pinMode(switchPin, INPUT_PULLUP);  // Set switch pin as input with internal pull-up resistor
 
  motor1.attach(7);    // Attach servo motor 1 to pin 7
  motor2.attach(8);    // Attach servo motor 2 to pin 8
  motor3.attach(9);    // Attach servo motor 3 to pin 9
  motor4.attach(10);   // Attach servo motor 4 to pin 10
}
 
void loop() {
  // Check if the switch is pressed to toggle the system on/off
  if (digitalRead(switchPin) == LOW) {
    systemOn = !systemOn;  // Toggle the system state
    delay(100);            // Debounce delay
  }
 
  if (systemOn) {
    // Trigger the diameter sensor
    digitalWrite(trigDiameterPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigDiameterPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigDiameterPin, LOW);
 
    // Read the duration of the pulse from the diameter sensor
    long diameterDuration = pulseIn(echoDiameterPin, HIGH);
 
    // Convert the duration to distance in millimeters for diameter
    float diameterDistance = diameterDuration * 0.034 / 2;
 
    // Trigger the height sensor
    digitalWrite(trigHeightPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigHeightPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigHeightPin, LOW);
 
    // Read the duration of the pulse from the height sensor
    long heightDuration = pulseIn(echoHeightPin, HIGH);
 
    // Convert the duration to distance in millimeters for height
    float heightDistance = heightDuration * 0.034 / 2;
 
    // Calculate the expected distance based on object dimensions
    float expectedDiameter1 = object1D;
    float expectedDiameter2 = object2D;
    float expectedDiameter3 = object3D;
    float expectedDiameter4 = object4D;
 
    float expectedHeight1 = object1H;
    float expectedHeight2 = object2H;
    float expectedHeight3 = object3H;
    float expectedHeight4 = object4H;
 
    // Map the measured distances to servo motor angles based on the expected distances and tolerance
    if (abs(diameterDistance - expectedDiameter1) <= tolerance && abs(heightDistance - expectedHeight1) <= tolerance) {
      // Move servo motor 1
      motor1.write(90);  // Adjust the angle as needed
      motor2.write(0);
      motor3.write(0);
      motor4.write(0);
    } else if (abs(diameterDistance - expectedDiameter2) <= tolerance && abs(heightDistance - expectedHeight2) <= tolerance) {
      // Move servo motor 2
      motor1.write(0);
      motor2.write(90);  // Adjust the angle as needed
      motor3.write(0);
      motor4.write(0);
    } else if (abs(diameterDistance - expectedDiameter3) <= tolerance && abs(heightDistance - expectedHeight3) <= tolerance) {
      // Move servo motor 3
      motor1.write(0);
      motor2.write(0);
      motor3.write(90);  // Adjust the angle as needed
      motor4.write(0);
    } else if (abs(diameterDistance - expectedDiameter4) <= tolerance && abs(heightDistance - expectedHeight4) <= tolerance) {
      // Move servo motor 4
      motor1.write(0);
      motor2.write(0);
      motor3.write(0);
      motor4.write(90);  // Adjust the angle as needed
    } else {
      // Stop all servo motors if the distances are out of expected range
      motor1.write(0);
      motor2.write(0);
      motor3.write(0);
      motor4.write(0);
    }
  } else {
    // Turn off all servo motors if the system is turned off
    motor1.write(0);
    motor2.write(0);
    motor3.write(0);
    motor4.write(0);
  }
 
  // Wait for a moment before the next iteration
  delay(100);
}