#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <DHT.h>

// Pin Declaration for Servos and Flashlight
const int legFR1 = 2,  legFR2 = 4,  legFR3 = 5;
const int legFL1 = 12, legFL2 = 13, legFL3 = 14;
const int legBR1 = 15, legBR2 = 16, legBR3 = 17;
const int legBL1 = 18, legBL2 = 19, legBL3 = 27;
const int FlashLight = 26;

// DHT22 setup
#define DHTPIN 25
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);

// OLED setup
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Servo objects
Servo servoFR1, servoFR2, servoFR3;
Servo servoFL1, servoFL2, servoFL3;
Servo servoBR1, servoBR2, servoBR3;
Servo servoBL1, servoBL2, servoBL3;

String action = "";

void setup() {
  // Serial communication
  Serial.begin(115200);

  // Servo Attachments
  attachServos();

  // DHT sensor setup
  dht.begin();

  // Flashlight Pin
  pinMode(FlashLight, OUTPUT);

  // OLED Display Initialization
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();

  display.setCursor(30, 10);display.println("Spider Robot");
  display.setCursor(45, 30);display.println("STARTED");
  display.display();

  Serial.println("Spider Robot : STARTED");

  delay(1000);
  display.clearDisplay();
}

void loop() {
  // Get input action from serial
  if (Serial.available() > 0) {
    action = Serial.readStringUntil('\n');
    action.trim();
    Serial.println("Received Action: " + action);

    // Display action on OLED
    displayAction(action);

    // Perform action based on input
    performAction(action);

    // Read and display DHT22 sensor data
    readDHT22();
  }

  delay(100); // Delay for stability
}

// Attach all servos to their respective pins
void attachServos() {
  servoFR1.attach(legFR1); servoFR2.attach(legFR2); servoFR3.attach(legFR3);
  servoFL1.attach(legFL1); servoFL2.attach(legFL2); servoFL3.attach(legFL3);
  servoBR1.attach(legBR1); servoBR2.attach(legBR2); servoBR3.attach(legBR3);
  servoBL1.attach(legBL1); servoBL2.attach(legBL2); servoBL3.attach(legBL3);
}

// Display action on OLED
void displayAction(String act) {
  display.clearDisplay();
  display.setCursor(30, 10);display.println("Spider Robot");

  display.setCursor(20, 20);
  display.print("ACT: ");
  display.println(act);
  display.display();
}

// Perform the corresponding action
void performAction(String act) {
  if (act == "front") {
    moveFront();
  } else if (act == "back") {
    moveBack();
  } else if (act == "left") {
    moveLeft();
  } else if (act == "right") {
    moveRight();
  } else if (act == "left rotate") {
    rotateLeft();
  } else if (act == "right rotate") {
    rotateRight();
  } else if (act == "inc height") {
    increaseHeight();
  } else if (act == "dec height") {
    decreaseHeight();
  } else if (act == "flashon") {
    digitalWrite(FlashLight, HIGH);
  } else if (act == "flashoff") {
    digitalWrite(FlashLight, LOW);
  } else {
    Serial.println("Invalid action");
  }
}



// Move front: moves the entire leg forward by rotating joints appropriately
void moveFront() {
  Serial.println("Moving Front");
  
  // Lift front legs slightly up and move them forward
  servoFR1.write(45); // Rotate joint 1 for forward movement
  servoFR2.write(30); // Lift leg slightly
  servoFR3.write(60); // Move foot forward
  delay(500);

  // Do the same for the other front leg
  servoFL1.write(45);
  servoFL2.write(30);
  servoFL3.write(60);
  delay(500);

  // Return legs to the starting position
  servoFR1.write(90); // Bring leg back to neutral
  servoFR2.write(90);
  servoFR3.write(90);
  
  servoFL1.write(90);
  servoFL2.write(90);
  servoFL3.write(90);
}

// Move back: moves the entire leg backward
void moveBack() {
  Serial.println("Moving Back");
  
  // Lift rear legs slightly up and move them backward
  servoBR1.write(135); // Rotate joint 1 backward
  servoBR2.write(30);  // Lift leg slightly
  servoBR3.write(60);  // Move foot backward
  delay(500);

  // Do the same for the other rear leg
  servoBL1.write(135);
  servoBL2.write(30);
  servoBL3.write(60);
  delay(500);

  // Return legs to the starting position
  servoBR1.write(90); // Bring leg back to neutral
  servoBR2.write(90);
  servoBR3.write(90);

  servoBL1.write(90);
  servoBL2.write(90);
  servoBL3.write(90);
}

