#include <Servo.h>

const int trigDiameterPin = 2;
const int echoDiameterPin = 3;

const int trigHeightPin = 4;
const int echoHeightPin = 5;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

const float object1D = 25.0;
const float object1H = 40.0;

const float object2D = 25.0;
const float object2H = 35.0;

const float object3D = 20.0;
const float object3H = 30.0;

const float object4D = 20.0;
const float object4H = 30.0;

const float tolerance = 2.0;

void setup() {
  pinMode(trigDiameterPin, OUTPUT);
  pinMode(echoDiameterPin, INPUT);

  pinMode(trigHeightPin, OUTPUT);
  pinMode(echoHeightPin, INPUT);

  motor1.attach(6);
  motor2.attach(7);
  motor3.attach(8);
  motor4.attach(9);
}

void loop() {
  digitalWrite(trigDiameterPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigDiameterPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigDiameterPin, LOW);

  long diameterDuration = pulseIn(echoDiameterPin, HIGH);

  float diameterDistance = diameterDuration * 0.034 / 2;

  digitalWrite(trigHeightPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigHeightPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigHeightPin, LOW);

  long heightDuration = pulseIn(echoHeightPin, HIGH);

  float heightDistance = heightDuration * 0.034 / 2;

  float expectedDiameter1 = object1D;
  float expectedDiameter2 = object2D;
  float expectedDiameter3 = object3D;
  float expectedDiameter4 = object4D;

  float expectedHeight1 = object1H;
  float expectedHeight2 = object2H;
  float expectedHeight3 = object3H;
  float expectedHeight4 = object4H;

  if (abs(diameterDistance - expectedDiameter1) <= tolerance && abs(heightDistance - expectedHeight1) <= tolerance) {
    motor1.write(90);
    motor2.write(0);
    motor3.write(0);
    motor4.write(0);
  } else if (abs(diameterDistance - expectedDiameter2) <= tolerance && abs(heightDistance - expectedHeight2) <= tolerance) {
    motor1.write(0);
    motor2.write(90);
    motor3.write(0);
    motor4.write(0);
  } else if (abs(diameterDistance - expectedDiameter3) <= tolerance && abs(heightDistance - expectedHeight3) <= tolerance) {
    motor1.write(0);
    motor2.write(0);
    motor3.write(90);
    motor4.write(0);
  } else if (abs(diameterDistance - expectedDiameter4) <= tolerance && abs(heightDistance - expectedHeight4) <= tolerance) {
    motor1.write(0);
    motor2.write(0);
    motor3.write(0);
    motor4.write(90);
  } else {
    motor1.write(0);
    motor2.write(0);
    motor3.write(0);
    motor4.write(0);
  }

  delay(100);
}