/*
  Author: Haniah Kring
  Version: v1.0
  Date: 8/28/22

  This script was developed to help detect when cars are at an unsafe distance
  while passing through the car wash. When three or more consecutive sensors are
  off (not detecting a car), they are at a safe distance. When two consecutive
  sensors are off, a signal is sent out to slow down the conveyor belt. When only
  one sensor is off, stop the belt to avoid any possible collisions.

*/

void setup() {

  // Initialize sensors as inputs
  pinMode(2, INPUT); // G
  pinMode(3, INPUT); // F
  pinMode(4, INPUT); // E
  pinMode(5, INPUT); // D
  pinMode(6, INPUT); // C
  pinMode(7, INPUT); // B
  pinMode(8, INPUT); // A


  // Set inputs to HIGH: once sensors are properly wired, inputs should default
  // to HIGH unless otherwise specified
  digitalWrite(2, HIGH);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);
  digitalWrite(5, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(7, HIGH);
  digitalWrite(8, HIGH);


  // Initialize Good, Slow, Stop outputs
  pinMode(11, OUTPUT); // Good
  pinMode(12, OUTPUT); // Slow
  pinMode(13, OUTPUT); // Stop
}


void loop() {

  // len of sensor array
  int len = 7;

  // array to hold sensor values
  int bin_array[len];

  // store sensor values in array
  for(int i = 0; i < len; i++) {
    bin_array[i] = digitalRead(i + 2);
  }

  // Convert binary -> decimal
  int result = bin_to_dec(bin_array, len);

  // Evaluate conditions for good / slow / stop
  if(result == 95 || result == 111 || result == 119 ||result == 123 || result == 125) { // Stop condition
    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
    digitalWrite(13, HIGH);
  }
  else if(result == 79 || result == 103 || result == 115 || result == 121) { // Slow condition
    digitalWrite(11, LOW);
    digitalWrite(12, HIGH);
    digitalWrite(13, LOW);
  }
  else { // Good condition or do nothing
    digitalWrite(11, HIGH);
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);
  }

}


// Helper function: converts binary array -> decimal number
int bin_to_dec(int array[], int len) {
    int output = 0;
    int power = 1;

    // loop through binary array
    for (int i = 0; i < len; i++)
    {
        output += array[(len - 1) - i] * power;
        // output goes 1*2^0 + 0*2^1 + 0*2^2 + ...
        power *= 2;
    }


    return output;
}