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

// Pin Declaration for LEDs (representing motors)
const int frontRightMotorPin = 19;
const int frontLeftMotorPin = 18;

const int backRightMotorPin = 34;
const int backLeftMotorPin = 35;

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);

String action = "";

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

  // Set motor pins and flashlight pin as output
  pinMode(frontRightMotorPin, OUTPUT);
  pinMode(frontLeftMotorPin, OUTPUT);
  pinMode(backRightMotorPin, OUTPUT);
  pinMode(backLeftMotorPin, OUTPUT);
  pinMode(FlashLight, OUTPUT);

  // DHT sensor setup
  dht.begin();

  // 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("Car 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
}

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

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

// Perform the corresponding action with LEDs acting as motors
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 == "flashon") {
    digitalWrite(FlashLight, HIGH);
  } else if (act == "flashoff") {
    digitalWrite(FlashLight, LOW);
  } else {
    Serial.println("Invalid action");
  }
}

// Move front: Turn on both front motors (LEDs)
void moveFront() {
  Serial.println("Moving Front");

  digitalWrite(frontRightMotorPin, HIGH);
  digitalWrite(frontLeftMotorPin, HIGH);
  delay(1000);

  digitalWrite(frontRightMotorPin, LOW);
  digitalWrite(frontLeftMotorPin, LOW);
}

// Move back: Turn on both back motors (LEDs)
void moveBack() {
  Serial.println("Moving Back");

  digitalWrite(backRightMotorPin, HIGH);
  digitalWrite(backLeftMotorPin, HIGH);
  delay(1000);

  digitalWrite(backRightMotorPin, LOW);
  digitalWrite(backLeftMotorPin, LOW);
}

// Move left: Turn on right motors (LEDs)
void moveLeft() {
  Serial.println("Moving Left");

  digitalWrite(frontRightMotorPin, HIGH);
  digitalWrite(backRightMotorPin, HIGH);
  delay(1000);

  digitalWrite(frontRightMotorPin, LOW);
  digitalWrite(backRightMotorPin, LOW);
}

// Move right: Turn on left motors (LEDs)
void moveRight() {
  Serial.println("Moving Right");

  digitalWrite(frontLeftMotorPin, HIGH);
  digitalWrite(backLeftMotorPin, HIGH);
  delay(1000);

  digitalWrite(frontLeftMotorPin, LOW);
  digitalWrite(backLeftMotorPin, LOW);
}

// Rotate left: Turn on both right motors for rotation
void rotateLeft() {
  Serial.println("Rotating Left");

  digitalWrite(frontRightMotorPin, HIGH);
  digitalWrite(backRightMotorPin, HIGH);
  delay(1000);

  digitalWrite(frontRightMotorPin, LOW);
  digitalWrite(backRightMotorPin, LOW);
}

// Rotate right: Turn on both left motors for rotation
void rotateRight() {
  Serial.println("Rotating Right");

  digitalWrite(frontLeftMotorPin, HIGH);
  digitalWrite(backLeftMotorPin, HIGH);
  delay(1000);

  digitalWrite(frontLeftMotorPin, LOW);
  digitalWrite(backLeftMotorPin, LOW);
}

// 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(24, 40);
  display.print("Temp: ");
  display.print(temp);
  display.print(" C");

  display.setCursor(24, 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(" %");
}