#include <ESP32Servo.h>          // Library for servomotor on ESP32
#include <NewPing.h>             // Library for ultrasonic sensor
#include <LiquidCrystal_I2C.h>   // Library for LCD
#include <DHT.h>                 // Library for DHT sensor
#include <Wire.h>
#include <MPU6050.h>             // Library for MPU6050 sensor
#include <WiFi.h>                // ESP32 WiFi library
#include <WebServer.h>           // Library for Web server
#include <HTTPClient.h>          // Library for HTTP client

#define DHTPIN 4                 // Pin where the DHT22 is connected
#define DHTTYPE DHT22            // DHT22 sensor type

const char* ssid = "Wokwi-GUEST";     // Wokwi WiFi network SSID
const char* password = "";            // Wokwi WiFi network password

DHT dht(DHTPIN, DHTTYPE);        // Initialize DHT sensor
MPU6050 mpu;                     // Initialize MPU6050 sensor

NewPing sonar(12, 13, 200);        // Setup NewPing. Pin 12 = trigger out, pin 13 = echo in, max. distance 200cm
Servo servo;                     // Define object servo
LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, 16 column and 2 rows
WebServer server(80);            // Create a web server object that listens for HTTP request on port 80

const int distMax = 30;          // Maximum distance
const int distMin = 10;          // Minimum distance
const int angleMax = 180;        // Maximum servo angle
const int angleMin = 0;          // Minimum servo angle

const int k = ((angleMax - angleMin) / (distMax - distMin)); // K factor for scaling
const int b = (angleMin - k * distMin);                      // Angle when the distance is at its minimum

const int midpointDistance = (distMax + distMin) / 2;  // Define midpoint distance
const int midpointAngle = k * midpointDistance + b;    // Calculate midpoint angle

String googleScriptID = "https://script.google.com/macros/s/AKfycbxPuI4S8urd28kVV-TKO7cEEDkxSv27d4C3LfHuTm0P8jS4PgBXSSsadu0TB6NKOibQ/exec";

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

  // Connect to WiFi network
  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();
  Serial.print("WiFi connected, IP address: ");
  Serial.println(WiFi.localIP());

  servo.attach(14);              // Connect servo to pin 14

  lcd.init();                   // Initialize LCD
  lcd.backlight();              // Backlight on

  dht.begin();                  // Initialize DHT sensor
  Wire.begin(21, 22);           // Initialize I2C with default SDA and SCL pins on ESP32
  mpu.initialize();             // Initialize MPU6050 sensor

  // Define web server routes
  server.on("/", handleRoot);               // Route for the root / web page
  server.on("/servo", handleServoControl);  // Route for controlling servo
  server.on("/data", handleSensorData);     // Route for sensor data
  server.begin();                           // Start the server
  Serial.println("HTTP server started");
}

void loop() {
  server.handleClient();  // Handle client requests

  // Update sensor data and servo control in the background
  int dist = sonar.ping_cm();       // Measure distance in cm to variable dist

  int angle; // Define angle variable

  // Check if distance is within limits
  if ((dist >= distMin) && (dist <= distMax)) {
    // If within limits, servo angle is proportional to distance
    angle = k * dist + b;
  } else {
    // If outside limits, set servo angle to extremes
    if (dist > distMax) {
      angle = angleMax; // Set servo angle to maximum
    } else {
      angle = angleMin; // Set servo angle to minimum
    }
  }

  // Set servo angle to the midpoint when distance matches midpoint distance
  if (dist == midpointDistance) {
    servo.write(midpointAngle);
  } else {
    servo.write(angle); // Set servo angle
  }

  // Read temperature and humidity from DHT22
  float temperature = dht.readTemperature();
  float humidity = dht.readHumidity();

  // Read acceleration and gyroscope data from MPU6050
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accelX = ax / 16384.0;
  float accelY = ay / 16384.0;
  float accelZ = az / 16384.0;
  float gyroX = gx / 131.0;
  float gyroY = gy / 131.0;
  float gyroZ = gz / 131.0;

  // Display data on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Distance: " + String(dist) + "cm");
  lcd.setCursor(0, 1);
  if ((dist < distMin) || (dist > distMax)) {
    lcd.print("Outside limits");
  } else {
    lcd.print("Within limits");
  }
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Angle: " + String(angle) + "deg");
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Temp: " + String(temperature) + "C");
  lcd.setCursor(0, 1);
  lcd.print("Humidity: " + String(humidity) + "%");
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("AccelX: " + String(accelX));
  lcd.setCursor(0, 1);
  lcd.print("AccelY: " + String(accelY));
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("AccelZ: " + String(accelZ));
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("GyroX: " + String(gyroX));
  lcd.setCursor(0, 1);
  lcd.print("GyroY: " + String(gyroY));
  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("GyroZ: " + String(gyroZ));
  delay(1000);

  // Send data to Google Sheets
  sendDataToGoogleSheets(temperature, humidity, accelX, accelY, accelZ, gyroX, gyroY, gyroZ);
}