// Move left: rotates the body to the left by moving the right legs forward
void moveLeft() {
  Serial.println("Moving Left");
  
  // Move front right leg forward and rear left leg backward to create left movement
  servoFR1.write(45);  // Rotate forward
  servoFR2.write(30);  // Lift leg slightly
  servoFR3.write(60);  // Move foot forward
  delay(500);

  servoBR1.write(135); // Rotate backward
  servoBR2.write(30);  // Lift leg slightly
  servoBR3.write(60);  // Move foot backward
  delay(500);

  // Reset leg positions
  servoFR1.write(90); // Neutral
  servoFR2.write(90);
  servoFR3.write(90);

  servoBR1.write(90); // Neutral
  servoBR2.write(90);
  servoBR3.write(90);
}

// Move right: rotates the body to the right by moving the left legs forward
void moveRight() {
  Serial.println("Moving Right");
  
  // Move front left leg forward and rear right leg backward to create right movement
  servoFL1.write(45);  // Rotate forward
  servoFL2.write(30);  // Lift leg slightly
  servoFL3.write(60);  // Move foot forward
  delay(500);

  servoBL1.write(135); // Rotate backward
  servoBL2.write(30);  // Lift leg slightly
  servoBL3.write(60);  // Move foot backward
  delay(500);

  // Reset leg positions
  servoFL1.write(90); // Neutral
  servoFL2.write(90);
  servoFL3.write(90);

  servoBL1.write(90); // Neutral
  servoBL2.write(90);
  servoBL3.write(90);
}

// Rotate left: rotate entire body left by adjusting all leg positions
void rotateLeft() {
  Serial.println("Rotating Left");
  
  // Move front legs left and rear legs right to rotate left
  servoFR1.write(45); // Rotate front right forward
  servoFL1.write(135); // Rotate front left backward
  delay(500);

  servoBR1.write(135); // Rotate rear right backward
  servoBL1.write(45);  // Rotate rear left forward
  delay(500);

  // Reset all leg positions
  servoFR1.write(90);
  servoFL1.write(90);
  servoBR1.write(90);
  servoBL1.write(90);
}

// Rotate right: rotate entire body right by adjusting all leg positions
void rotateRight() {
  Serial.println("Rotating Right");
  
  // Move front legs right and rear legs left to rotate right
  servoFR1.write(135); // Rotate front right backward
  servoFL1.write(45);  // Rotate front left forward
  delay(500);

  servoBR1.write(45);  // Rotate rear right forward
  servoBL1.write(135); // Rotate rear left backward
  delay(500);

  // Reset all leg positions
  servoFR1.write(90);
  servoFL1.write(90);
  servoBR1.write(90);
  servoBL1.write(90);
}

// Increase height: lift all legs by raising joint 2
void increaseHeight() {
  Serial.println("Increasing Height");

  // Lift all legs by raising joint 2
  servoFR2.write(60); // Lift front right leg
  servoFL2.write(60); // Lift front left leg
  servoBR2.write(60); // Lift rear right leg
  servoBL2.write(60); // Lift rear left leg
  delay(500);

  // Reset height to neutral
  servoFR2.write(90);
  servoFL2.write(90);
  servoBR2.write(90);
  servoBL2.write(90);
}

// Decrease height: lower all legs by lowering joint 2
void decreaseHeight() {
  Serial.println("Decreasing Height");

  // Lower all legs by lowering joint 2
  servoFR2.write(120); // Lower front right leg
  servoFL2.write(120); // Lower front left leg
  servoBR2.write(120); // Lower rear right leg
  servoBL2.write(120); // Lower rear left leg
  delay(500);

  // Reset height to neutral
  servoFR2.write(90);
  servoFL2.write(90);
  servoBR2.write(90);
  servoBL2.write(90);
}








// Read DHT22 temperature and humidity and display on OLED
void readDHT22() {
  float temp = dht.readTemperature();
  float hum = dht.readHumidity();
  
  if (isnan(temp) || isnan(hum)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }

  // Display DHT data on OLED
  display.setCursor(0, 40);
  display.print("Temp: ");
  display.print(temp);
  display.print(" C");

  display.setCursor(0, 50);
  display.print("Hum: ");
  display.print(hum);
  display.println(" %");
  display.display();

  Serial.print("Temp: ");
  Serial.print(temp);
  Serial.print(" C, Hum: ");
  Serial.print(hum);
  Serial.println(" %");
}