#include <ESP32Servo.h>
#include <Stepper.h>
//#include <AccelStepper.h> 
#include <Arduino.h>
#include <WiFi.h>
#include <HTTPClient.h>

//#include <ESP32Servo.h>

//int pushButton_1 = 34;     // Pin Button
//int pushButton_2 = 22;     // Pin Button
//int motor_2 = 13;        // Pin motor
//int motor_1 = 26;
//int buttonState1 = 0; // Variabel untuk menyimpan status tombol 1
//int buttonState2 = 0; // Variabel untuk menyimpan status tombol 2

#define TRIGPIN_1  33  
#define ECHOPIN_1  25
#define TRIGPIN_2  2  
#define ECHOPIN_2  4 
#define TRIGPIN_3 15  // Pin ECHO sensor HC-SR04
#define ECHOPIN_3 5  // Pin TRIG sensor HC-SR04
#define SERVO1_PIN 26  // ESP32 pin GIOP26 connected to Servo Motor's pin
#define SERVO2_PIN 13  // ESP32 pin GIOP13 connected to Servo Motor's pin
#define DISTANCE_THRESHOLD1  20 // Jarak meters
#define DISTANCE_THRESHOLD2  20 // Jarak kapal mejauh di bidang 3 dalam meters
const int stepsPerRevolution = 20;  // Ubah sesuai jumlah langkah per revolusi motor Anda

// Inisialisasi library stepper pada pin 14 hingga 27:
Stepper myStepper(stepsPerRevolution, 14, 27 );
Servo servo; // create servo object to control a servo
//#define kecepatanSuara 0.034  //#define digunakan untuk membuat sebuah konstanta

byte led1 = 18;
byte led2 = 19;
byte led3 = 21;
const byte dirPin = 12;  // varaible speed stepper motor
const byte stepPin = 13;
const byte dirPinB = 14;  // varaible speed stepper motor
const byte stepPinB = 27;

float jarakcm;
float duration_us, distance_cm;
//AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
String serverName = "https://api.thingspeak.com/update?api_key=H8ZYPW4D832LK75T&field1=";

void setup() 
{
  Serial.begin(115200);  // Starts the serial communication
  // Setel kecepatan motor stepper menjadi 1000 rpm:
  myStepper.setSpeed(1000);
  //stepper.setMaxSpeed(2000);
  //stepper.setSpeed(200);

  // Konfigurasi HCSR 1
  pinMode(TRIGPIN_1, OUTPUT);
  pinMode(ECHOPIN_1, INPUT);

  // Konfigurasi HCSR 2
  Serial.begin (9600);       // initialize serial port
  pinMode(TRIGPIN_2, OUTPUT); // set ESP32 pin to output mode
  pinMode(ECHOPIN_2, INPUT);  // set ESP32 pin to input mode
  servo.attach(SERVO2_PIN);   // attaches the servo on pin 9 to the servo object
  servo.write(0);

  //Sensor HCSR menampilkan led bar delay agak lama
  pinMode(led1, OUTPUT); //LED BAR
  pinMode(led2, OUTPUT); //LED BAR
  pinMode(led3, OUTPUT); //LED BAR

 // pinMode(motor_1, OUTPUT);  
  //pinMode(pushButton_1, INPUT_PULLUP);
  //pinMode(motor_2, OUTPUT);  
  //pinMode(pushButton_2, INPUT_PULLUP);

}

void loop() 
{
//BAGIAN 1
// Pengukuran jarak menggunakan sensor ultrasonik
  digitalWrite(TRIGPIN_1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGPIN_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGPIN_1, LOW);
  long duration = pulseIn(ECHOPIN_1, HIGH);
  int distance = duration * 0.034 / 2;  // Menghitung jarak dalam sentimeter

  // Tampilkan jarak yang diukur di Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Gerakkan motor stepper berdasarkan jarak yang diukur
  if (distance < 100) 
  {
    // Jika jarak kurang dari 100 cm, putar motor ke arah tertentu (sesuai kebutuhan)
    Serial.println("Menggerakkan motor stepper");
    myStepper.step(1000);  // Ubah jumlah langkah sesuai kebutuhan Anda
    delay(100); // Tambahkan delay sesuai kebutuhan
  } 
  
  else 
  {
    // Jika jarak lebih besar dari atau sama dengan 10 cm, tidak ada gerakan motor
    Serial.println("Tidak ada gerakan motor stepper");
    delay(100); // Tambahkan delay sesuai kebutuhan
  }

// LED BAR
// ambil jarak dari sensor
  unsigned long durasi = 0;
  //memerintahkan sensor ultrasonik
  //digitalWrite(TRIGPIN_1, LOW);
  //delayMicroseconds(2);
  //digitalWrite(TRIGPIN_1, HIGH);
  //delayMicroseconds(10);
  //digitalWrite(TRIGPIN_1, LOW);

  //mencari durasi waktu
  //durasi = pulseIn(ECHOPIN,HIGH);

  //menghitung jarak
  //jarakcm = durasi * kecepatanSuara / 2;

  Serial.print("Jarak = ");
  Serial.println(jarakcm);

  //matikan semua LED
  digitalWrite(led1, LOW);
  digitalWrite(led2, LOW);
  digitalWrite(led3, LOW);
  
  //jika jarak kurang dari sama dengan 10
  if(jarakcm<=100){
    digitalWrite(led1, HIGH);
  }
  if(jarakcm>100 && jarakcm<=250){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, HIGH);
  }
  if(jarakcm>250 && jarakcm<=400){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, HIGH);
  }

  delay(300); // this speeds up the simulation


  /////////////////////////////////////////////////////////
  //HCSR2 pada bidang 3 Kapal menjauh
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIGPIN_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGPIN_2, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHOPIN_2, HIGH);
  // calculate the distance
  distance_cm = 0.017 * duration_us;

  if (distance_cm < DISTANCE_THRESHOLD2)
    servo.write(0); // rotate servo motor to 90 degree
  else
    servo.write(90);  // rotate servo motor to 0 degree

  // print the value to Serial Monitor
  Serial.print("distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  delay(500);


  /////////////////////////////////////////////////////////
  //stepper.runSpeed();
  // buttonState1 = digitalRead(pushButton_1);

   // if (buttonState1 == LOW) 
    {
    //digitalWrite(motor_1, HIGH);
    } 

   // else 
    {
    //digitalWrite(motor_1, LOW);
    }

    // Jika tombol 1 tidak ditekan, baca status tombol 2
   //buttonState2 = digitalRead(pushButton_2);

   //if (buttonState2 == LOW) 
    {
//      digitalWrite(motor_2, HIGH);
    } 
    
   // else 
    {
  //    digitalWrite(motor_2, LOW);
    }
}