#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 buzzerPin = 6;          // Digital pin for the buzzer
 
const int motor1Pin = 7;          // Digital pin for servo motor 1
const int motor2Pin = 8;          // Digital pin for servo motor 2
const int motor3Pin = 9;          // Digital pin for servo motor 3
const int motor4Pin = 10;         // Digital pin for servo motor 4
 
const int led1Pin = 11;           // Digital pin for LED 1
const int led2Pin = 12;           // Digital pin for LED 2
const int led3Pin = 13;           // Digital pin for LED 3
const int led4Pin = A0;           // Digital pin for LED 4
 
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(buzzerPin, OUTPUT);      // Set buzzer pin as output
  
  pinMode(led1Pin, OUTPUT);        // Set LED 1 pin as output
  pinMode(led2Pin, OUTPUT);        // Set LED 2 pin as output
  pinMode(led3Pin, OUTPUT);        // Set LED 3 pin as output
  pinMode(led4Pin, OUTPUT);        // Set LED 4 pin as output
 
  motor1.attach(motor1Pin);        // Attach servo motor 1 to pin 7
  motor2.attach(motor2Pin);        // Attach servo motor 2 to pin 8
  motor3.attach(motor3Pin);        // Attach servo motor 3 to pin 9
  motor4.attach(motor4Pin);        // 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;
 
    // Check if the measured distances are within the tolerances
    bool withinTolerances = (abs(diameterDistance - expectedDiameter1) <= tolerance && abs(heightDistance - expectedHeight1) <= tolerance) ||
                            (abs(diameterDistance - expectedDiameter2) <= tolerance && abs(heightDistance - expectedHeight2) <= tolerance) ||
                            (abs(diameterDistance - expectedDiameter3) <= tolerance && abs(heightDistance - expectedHeight3) <= tolerance) ||
                            (abs(diameterDistance - expectedDiameter4) <= tolerance && abs(heightDistance - expectedHeight4) <= tolerance);
 
    // If the measured distances are outside of the tolerances, activate the buzzer
    if (!withinTolerances) {
      digitalWrite(buzzerPin, HIGH);
    } else {
      digitalWrite(buzzerPin, LOW);
    }
 
    // Map the measured distances to servo motor angles based on the expected distances and tolerance
    if (withinTolerances) {
      // Adjust servo motor angles based on the measured distances
      // ...
    } else {
      // Stop all servo motors if the distances are out of expected range
      // ...
    }
 
    // Control LEDs based on servo movements
    controlLEDs();
  } else {
    // Turn off all servo motors and LEDs if the system is turned off
    motor1.write(0);
    motor2.write(0);
    motor3.write(0);
    motor4.write(0);
    
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, LOW);
    digitalWrite(led3Pin, LOW);
    digitalWrite(led4Pin, LOW);
  }
 
  // Wait for a moment before the next iteration
  delay(100);
}
 
// Function to control LEDs based on servo movements