#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
const int trigPin = 5;
const int echoPin = 18;
const int servoPin = 19;
#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 
#define LED1 23
#define LED2  4
//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701
#define OLED_RESET     -1 
#define SCREEN_ADDRESS 0x3c
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

long duration;
float distanceCm;
float distanceInch;

int i = 0;
int pos = 0;
Servo servo;
void setup() {
  Serial.begin(115200); 
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, 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(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distanceCm = duration * SOUND_SPEED/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 (distanceCm  >= 100)
  {
    i = 1;
  }
  
  if(distanceCm  <= 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("Distance (cm):");//เเสดงคำ ใน OLED
  oled.println(distanceCm);//เเสดงค่าระยะน้ำ ใน OLED
  oled.display(); 
  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);

  delay(100);

  for (pos = 0; pos <= 90; pos += 1) {
    servo.write(pos);
    delay(15);
  }
  delay(1000);
  for (pos = 90; pos >= 0; pos -= 1) {
    servo.write(pos);
    delay(15);
  }
  delay(1000);
}