//Defines Pin Numbers
const byte trigPin = 2;
const byte echoPin = 3;
const byte redLED = 4;
const byte buzzerPin = 5;
const byte greenLED = 7;


//Defines other constants and variables
const unsigned long threshold_cm = 106 ; // cm that is ~42 inches
unsigned long partDetectionStartTime = 0;
unsigned long partDetectionEndTime = 0;
const unsigned long idleDelay = 3000;

enum {READY_TO_FEED, PART_PRESENT, WAITING} state = READY_TO_FEED;

unsigned long distance() {
  static unsigned long previousDistance_cm = 0;
  const unsigned long delayBetweenPings = 60;
  static unsigned long previousPing = -delayBetweenPings;

  if (millis() - previousPing >= delayBetweenPings) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    previousPing = millis();
    previousDistance_cm = pulseIn(echoPin, HIGH) * 0.0343 / 2;
  }
  return previousDistance_cm;
}

void signalReady(bool ready) {
  if (ready) {
    digitalWrite(redLED, LOW);
    digitalWrite(greenLED, HIGH);
  } else {
    digitalWrite(redLED, HIGH);
    digitalWrite(greenLED, LOW);
  }
}

void setup() {
  pinMode(buzzerPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  signalReady(true);
  Serial.begin(115200);
}

void loop() {

  switch (state) {
    case READY_TO_FEED: // check if a part is detected
      if (distance() < threshold_cm) {
        partDetectionStartTime = millis();
        signalReady(false);
        state = PART_PRESENT;
        // debug
        Serial.println(F("Part in front of the sensor."));
      }
      break;

    case PART_PRESENT: // check if the part is gone
      if (distance() >= threshold_cm) { // consider an hysteresis
        partDetectionEndTime = millis();
        state = WAITING;
        // debug
        Serial.print(F("The part stayed "));
        Serial.print((partDetectionEndTime - partDetectionStartTime) / 1000.0, 2);
        Serial.println(F("s in front of the sensor."));
      }
      break;

    case WAITING: // should wait for idleDelay
      if (millis() - partDetectionEndTime >=  idleDelay) { // did we wait long enough ?
        signalReady(true);
        state = READY_TO_FEED;
        // debug
        Serial.println(F("OK, we waited long enough, get the next part."));
        tone(buzzerPin, 1500, 50); delay(80);
        tone(buzzerPin, 500, 50); 
      }
      else if (distance() < threshold_cm) { // operator did not wait for the right amount of time. Complain loudly
        tone(buzzerPin, 3000, 1000);
        Serial.println(F("OPERATOR MISTAKE!! Please wait long enough to get the next part."));
        partDetectionStartTime = millis();
        signalReady(false);
        state = PART_PRESENT;
      }
      break;
  }
}