#include <ESP32Servo.h>

#define TRIG_PIN 32 // ESP32 pin GPIO 23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 35 // ESP32 pin GPIO 22 connected to Ultrasonic Sensor's ECHO pin
#define SERVO_PIN 13 // ESP32 pin GPIO 26 connected to Servo Motor's pin
#define DISTANCE_THRESHOLD 60 // centimeters
// variables will change:
float duration_us, distance_cm;

const int servoPin = 26;
const int servoPin2 = 25;

#define SW1 2 
#define SW2 15
#define SW3 0
#define SW4 4   

Servo servo;
Servo servo3;


int pos = 0;
int pos_Pan=0;
int pos_Tilt=0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Chaiyan Chanapromma");
  servo.attach(servoPin, 500, 2400);
  servo3.attach(servoPin2, 500, 2400);
  servo.write(pos);
  servo3.write(pos);
  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);
  pinMode(SW3, INPUT_PULLUP);
  pinMode(SW4, INPUT_PULLUP);
  
  pinMode(TRIG_PIN, OUTPUT); // set ESP32 pin to output mode
  pinMode(ECHO_PIN, INPUT);  // set ESP32 pin to input mode
}

void loop() {
 // ultrasonic_control();
  delay(10);
  if (digitalRead(SW1) == LOW){
    //servo_control();
    servo1_select();
    delay(20);
  }

  if (digitalRead(SW2) == LOW){
    servo1_unselect();
    delay(20);
  }

  if (digitalRead(SW3) == LOW){
    servo_left_up();

  }

  if(digitalRead(SW4) == LOW){
    servo_left_down();
  }

  //
  //delay(10); // this speeds up the simulation
  
}

void servo_control(){
  // put your main code here, to run repeatedly:
    for (pos = 0; pos <= 180; pos += 10) {
    servo.write(pos);
    delay(100);
  }
  for (pos = 180; pos >= 0; pos -= 10) {
    servo.write(pos);
    delay(100);
  }
}

void servo1_select(){
  if (pos<180){
    pos += 5;
    servo.write(pos);
    //Serial.println(pos);
    delay(100);
  }
}

void servo1_unselect(){
  if (pos>0){
    pos -= 5;
    servo.write(pos);
    //Serial.println(pos);
    delay(100);
  }
}

void servo_left_up(){
  servo3.write(70);
  Serial.println(70);
  delay(15);
}

void servo_left_down(){
  servo3.write(0);
  Serial.println(0);
  delay(15);

}

void ultrasonic_control(){

   // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

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

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

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

  delay(15);
}