#include <Servo.h>   // names the motor type

const int trigDiameterPin = 2;   // labels the arduino uno pins that connects to the ultrasonic sensors trig and echo pins
const int echoDiameterPin = 3;

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

Servo motor1;   // names the individual servos
Servo motor2;
Servo motor3;
Servo motor4;

const int objectD1 = 25;   // labels the expected distance input from the sensors in mm
const int objectH1 = 40;

const int objectD2 = 25;
const int objectH2 = 35;

const int objectD3 = 20;
const int objectH3 = 35;

const int objectD4 = 20;
const int objectH4 = 30;

const int tolerance = 2;   // labels the allowed tolerance for each measurement

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(trigHeightPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigHeightPin, LOW);

  long diameterDuration = pulseIn(echoDiameterPin, HIGH);

  int diameterDistance = diameterDuration * 0.034 / 2;

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

  long heightDuration = pulseIn(echoHeightPin, HIGH);

  int heightDistance = heightDuration * 0.034 / 2;

  int expectedDiameter1 = objectD1;
  int expectedDiameter2 = objectD2;
  int expectedDiameter3 = objectD3;
  int expectedDiameter4 = objectD4;

  int expectedHeight1 = objectH1;
  int expectedHeight2 = objectH2;
  int expectedHeight3 = objectH3;
  int expectedHeight4 = objectH4;

  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);
}