// Load Wi-Fi library
#include <WiFi.h>
#include <ESP32Servo.h>
// Replace with your network credentials
const char* ssid = "SSID";
const char* password = "Password";

// Set web server port number to 80
WiFiServer server(80);

// Variable to store the HTTP request
String header;


Servo myServo_1_a;
Servo myServo_1_b;
Servo myServo_2_a;
Servo myServo_2_b;
Servo myServo_3_a;
Servo myServo_3_b;

Servo myServo_4_a;
Servo myServo_4_b;
Servo myServo_5_a;
Servo myServo_5_b;
Servo myServo_6_a;
Servo myServo_6_b;// 

static const int servo_1_a = 32;
static const int servo_1_b = 25;
static const int servo_2_a = 27;
static const int servo_2_b = 12;
static const int servo_3_a = 13;
static const int servo_3_b = 15;

static const int servo_4_a = 2;
static const int servo_4_b = 16;
static const int servo_5_a = 5;
static const int servo_5_b = 19;
static const int servo_6_a = 21;
static const int servo_6_b = 22;

// Auxiliar variables to store the current output state
String output4PinState = "off";
String output3PinState = "off";
bool ok_left = false;
bool ok_right = false;
//String state2;


// Assign output variables to GPIO pins
const int output4 = 4;


// Current time
unsigned long currentTime = millis();
// Previous time
unsigned long previousTime = 0; 
// Define timeout time in milliseconds (example: 2000ms = 2s)
const long timeoutTime = 2000;

const int trigPin = 33;
const int echoPin = 26;

//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

long duration;
float distanceCm;

void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  // Initialize the output variables as outputs
  pinMode(output4, OUTPUT);
  digitalWrite(output4, LOW);

  // Connect to Wi-Fi network with SSID and password
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  // Print local IP address and start web server
  Serial.println("");
  Serial.println("WiFi connected.");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  server.begin();

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  myServo_1_a.setPeriodHertz(50);
  myServo_1_b.setPeriodHertz(50);
  myServo_2_a.setPeriodHertz(50);
  myServo_2_b.setPeriodHertz(50);
  myServo_3_a.setPeriodHertz(50);
  myServo_3_b.setPeriodHertz(50);
  // standard 50 hz servo
  myServo_4_a.setPeriodHertz(50);
  myServo_4_b.setPeriodHertz(50);
  myServo_5_a.setPeriodHertz(50);
  myServo_5_b.setPeriodHertz(50);
  myServo_6_a.setPeriodHertz(50);
  myServo_6_b.setPeriodHertz(50);

//  myServoUltra.setPeriodHertz(50);

  myServo_1_a.attach(servo_1_a , 1000, 2000);
  myServo_1_b.attach(servo_1_b , 1000, 2000);
  myServo_2_a.attach(servo_2_a , 1000, 2000);
  myServo_2_b.attach(servo_2_b , 1000, 2000);
  myServo_3_a.attach(servo_3_a , 1000, 2000);
  myServo_3_b.attach(servo_3_b , 1000, 2000);

  myServo_4_a.attach(servo_4_a , 1000, 2000);
  myServo_4_b.attach(servo_4_b , 1000, 2000);
  myServo_5_a.attach(servo_5_a , 1000, 2000);
  myServo_5_b.attach(servo_5_b , 1000, 2000);
  myServo_6_a.attach(servo_6_a , 1000, 2000);
  myServo_6_b.attach(servo_6_b , 1000, 2000);

  myServo_1_a.write(80);
  myServo_1_b.write(100);
  delay(200);
  myServo_2_a.write(90);
  myServo_2_b.write(90);
  delay(200);
  myServo_3_a.write(90);
  myServo_3_b.write(90);
  delay(200);
  myServo_4_a.write(100);
  myServo_4_b.write(90);
  delay(200);
  myServo_5_a.write(100);
  myServo_5_b.write(90);
  delay(200);
  myServo_6_a.write(90);
  myServo_6_b.write(90);

  delay(3000);
}

void loop(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  
  // Calculate the distance
  distanceCm = duration * SOUND_SPEED/2;
  // Prints the distance in the Serial Monitor
  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);
  
  delay(100);
  if (distanceCm < 50){
    int randomNumber = random(0,2);
    if (randomNumber == 0){
      Serial.println(randomNumber);
      right();
      right();
      web();
      }
    else{
      Serial.println(randomNumber);
      left();
      left();
      web();
      }
    }
  else{
    Serial.println("forward");
    forward();
    }
}

