#define BLYNK_TEMPLATE_ID "TMPL3mpB-ef1X"
#define BLYNK_DEVICE_NAME "Obstacle Avoiding Robot"
#define BLYNK_AUTH_TOKEN "Sx-tDIt_IsF6KAPngKxsYWFP_T7Dksl2"

// Libraries
#include <BlynkSimpleEsp8266.h>
#include <NewPing.h>

// Pin Definitions
#define TRIG_PIN 7   // Ultrasonic Sensor Trigger
#define ECHO_PIN 8   // Ultrasonic Sensor Echo
#define LEFT_MOTOR_FORWARD 9
#define LEFT_MOTOR_BACKWARD 10
#define RIGHT_MOTOR_FORWARD 11
#define RIGHT_MOTOR_BACKWARD 12 

// Parameters
#define MAX_DISTANCE 200  // Max distance to detect obstacles
#define SAFE_DISTANCE 20  // Minimum safe distance

// Blynk Variables
char auth[] = "Sx-tDIt_IsF6KAPngKxsYWFP_T7Dksl2";
char ssid[] = "Airtel_mood_6525";
char pass[] = "Ammulu@1234";

// Ultrasonic Sensor
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Movement Functions
void moveForward() {
  analogWrite(LEFT_MOTOR_FORWARD, 150);
  analogWrite(RIGHT_MOTOR_FORWARD, 150);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
}

void moveBackward() {
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, 150);
  analogWrite(RIGHT_MOTOR_BACKWARD, 150);
}

void turnLeft() {
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_FORWARD, 150);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
}

void turnRight() {
  analogWrite(LEFT_MOTOR_FORWARD, 150);
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
}

void stopMotors() {
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
}

void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);
  
  // Blynk Initialization
  Blynk.begin(auth, ssid, pass);

  // Motor Pins Setup
  pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
  pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
  pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
  pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);

  Serial.println("Robot Ready");
}

void loop() {
  Blynk.run(); // Run Blynk
  unsigned int distance = sonar.ping_cm(); // Get distance in cm

  // Send telemetry data to Blynk
  Blynk.virtualWrite(V0, distance); // Distance data
  if (distance < SAFE_DISTANCE && distance > 0) {
    stopMotors();
    moveBackward();
    delay(500);
    turnLeft();
    delay(500);
  } else {
    moveForward();
  }

  delay(100);
}