#include <DFRobot_DHT11.h>

// Initialize DHT11 sensors
DFRobot_DHT11 sensor1;
DFRobot_DHT11 sensor2;

// Pin definitions for DHT11 sensors
#define DHT11_PIN1 7
#define DHT11_PIN2 13

// Desired temperature in degrees Celsius
const int desiredTemperature = 22;

// Motor control pin definitions
const int motor1SpeedPin = 5;
const int motor1DirPin1 = 4;
const int motor1DirPin2 = 3;

const int motor2SpeedPin = 11;
const int motor2DirPin1 = 10;
const int motor2DirPin2 = 9;

// Motor speed variables
int motor1Speed = 0;
int motor2Speed = 0;

// Derivative control variables
const float Kd = 0.5; // Derivative gain
int prevErrorMotor1 = 0;
int prevErrorMotor2 = 0;
unsigned long prevTime = 0;

void setup() {
  // Initialize motor pins as outputs
  pinMode(motor1SpeedPin, OUTPUT);
  pinMode(motor1DirPin1, OUTPUT);
  pinMode(motor1DirPin2, OUTPUT);

  pinMode(motor2SpeedPin, OUTPUT);
  pinMode(motor2DirPin1, OUTPUT);
  pinMode(motor2DirPin2, OUTPUT);

  // Initialize serial communication for debugging
  Serial.begin(9600);

  // Store the initial time
  prevTime = millis();
}

void loop() {
  // Read data from DHT11 sensors
  sensor1.read(DHT11_PIN1);
  sensor2.read(DHT11_PIN2);

  // Display temperature and humidity data
  Serial.print("Temperature Sensor 1: ");
  Serial.println(sensor1.temperature);
  Serial.print("Humidity Sensor 1: ");
  Serial.println(sensor1.humidity);
  Serial.print("Temperature Sensor 2: ");
  Serial.println(sensor2.temperature);
  Serial.print("Humidity Sensor 2: ");
  Serial.println(sensor2.humidity);

  // Set motor direction
  digitalWrite(motor1DirPin1, HIGH);
  digitalWrite(motor1DirPin2, LOW);

  digitalWrite(motor2DirPin1, HIGH);
  digitalWrite(motor2DirPin2, LOW);

  // Compute temperature errors
  int errorMotor1 = sensor1.temperature - desiredTemperature;
  int errorMotor2 = sensor2.temperature - desiredTemperature;

  // Calculate time difference in seconds
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - prevTime) / 1000.0;

  // Calculate derivative terms for each motor
  float derivativeMotor1 = (errorMotor1 - prevErrorMotor1) / deltaTime;
  float derivativeMotor2 = (errorMotor2 - prevErrorMotor2) / deltaTime;

  // Update previous error values
  prevErrorMotor1 = errorMotor1;
  prevErrorMotor2 = errorMotor2;

  // Update the previous time
  prevTime = currentTime;

  // Compute motor speeds with derivative control
  motor1Speed = calculateMotorSpeed(errorMotor1, derivativeMotor1);
  motor2Speed = calculateMotorSpeed(errorMotor2, derivativeMotor2);

  // Write motor speeds
  analogWrite(motor1SpeedPin, motor1Speed);
  analogWrite(motor2SpeedPin, motor2Speed);

  // Wait for 1 second before repeating the loop
  delay(1000);
}

// Function to calculate motor speed based on error and derivative
int calculateMotorSpeed(int error, float derivative) {
  int speed = 0;

  if (error > 5) {
    speed = 255;
  } else if (error > 3) {
    speed = 200 + Kd * derivative;
  } else if (error > 0) {
    speed = 150 + Kd * derivative;
  } else {
    speed = 0;
  }

  // Ensure speed is within valid PWM range
  return constrain(speed, 0, 255);
}