void web(){
  WiFiClient client = server.available();   // Listen for incoming clients

  if (client) {                             // If a new client connects,
    currentTime = millis();
    previousTime = currentTime;
    Serial.println("New Client.");          // print a message out in the serial port
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected() && currentTime - previousTime <= timeoutTime) {  // loop while the client's connected
      currentTime = millis();
      if (client.available()) {             // if there's bytes to read from the client,
        char c = client.read();             // read a byte, then
        Serial.write(c);                    // print it out the serial monitor
        header += c;
        if (c == '\n') {                    // if the byte is a newline character
          // if the current line is blank, you got two newline characters in a row.
          // that's the end of the client HTTP request, so send a response:
          if (currentLine.length() == 0) {
            // HTTP headers always start with a response code (e.g. HTTP/1.1 200 ok_left)
            // and a content-type so the client knows what's coming, then a blank line:
            client.println("HTTP/1.1 200 ok_left");
            client.println("Content-type:text/html");
            client.println("Connection: close");
            client.println();
            
            // turns the GPIOs on and off
            if (header.indexOf("GET /left/on") >= 0) {
              Serial.println("GPIO 4 on");
              output4PinState = "on";
              ok_left = true;
              
              
              //digitalWrite(output4, HIGH);
            } else if (header.indexOf("GET /left/off") >= 0) {
              Serial.println("GPIO 4 off");
              output4PinState = "off";
              //digitalWrite(output4, LOW);
              ok_left = false;
            } 

            if (header.indexOf("GET /right/on") >= 0) {
              Serial.println("Right on");
              output3PinState = "on";
              ok_right = true;
              right();
              //digitalWrite(output4, HIGH);
            } else if (header.indexOf("GET /right/off") >= 0) {
              Serial.println("Right off");
              output3PinState = "off";
              ok_right = false;
              forward();
              //digitalWrite(output4, LOW);
            } 
            
            // Display the HTML web page
            client.println("<!DOCTYPE html><html>");
            client.println("<head><meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">");
            client.println("<link rel=\"icon\" href=\"data:,\">");
            // CSS to style the on/off buttons 
            // Feel free to change the background-color and font-size attributes to fit your preferences
            client.println("<style>html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}");
            client.println(".button { background-color: #4CAF50; border: none; color: white; padding: 16px 40px;");
            client.println("text-decoration: none; font-size: 30px; margin: 2px; cursor: pointer;}");
            client.println(".button2 {background-color: #555555;}</style></head>");
            
            // Web Page Heading
            
            
            // Display current state, and ON/OFF buttons for GPIO 4  
            client.println("<body><h1>Spider Network</h1>");
            
            // Display current state, and ON/OFF buttons for GPIO 4  
            client.println("<p>Left Control -" + output4PinState + "</p>");
            // If the output4State is off, it displays the ON button       
            if (output4PinState=="off") {
              client.println("<p><a href=\"/left/on\"><button class=\"button\">ON</button></a></p>");
            } else {
              client.println("<p><a href=\"/left/off\"><button class=\"button button2\">OFF</button></a></p>");
            }

            client.println("<p>Right Control -" + output3PinState + "</p>");
            // If the output4State is off, it displays the ON button       
            if (output3PinState=="off") {
              client.println("<p><a href=\"/right/on\"><button class=\"button\">ON</button></a></p>");
            } else {
              client.println("<p><a href=\"/right/off\"><button class=\"button button2\">OFF</button></a></p>");
            }
            client.println("</body></html>");
            
            // The HTTP response ends with another blank line
            client.println();
            // Break out of the while loop
            break;
          } else { // if you got a newline, then clear currentLine
            currentLine = "";
          }
        } else if (c != '\r') {  // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }
      }
    }
    // Clear the header variable
    header = "";
    // Close the connection
    client.stop();
    Serial.println("Client disconnected.");
    Serial.println("");
  }
  //output4PinState = "off";
  while(ok_left){
     Serial.println("GPIO 4 on");
     right();
     web();
  }
   while(ok_right){
     Serial.println("GPIO 3 on");
     left();
     web();  
     }
  }


void forward(){
   Serial.println("forward here"); // lifts the second leg
   myServo_5_b.write(40);
   myServo_1_b.write(60);
   myServo_3_b.write(40);
   delay(70);
   //delay(800);
   //Serial.println("7"); // lifts the second leg
   myServo_5_a.write(160);
   myServo_1_a.write(20);
   myServo_3_a.write(0);
   delay(100);
   //delay(800);
   //Serial.println("1");// other set to provide support
   myServo_5_b.write(90);//40
   myServo_1_b.write(100);
   myServo_3_b.write(100);
   delay(150);
   //delay(800);
   //Serial.println("4"); // sets the first stepbon ground
   myServo_6_b.write(50);
   myServo_4_b.write(40);
   myServo_2_b.write(40);
   delay(70);
   //delay(800);
   //Serial.println("7"); // lifts the second leg
   myServo_5_a.write(40);
   myServo_1_a.write(140);
   myServo_3_a.write(150);
   delay(150);
   //delay(800);
    //Serial.println("6"); // takes the first step
   myServo_6_a.write(160);
   myServo_4_a.write(150);
   myServo_2_a.write(30);
   delay(150);
   //delay(800);
    //Serial.println("4"); // sets the first stepbon ground
   myServo_6_b.write(90);
   myServo_4_b.write(90);
   myServo_2_b.write(90);
   delay(70);
   //delay(800);
   //Serial.println("1");// other set to provide support
   myServo_5_b.write(40);//40
   myServo_1_b.write(60);
   myServo_3_b.write(40);
   delay(150);
   //delay(800);
   //Serial.println("6"); // takes the first step
   myServo_6_a.write(40);
   myServo_4_a.write(40);
   myServo_2_a.write(150);
   delay(150);
   //delay(800);
   //Serial.println("2"); // lift the leg for first step.
   myServo_6_b.write(90);
   myServo_4_b.write(90);
   myServo_2_b.write(90);
   delay(100);
   //delay(800);


  }

void right(){
  myServo_6_b.write(50);
 myServo_4_b.write(50);
 myServo_2_b.write(50);
 delay(200);
 //delay(800);;
 myServo_6_a.write(30);
 myServo_4_a.write(30);
 myServo_2_a.write(30);
 delay(200);
 //delay(800);;
 myServo_6_b.write(90);
 myServo_4_b.write(90);
 myServo_2_b.write(90);
 delay(200);
 //delay(800);;
 
 myServo_1_b.write(50);
 myServo_3_b.write(50);
 myServo_5_b.write(50);
 delay(200);
 //delay(800);;

 myServo_6_a.write(150);
 myServo_4_a.write(150);
 myServo_2_a.write(150);
 delay(200);
 //delay(800);;
 
 
 myServo_1_a.write(30);
 myServo_3_a.write(30);
 myServo_5_a.write(30);
 delay(200);
 
 //delay(800);;
 myServo_1_b.write(90);
 myServo_3_b.write(90);
 myServo_5_b.write(90);
 delay(200);
 //delay(800);;
 myServo_6_b.write(50);
 myServo_4_b.write(50);
 myServo_2_b.write(50);
 delay(200);
 //delay(800);;
 
 myServo_1_a.write(150);
 myServo_3_a.write(150);
 myServo_5_a.write(150);
 delay(200);
 //delay(800);;
 

  }

void left(){
 myServo_6_b.write(40);
 myServo_4_b.write(40);
 myServo_2_b.write(50);
 delay(150);
 //delay(800);
 myServo_6_a.write(150);
 myServo_4_a.write(150);
 myServo_2_a.write(150);
 delay(150);
 //delay(800);
 myServo_6_b.write(90);
 myServo_4_b.write(90);
 myServo_2_b.write(90);
 delay(150);
 //delay(800);
 myServo_1_b.write(40);
 myServo_3_b.write(40);
 myServo_5_b.write(40);
 delay(150);
 //delay(800);
 myServo_6_a.write(30);
 myServo_4_a.write(30);
 myServo_2_a.write(30);
 delay(150);
 
 //delay(800);
 myServo_1_a.write(150);
 myServo_3_a.write(150);
 myServo_5_a.write(150);
 delay(150);
 //delay(800);
 myServo_1_b.write(90);
 myServo_3_b.write(90);
 myServo_5_b.write(90);
 delay(150);
 //delay(800);
 myServo_6_b.write(40);
 myServo_4_b.write(40);
 myServo_2_b.write(40);
 delay(150);
 //delay(800);
 myServo_1_a.write(30);
 myServo_3_a.write(30);
 myServo_5_a.write(30);
 delay(150);
 //delay(800);
  }