#include <HCSR04.h>
#include <Bounce2.h>
#include <TM1637Display.h>
#define CLK 3
#define DIO 4
TM1637Display display(CLK, DIO);
const int relayUp = 7;  // insert pin number for motors going up
const int relayDown = 7; // insert pin number for motors going down
const int triggerPin = 13; //
const int echoPin = 12;
int desiredDistance;
int presetNumber;
#define NUM_BUTTONS 4
const uint8_t BUTTON_PINS[NUM_BUTTONS] = {53, 51, 49, 47};  // insert preset button pin numbers
Bounce * buttons = new Bounce[NUM_BUTTONS];
UltraSonicDistanceSensor distanceSensor(triggerPin, echoPin);
int distance = distanceSensor.measureDistanceCm();
const int ledPin0 =  9;  // insert LED pin number for preset 1
const int ledPin1 =  9;  // insert LED pin number for preset 2
const int ledPin2 =  9;  // insert LED pin number for preset 3
const int ledPin3 =  9;  // insert LED pin number for preset 4
const int ledUp =  9;    // insert pin number for LED when motors going up
const int ledDown =  9;  // insert pin number for LED when motors going down
bool needToToggleMotor = false;
bool buttonUp = true;
bool buttonDown = true;


void setup() {
 for (int i = 0; i < NUM_BUTTONS; i++) {
    buttons[i].attach(BUTTON_PINS[i] , INPUT_PULLUP);
    buttons[i].interval(25);
 }
  pinMode(relayUp, OUTPUT);
  pinMode(relayDown, OUTPUT);
  pinMode(ledPin0, OUTPUT);
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(ledPin3, OUTPUT);
  pinMode(ledUp, OUTPUT);
  pinMode(ledDown, OUTPUT);
  pinMode(23, INPUT_PULLUP);  //insert button Up pin number
  pinMode(25, INPUT_PULLUP);  //insert button Down pin number
  display.setBrightness(5);
  display.showNumberDec(distance);
}

void loop() {
    buttonUp = digitalRead(23);
  buttonDown = digitalRead(25);
 for (int i = 0; i < NUM_BUTTONS; i++)  {
    buttons[i].update();
     if ( buttons[i].fell() ) {
       presetNumber = i;
       needToToggleMotor = true;
       distance = distanceSensor.measureDistanceCm();
        switch (presetNumber){
        case 0:
        desiredDistance = 3;  // insert desired distance from floor in cm for preset 1
        digitalWrite(ledPin0, HIGH);
        break;

        case 1:
        desiredDistance = 5;  // insert desired distance from floor in cm for preset 2
        digitalWrite(ledPin1, HIGH);
        break;

        case 2:
        desiredDistance = 7;  // insert desired distance from floor in cm for preset 3
        digitalWrite(ledPin2, HIGH);
        break;

        case 3:
        desiredDistance = 9;  // insert desired distance from floor in cm for preset 4
        digitalWrite(ledPin3, HIGH);
        break;
        }
    }
 }
 if (distance > desiredDistance && needToToggleMotor == true) {
   distance = distanceSensor.measureDistanceCm();
   display.showNumberDec(distance);
   digitalWrite(relayDown, HIGH);
}
else if (distance < desiredDistance && needToToggleMotor == true) {
   distance = distanceSensor.measureDistanceCm();
   display.showNumberDec(distance);
   digitalWrite(relayUp, HIGH);
}
else if (buttonUp == false){
  distance = distanceSensor.measureDistanceCm();
   display.showNumberDec(distance);
   digitalWrite(ledUp, HIGH);
   digitalWrite(relayUp, HIGH);
  }
else if (buttonDown == false){
  distance = distanceSensor.measureDistanceCm();
   display.showNumberDec(distance);
  digitalWrite(ledDown, HIGH);
  digitalWrite(relayDown, HIGH);
  }
else {
       buttonUp = true;
       buttonDown = true;
       needToToggleMotor = false;
       digitalWrite(ledPin0, LOW);
       digitalWrite(ledPin1, LOW);
       digitalWrite(ledPin2, LOW);
       digitalWrite(ledPin3, LOW);
       digitalWrite(ledUp, LOW);
       digitalWrite(ledDown, LOW);
       digitalWrite(relayUp, LOW);
       digitalWrite(relayDown, LOW);
       display.clear();
}
  delay(60);
}