void handleRoot() {
  String html = "<html>\
    <head>\
      <title>ESP32 Web Server</title>\
      <style>\
        body { font-family: Arial; text-align: center; }\
        .button { padding: 10px 20px; margin: 10px; }\
      </style>\
    </head>\
    <body>\
      <h1>ESP32 Web Server</h1>\
      <button class=\"button\" onclick=\"sendServoCommand(0)\">0°</button>\
      <button class=\"button\" onclick=\"sendServoCommand(90)\">90°</button>\
      <button class=\"button\" onclick=\"sendServoCommand(180)\">180°</button>\
      <br>\
      <button class=\"button\" onclick=\"getSensorData()\">Get Sensor Data</button>\
      <div id=\"sensorData\"></div>\
      <script>\
        function sendServoCommand(angle) {\
          fetch('/servo?angle=' + angle);\
        }\
        function getSensorData() {\
          fetch('/data').then(response => response.json()).then(data => {\
            document.getElementById('sensorData').innerHTML = JSON.stringify(data);\
          });\
        }\
      </script>\
    </body>\
  </html>";
  server.send(200, "text/html", html);
}

void handleServoControl() {
  if (server.hasArg("angle")) {
    int angle = server.arg("angle").toInt();
    servo.write(angle);
    server.send(200, "text/plain", "Servo angle set to " + String(angle));
  } else {
    server.send(400, "text/plain", "Angle parameter missing");
  }
}

void handleSensorData() {
  float temperature = dht.readTemperature();
  float humidity = dht.readHumidity();

  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accelX = ax / 16384.0;
  float accelY = ay / 16384.0;
  float accelZ = az / 16384.0;
  float gyroX = gx / 131.0;
  float gyroY = gy / 131.0;
  float gyroZ = gz / 131.0;

  String data = "{";
  data += "\"temperature\":" + String(temperature) + ",";
  data += "\"humidity\":" + String(humidity) + ",";
  data += "\"accelX\":" + String(accelX) + ",";
  data += "\"accelY\":" + String(accelY) + ",";
  data += "\"accelZ\":" + String(accelZ) + ",";
  data += "\"gyroX\":" + String(gyroX) + ",";
  data += "\"gyroY\":" + String(gyroY) + ",";
  data += "\"gyroZ\":" + String(gyroZ);
  data += "}";

  server.send(200, "application/json", data);
}

void sendDataToGoogleSheets(float temperature, float humidity, float accelX, float accelY, float accelZ, float gyroX, float gyroY, float gyroZ) {
  if (WiFi.status() == WL_CONNECTED) {
    HTTPClient http;
    String url = googleScriptID + "?temperature=" + String(temperature) + "&humidity=" + String(humidity) + "&accelX=" + String(accelX) + "&accelY=" + String(accelY) + "&accelZ=" + String(accelZ) + "&gyroX=" + String(gyroX) + "&gyroY=" + String(gyroY) + "&gyroZ=" + String(gyroZ);
    http.begin(url);
    http.setTimeout(10000);  // Set timeout to 10 seconds
    int httpCode = http.GET();
    if (httpCode > 0) {
      Serial.println("HTTP Request Sent");
      String payload = http.getString();
      Serial.println("Response: " + payload);
    } else {
      Serial.print("Error on HTTP request: ");
      Serial.println(httpCode); // Print HTTP error code
    }
    http.end();
  } else {
    Serial.println("WiFi not connected");
  }
}