#include <Wire.h>
#include <TM1637.h>
#include <HCSR04.h>

HCSR04 hc1(15, 14); // (trig pin , echo pin) -> in
HCSR04 hc2(5, 4); // -> out

const int CLK = 11;
const int DIO = 10;

TM1637 tm(CLK, DIO);

String sequence = "";

int timeoutCounter = 0;
int currentPeople = 0;
int maxPeople = 5;

int distance1, distance2, initialDistance1, initialDistance2;
 
#define relay 8

unsigned int counter = 0;

void setup() {
  tm.init();
  tm.set(BRIGHT_TYPICAL);
  Serial.begin(9600);
  pinMode(relay, OUTPUT);
  digitalWrite(relay, HIGH);
  initialDistance1 = hc1.dist();
  initialDistance2 = hc2.dist();
}

void loop() {
  delay(200);// reading will be taken after 200 milliseconds
  distance1 = hc1.dist();
  distance2 = hc2.dist();

  //Process the data
  if(distance1 < initialDistance1 - 30 && sequence.charAt(0) != '1'){
    sequence += "1";
  }
  else if(distance2 < initialDistance2 - 30 && sequence.charAt(0) != '2'){
    sequence += "2";
  }
  
  if(sequence.equals("12")){
    currentPeople++;  
    sequence="";
    delay(550);
  }
  else if(sequence.equals("21") && currentPeople > 0){
    currentPeople--;
    sequence="";
    delay(550);
  }

  //Resets the sequence if it is invalid or timeouts
  if(sequence.length() > 2 || sequence.equals("11") || sequence.equals("22") || timeoutCounter > 200){
    sequence="";
  }

  if(sequence.length() == 1){ //
    timeoutCounter++;
  }
  else{
    timeoutCounter=0;
  }
 //Print values to serial
  Serial.print("Seq: ");
  Serial.println(sequence);
  Serial.print("S1: ");
  Serial.println(distance1);
  Serial.print("S2: ");
  Serial.println(distance2);
  Serial.print("Current people: ");
  Serial.println(currentPeople);
  
  //displayText(String(currentPeople));
      
  //If the number of people is high, trigger the buzzer
  /*if(currentPeople > maxPeople){
    tone(buzzerPin, 1, 500);
    displayText(String(currentPeople));
  }
  else{
    noTone(buzzerPin);
  }*/


}