#include <BlynkSimpleEsp32.h>
#define echoPin 19 
#define trigPin 18 
long duration, distance; 

const int majuP = 34; //push untuk motor maju 1
const int munP = 35; //push untuk motor mundur 2
const int majuLS = 32; //LS untuk motor maju 3
const int munLS = 33; //LS untuk motor mundur 4
const int seleP = 25; //push untuk sele 5

#define maju 23
#define mundur 22
#define sele 21

int butState = 0;
int lastButState = 0;
int butState1 = 0;
int lastButState1 = 0;
int butState2 = 0;
int lastButState2 = 0;

#define BLYNK_PRINT Serial
#define BLYNK_TEMPLATE_ID "TMPL6L4xiRaR3"
#define BLYNK_TEMPLATE_NAME "Smart Home"
#define BLYNK_AUTH_TOKEN "SzzSAJtGlPyc4EcELX987xBxMBmwC_vh"

char auth[] =  BLYNK_AUTH_TOKEN; 
char ssid[] = "Wokwi-GUEST";
char pass[] = "";


BLYNK_WRITE(V0)
{
  int pinValue = param.asInt();
  if (pinValue == 1){
    digitalWrite(mundur, HIGH);}
  else {
    digitalWrite(mundur, LOW);}
}

BLYNK_WRITE(V1)
{
  int pinValue = param.asInt();
  if (pinValue == 1){
    digitalWrite(maju, HIGH);}
  else {
    digitalWrite(maju, LOW);}
}

BLYNK_WRITE(V2)
{
  int pinValue = param.asInt();
  if (pinValue == 1){
    digitalWrite(sele, HIGH);}
  else {
    digitalWrite(sele, LOW);}
}

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(majuP, INPUT_PULLUP);
  pinMode(munP, INPUT_PULLUP);
  pinMode(majuLS, INPUT_PULLUP);
  pinMode(munLS, INPUT_PULLUP);
  pinMode(seleP, INPUT_PULLUP);
  pinMode(maju, OUTPUT);
  pinMode(mundur, OUTPUT);
  pinMode(sele, OUTPUT);
  Serial.begin(115200);
  Blynk.begin(auth, ssid, pass);
}

void bukaDalam(){
butState = digitalRead(munP);   
  while(butState) {
    digitalWrite(mundur, HIGH);
    Serial.println("gerbang buka");
    if(munLS){
      digitalWrite(mundur, LOW);
      Serial.println("Stop");
      break;
    }
  } 
  // lastButState = butState;
}

void tutupDalam(){
  butState1 = digitalRead(majuP); 
  while(butState1) {
    digitalWrite(maju, HIGH);
    Serial.println("gerbang tutup");
    if(majuLS){
      digitalWrite(maju, LOW);
      Serial.println("Stop");
      break;
    }
  }
  ultra();
  // lastButState1 = butState1;
}

void Sele(){
  butState2 = digitalRead(seleP); 
  if(butState2 != lastButState2 && butState2 == HIGH) {
    digitalWrite(sele, !digitalRead(sele));
    Serial.println("sele");
   }
  lastButState2 = butState2;
}

void ultra(){
  if(distance < 15){
    Serial.print(distance);
    digitalWrite(maju, LOW);
    digitalWrite(mundur, LOW);
  }
}

void loop() {
  digitalWrite(trigPin, LOW);delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration/58.2;

  bukaDalam();
  tutupDalam();
  Sele();

  Blynk.run();
  delay(500);
}