#include "DHTesp.h"
#define TRIG_PIN 18 // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 5 // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
#define LEDR 13
#define LEDY 12

float duration_us, distance_cm;
const int DHT_PIN = 15;
DHTesp dhtSensor;

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <math.h> // Include the math library for degrees conversion

const int servoPin = 33;
Servo servo;
Adafruit_MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 16, 2);
TaskHandle_t sensorTask, outputTask;

// Declare gyro angle variables at a broader scope
float gyroXDeg = 0.0;
float gyroYDeg = 0.0;
float gyroZDeg = 0.0;

// Function to convert radians to degrees
float radiansToDegrees(float radians) {
  return radians * 180.0 / PI;
}

void setupSensorTask(void* pvParameters) {
  (void)pvParameters;
  for (;;) {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    // Calculate the rotation angles in degrees
    gyroXDeg = radiansToDegrees(g.gyro.x);
    gyroYDeg = radiansToDegrees(g.gyro.y);
    gyroZDeg = radiansToDegrees(g.gyro.z);

    // Print out the gyro angles in degrees on the serial monitor
    Serial.print("Rotation X: ");
    Serial.print(gyroXDeg);
    Serial.print(" degrees, Y: ");
    Serial.print(gyroYDeg);
    Serial.print(" degrees, Z: ");
    Serial.print(gyroZDeg);
    Serial.println(" degrees");

    vTaskDelay(500 / portTICK_PERIOD_MS);  // Delay for sensor reading
  }
}

void setupOutputTask(void* pvParameters) {
  (void)pvParameters;
  for (;;) {
    // Handle servo and LCD output here
    int servoAngle = servo.read();
    int servoMinAngle = 0; // Minimum servo angle
    int servoMaxAngle = 180; // Maximum servo angle
    int degreesMin = 0;     // Minimum degrees
    int degreesMax = 180;   // Maximum degrees

    float servoAngleDegrees = map(servoAngle, servoMinAngle, servoMaxAngle, degreesMin, degreesMax);

    // Print out the servo angle in degrees
    Serial.print("Servo Angle: ");
    Serial.print(servoAngle);
    Serial.println(" degrees");

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Servo Angle: ");
    lcd.print(servoAngle);

    // Check gyro values and control servo using gyroXDeg, gyroYDeg, and gyroZDeg
    if (abs(gyroXDeg) > 20 || abs(gyroYDeg) > 20 || abs(gyroZDeg) > 20) {
      servo.write(90);
      delay(500);
      servo.write(0);
    }

    vTaskDelay(500 / portTICK_PERIOD_MS);  // Delay for output handling
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(LEDR, OUTPUT);
  pinMode(LEDY, OUTPUT);
  // configure the trigger pin to output mode
  pinMode(TRIG_PIN, OUTPUT);
  // configure the echo pin to input mode
  pinMode(ECHO_PIN, INPUT);
  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);

    // Create and start tasks on different cores
  xTaskCreatePinnedToCore(
      setupSensorTask,
      "SensorTask",
      10000,
      NULL,
      1,
      &sensorTask,
      0
  );

  xTaskCreatePinnedToCore(
      setupOutputTask,
      "OutputTask",
      10000,
      NULL,
      1,
      &outputTask,
      1
  );
  
  servo.attach(servoPin, 500, 2400);
  Serial.begin(115200);
  while (!Serial)
    delay(10);

  Serial.println("Adafruit MPU6050 test!");

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("MPU6050 Test");

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    lcd.setCursor(0, 1);
    lcd.print("MPU6050 Not Found");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 degrees/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 degrees/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 degrees/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 degrees/s");
      break;
  }

  Serial.println("");
}

void loop() {
  // 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;
  Serial.print("distanceA: ");
  Serial.print(distance_cm);
  Serial.println(" cm      ");

  TempAndHumidity  data = dhtSensor.getTempAndHumidity();
  Serial.println("Temp: " + String(data.temperature, 2) + "°C");
  Serial.println("Humidity: " + String(data.humidity, 1) + "%"); 
  Serial.println("---");
  Serial.println(data.temperature);
  delay(2000); // Wait for a new reading from the sensor (DHT22 has ~0.5Hz sample rate)

  if (data.temperature < 10 && data.humidity > 40)
  {
    digitalWrite(LEDR, HIGH);
  }
  else
  {
    digitalWrite(LEDR, LOW);
  }

  if (distance_cm < 200)
  {
    digitalWrite(LEDY, HIGH);
  }
  else
  {
    digitalWrite(LEDY, LOW);
  }
}