#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
const int trigPin1 = 4;
const int echoPin1 = 23;
const int trigPin2 = 5;
const int echoPin2 = 18;
const int servoPin = 19;
#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 
#define LED1 2
#define LED2  15
//define sound speed in cm/uS
#define SOUND_SPEED1 0.034
#define SOUND_SPEED2 0.034
#define OLED_RESET     -1 
#define SCREEN_ADDRESS 0x3c
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

long duration1;
long duration2;
float distanceCm1;
float distanceCm2;

int i = 0;
int pos = 0;
Servo servo;
void setup() {
  Serial.begin(115200); 
  pinMode(trigPin1, OUTPUT); 
  pinMode(echoPin1, INPUT); 
   pinMode(trigPin2, OUTPUT); 
  pinMode(echoPin2, INPUT); 
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
   servo.attach(servoPin, 500, 2400);
   /*if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);*/
   /* if(!oled.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
   delay(2000);         // wait two seconds for initializing
  oled.clearDisplay();
  oled.setTextSize(1);         // set text size
  oled.setTextColor(1);    // set text color
  oled.setCursor(10,10);       // set position to display (x,y)
  oled.print("Robotronix"); // set text
 
  oled.display(); 
 /*  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(1);
  display.setCursor(10,10); display.print("Motion Detected!");
  display.display();
  delay(3000);*/
}

void loop() {
  
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
    duration1 = pulseIn(echoPin1, HIGH);
    distanceCm1 = duration1 * SOUND_SPEED1/2;
    duration2 = pulseIn(echoPin2, HIGH);
    distanceCm2 = duration2 * SOUND_SPEED2/2;
   /*for (pos = 0; pos <= 90; pos += 1) {//servo
    servo.write(pos);
    delay(15);
  }
  delay(1000);
  for (pos = 90; pos >= 0; pos -= 1) {
    servo.write(pos);
    delay(15);
  }
  delay(1000); */
  if (distanceCm1  >= 100)
  {
    i = 1;
  }
  
  if(distanceCm1  <= 30 )
  {
    i = 2;
  }
  
  if(i == 2)
  {
    digitalWrite(LED1, HIGH);//LED1 คือปั๊ม1 ทำงาน
    digitalWrite(LED2, LOW);
    /*oled.setCursor(5,35);       
  oled.print("P2 OFF");
  oled.setCursor(5,25);      
  oled.print("P1 ON");
  oled.display();
  delay(100);*/
  }
  if(i == 1)
  {
    digitalWrite(LED1, LOW);
    digitalWrite(LED2, HIGH);//ปั๊มน้ำ2ทำงาน
  /*oled.setCursor(5,35);       
  oled.print("P2 ON");
  oled.setCursor(5,25);       
  oled.print("P1 OFF");
  oled.display();
  delay(100);*/
  }
  
   else 
  {
    digitalWrite(LED1, HIGH);
    digitalWrite(LED2, LOW);
   /* oled.setCursor(5,35);       
  oled.print("P2 OFF");
  oled.setCursor(5,25);       
  oled.print("P1 OFF");
  oled.display();
  delay(100);*/
  }
   delay(100);
 if(!oled.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
    
   delay(100);        
  oled.clearDisplay();
  oled.setTextSize(1);        
  oled.setTextColor(1);   
  oled.setCursor(5,5);       
  oled.print("Distance1 (cm):");//เเสดงคำ ใน OLED
  oled.println(distanceCm1);//เเสดงค่าระยะน้ำ ใน OLED
  oled.setCursor(5,15); 
  oled.print("Distance2 (cm):");//เเสดงคำ ใน OLED
  oled.println(distanceCm2);//เเสดงค่าระยะน้ำ ใน OLED
  oled.display(); 
  Serial.print("Distance1 (cm): ");
  Serial.println(distanceCm1);
  Serial.print("Distance2 (cm): ");
  Serial.println(distanceCm2);
  delay(100);

 
     
 
}