#include <DHT.h>

// -------- ULTRASONIC SENSOR --------
#define TRIG A0
#define ECHO 13

// -------- DHT22 SENSOR --------
#define DHTPIN A1
#define DHTTYPE DHT22

DHT dht(DHTPIN, DHTTYPE);

// -------- MOTOR 1 --------
#define M1A 9
#define M1B 8

// -------- MOTOR 2 --------
#define M2A 7
#define M2B 6

long duration;
float distance;

float temperature;
float humidity;

void setup() {

  Serial.begin(9600);

  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);

  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  dht.begin();

  stopRobot();

  delay(2000);
}

void loop() {

  // Read ultrasonic distance
  distance = getDistance();

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

  // Print distance
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print(" cm | ");

  // Print DHT22 values
  if (isnan(humidity) || isnan(temperature)) {

    Serial.println("DHT22 Error!");

  } else {

    Serial.print("Temperature: ");
    Serial.print(temperature);
    Serial.print(" C | Humidity: ");
    Serial.print(humidity);
    Serial.println(" %");
  }


  // -------- OBJECT DETECTED --------
  if (distance <= 25) {

    // Stop robot
    stopRobot();

    // Stop for 5 seconds
    delay(5000);

    // Turn LEFT
    turnLeft();

    // Turning time
    delay(500);

    // Stop after turning
    stopRobot();
    delay(200);

  } else {

    // No object → move straight
    moveForward();
  }

  delay(100);
}


// =================================
// ULTRASONIC FUNCTION
// =================================

float getDistance() {

  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);

  digitalWrite(TRIG, LOW);

  duration = pulseIn(ECHO, HIGH, 30000);

  if (duration == 0) {
    return 999;
  }

  float dist = duration * 0.034 / 2;

  return dist;
}


// =================================
// MOVE FORWARD
// =================================

void moveForward() {

  digitalWrite(M1A, HIGH);
  digitalWrite(M1B, LOW);

  digitalWrite(M2A, HIGH);
  digitalWrite(M2B, LOW);
}


// =================================
// STOP ROBOT
// =================================

void stopRobot() {

  digitalWrite(M1A, LOW);
  digitalWrite(M1B, LOW);

  digitalWrite(M2A, LOW);
  digitalWrite(M2B, LOW);
}


// =================================
// TURN LEFT
// =================================

void turnLeft() {

  // Left motor backward
  digitalWrite(M1A, LOW);
  digitalWrite(M1B, HIGH);

  // Right motor forward
  digitalWrite(M2A, HIGH);
  digitalWrite(M2B, LOW);
}