#include <Arduino.h>
#include <DHT.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <PID_v1.h>
#include <Arduino_FreeRTOS.h>

// Pin Definitions
#define DHTPIN 2
#define DHTTYPE DHT22
#define PIRPIN 3
#define LED_LIGHT_PIN 4
#define LED_ALARM_PIN 5
#define LDR_PIN A0
#define SERVO_PIN 10

// Create DHT instance
DHT dht(DHTPIN, DHTTYPE);

// Create LCD object
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Create Servo object
Servo servoMotor;

// PID variables
double Setpoint, Input, Output;
double Kp = 2.0, Ki = 5.0, Kd = 1.0;

// PID object
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

// Task Handles
TaskHandle_t TaskDHT = NULL;
TaskHandle_t TaskLDR = NULL;
TaskHandle_t TaskPIR = NULL;
TaskHandle_t TaskServoPID = NULL;
TaskHandle_t TaskLCD = NULL;

void setup() {
  // Initialize Serial for debugging
  Serial.begin(9600);

  // Initialize DHT sensor
  dht.begin();

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

  // Set pin modes for LED and PIR
  pinMode(LED_LIGHT_PIN, OUTPUT);
  pinMode(LED_ALARM_PIN, OUTPUT);
  pinMode(PIRPIN, INPUT);
  pinMode(LDR_PIN, INPUT);

  // Attach Servo
  servoMotor.attach(SERVO_PIN);

  // Initialize PID control
  Setpoint = 50; // Example target value for servo position
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, 180); // Servo angle limits

  // Create RTOS Tasks
  xTaskCreate(taskDHT, "DHT Sensor Task", 1000, NULL, 1, &TaskDHT);
  xTaskCreate(taskLDR, "LDR Sensor Task", 1000, NULL, 1, &TaskLDR);
  xTaskCreate(taskPIR, "PIR Sensor Task", 1000, NULL, 1, &TaskPIR);
  xTaskCreate(taskServoPID, "Servo PID Task", 1000, NULL, 1, &TaskServoPID);
  xTaskCreate(taskLCD, "LCD Task", 1000, NULL, 1, &TaskLCD);

  // Start the FreeRTOS Scheduler
  vTaskStartScheduler();
}

void loop() {
  // Empty. RTOS tasks are handling the functionality.
}

// Task to monitor temperature and humidity
void taskDHT(void *pvParameters) {
  while (1) {
    float temp = dht.readTemperature();
    float humidity = dht.readHumidity();

    if (!isnan(temp) && !isnan(humidity)) {
      Serial.print("Temp: ");
      Serial.print(temp);
      Serial.print(" Humidity: ");
      Serial.println(humidity);
    } else {
      Serial.println("Failed to read from DHT sensor!");
    }

    vTaskDelay(pdMS_TO_TICKS(2000)); // Run every 2 seconds
  }
}

// Task to control lights based on LDR sensor
void taskLDR(void *pvParameters) {
  while (1) {
    int lightLevel = analogRead(LDR_PIN);
    
    if (lightLevel < 500) {
      digitalWrite(LED_LIGHT_PIN, HIGH);  // Turn on the light if it's dark
    } else {
      digitalWrite(LED_LIGHT_PIN, LOW);   // Turn off the light otherwise
    }

    vTaskDelay(pdMS_TO_TICKS(1000)); // Run every 1 second
  }
}

// Task to detect motion using PIR sensor
void taskPIR(void *pvParameters) {
  while (1) {
    int pirState = digitalRead(PIRPIN);
    
    if (pirState == HIGH) {
      digitalWrite(LED_ALARM_PIN, HIGH);  // Turn on alarm LED
      Serial.println("Motion Detected!");
    } else {
      digitalWrite(LED_ALARM_PIN, LOW);   // Turn off alarm LED
    }

    vTaskDelay(pdMS_TO_TICKS(500)); // Run every 500 milliseconds
  }
}

// Task for PID control of the servo motor
void taskServoPID(void *pvParameters) {
  while (1) {
    int pirState = digitalRead(PIRPIN);

    // Use the PIR motion sensor as input for PID control
    Input = pirState * 180; // Map PIR state to 0 or 180 for simplicity
    myPID.Compute(); // Compute the PID output

    // Output to Servo
    servoMotor.write(Output);

    Serial.print("Servo Position: ");
    Serial.println(Output);

    vTaskDelay(pdMS_TO_TICKS(100)); // Run every 100 milliseconds
  }
}

// Task to update the LCD display
void taskLCD(void *pvParameters) {
  while (1) {
    float temp = dht.readTemperature();
    float humidity = dht.readHumidity();

    if (!isnan(temp) && !isnan(humidity)) {
      lcd.setCursor(0, 0);
      lcd.print("Temp: ");
      lcd.print(temp);
      lcd.print(" C");

      lcd.setCursor(0, 1);
      lcd.print("Hum: ");
      lcd.print(humidity);
      lcd.print(" %");
    } else {
      lcd.setCursor(0, 0);
      lcd.print("Sensor Error   ");
      lcd.setCursor(0, 1);
      lcd.print("Check Sensors  ");
    }

    vTaskDelay(pdMS_TO_TICKS(2000)); // Update every 2 seconds
  }
}