#define pinVertical 35
#define pinHorizontal 12
#define pinPencet 23
#define pinEnableStepper 34
#define pinDirKiri 17
#define pinDirKanan 16
#define pinStep 13
#define pinTriggerSonic1 4
#define pinTriggerSonic2 5
#define pinEchoDepan 33
#define pinEchoBelakang 32
#define pinTemp1 14
#define pinTemp2 27
#define pinTemp3 26
#define pinTemp4 25
#define pinPiezzo 18

int X, Y;
bool oto = false;
const float BETA = 3950;
enum nilaiKondisi {
  maju, mundur, berhenti
};

nilaiKondisi kondisi = berhenti;

float suhuMax = 79.00;
int jarakMin = 15;

int ukurJarak(int echoPin, int triggerPin){
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(5);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(triggerPin, LOW);


  int jarak, duration;
  duration = pulseIn(echoPin,HIGH, 30000);
  return jarak = duration * 0.0343 / 2;
}

float ukurSuhu(int pin){
  int analogValue = analogRead(pin);
  float celsius = 1 / (log(1 / (4095. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  return celsius;
}

bool cekSuhu(){
  float Suhu1 = ukurSuhu(pinTemp1);
  float Suhu2 = ukurSuhu(pinTemp2);
  float Suhu3 = ukurSuhu(pinTemp3);
  float Suhu4 = ukurSuhu(pinTemp4);

  return (Suhu1 < suhuMax && Suhu2 < suhuMax && Suhu3 < suhuMax && Suhu4 < suhuMax); 
}

bool cekJarakDepan(){
  int jarakDepan = ukurJarak(pinEchoDepan, pinTriggerSonic1);
  // Serial.print("JARAK DEPAN = ");Serial.print(jarakDepan);
  return (jarakDepan > jarakMin);
}

bool cekJarakBelakang(){
  int jarakBelakang = ukurJarak(pinEchoBelakang, pinTriggerSonic2);
  // Serial.print("JARAK BELAKANG = ");Serial.print(jarakBelakang);
  return (jarakBelakang > jarakMin);
}

void moveStepper(int step){
  for (int i=0;i<step;i++){
    digitalWrite(pinStep, HIGH);
    delayMicroseconds(1);
    digitalWrite(pinStep, LOW);
    delayMicroseconds(1);
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(pinEchoDepan, INPUT);
  pinMode(pinEchoBelakang, INPUT);
  pinMode(pinTemp1, INPUT);
  pinMode(pinTemp2, INPUT);
  pinMode(pinTemp3, INPUT);
  pinMode(pinTemp4, INPUT);
  pinMode(pinPencet, INPUT);
  pinMode(pinVertical, INPUT);
  pinMode(pinHorizontal, INPUT);
  pinMode(pinEnableStepper, OUTPUT);
  pinMode(pinDirKiri, OUTPUT);
  pinMode(pinDirKanan, OUTPUT);
  pinMode(pinStep, OUTPUT);
  pinMode(pinTriggerSonic1, OUTPUT);
  pinMode(pinTriggerSonic2, OUTPUT);
  pinMode(pinPiezzo, OUTPUT);
  digitalWrite(pinEnableStepper, LOW);
}

void loop() {
  oto = digitalRead(pinPencet);
  // Serial.print("Otomatis? ");Serial.println(oto);
  if (cekSuhu()){
    if (oto == 0){
      // Serial.println("Jalan");
      Y = analogRead(pinVertical)-2048;
      if (Y > 20 && cekJarakDepan()){
        digitalWrite(pinEnableStepper, LOW);
        digitalWrite(pinDirKiri, LOW);
        digitalWrite(pinDirKanan, HIGH);
        moveStepper(1);
      }
      else if(Y<-20 && cekJarakBelakang()){
        digitalWrite(pinEnableStepper, LOW);
        digitalWrite(pinDirKiri, HIGH);
        digitalWrite(pinDirKanan, LOW);
        moveStepper(1);
      }
      else{
        digitalWrite(pinEnableStepper, HIGH);
      }
    }
    else{
      bool depan = cekJarakDepan();
      delay(100);
      bool belakang = cekJarakBelakang();
      // Serial.println(depan);
      // Serial.println(belakang);
  
      switch (kondisi) {
        case maju:
          if (!depan) {             
            if (belakang) kondisi = mundur;
            else kondisi = berhenti;
          }
          break;

        case mundur:
          if (!belakang) {           
            if (depan) kondisi = maju;
            else kondisi = berhenti;
          }
          break;

        case berhenti:
          if (depan) kondisi = maju;
          else if (belakang) kondisi = mundur;
          break;
      }
      if (kondisi == maju){
        digitalWrite(pinEnableStepper, LOW);
        digitalWrite(pinDirKiri, LOW);
        digitalWrite(pinDirKanan, HIGH);
        moveStepper(1);
      }
      else if(kondisi == mundur){
        digitalWrite(pinEnableStepper, LOW);
        digitalWrite(pinDirKiri, HIGH);
        digitalWrite(pinDirKanan, LOW);
        moveStepper(1);
      }
      else{
        digitalWrite(pinEnableStepper, HIGH);
      }
    }
  }
  else{
    Serial.println("Overheat");
    tone(pinPiezzo, 1000, 100);
    delay(100);
    tone(pinPiezzo, 0, 100);
    delay(100);
  }
}