//Intelligent Blind man's Stick - with GPS (NEO-6M) & Gyroscope(MPU6050) assistance synchronized with Blynk- IoT------//

// Ultrasonic sensors declarations
const int TRIG_PIN1 = 5;
const int ECHO_PIN1 = 18; 
const int TRIG_PIN2 = 33;
const int ECHO_PIN2 = 9; 
const int TRIG_PIN3 = 37;
const int ECHO_PIN3 = 4;

const int LED_PIN = 26;
const int BUZZER_PIN = 20;

//define sound speed in cm/uS
//define SOUND_SPEED 0.034
// Blynk synchronization
#define BLYNK_TEMPLATE_ID "TMPL3u9clHnOO"
#define BLYNK_TEMPLATE_NAME "BLINDSTICK1"
#define BLYNK_AUTH_TOKEN "WZSKWYTJ1YCkDsfwL3xcUQqKA_KvzqJj"

#define BLYNK_PRINT Serial
//#define BLYNK_PRINT Serial1
#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>
#include <TinyGPS++.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <SoftwareSerial.h>



//Intelligent Blind man's Stick - with GPS (NEO-6M) & Gyroscope(MPU6050) assistance synchronized with Blynk- IoT------//

// Ultrasonic sensors declarations

#define GPS_BAUDRATE 9600
//SoftwaSerial GPS_Serial(1); // Use Serial1 for GPS
HardwareSerial GPS_Serial(1);
// MPU6050 configuration
#define MPU6050_DEVICE_ID 0x68

// WiFi credentials
char auth[] = BLYNK_AUTH_TOKEN; 
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

// Timer for Blynk
BlynkTimer timer;

// Sensor objects
Adafruit_MPU6050 mpu;
TinyGPSPlus gps;

// GPS data
float lati, longi, speed;
int sats, alti;
String loc;

// Gyroscope data handling
int check(float []);
int m = 0;
float temp[6];

void setup() {
  // Initialize Serial for debugging
  Wire.setPins(13, 14); // Set the I2C pins before begin
  Wire.begin();
  Serial.begin(9600);
  
  GPS_Serial.begin(GPS_BAUDRATE, SERIAL_8N1, 17,16);
  // Initialize Serial1 for GPS
  //GPS_Serial.begin(GPS_BAUDRATE);
  //GPS_Serial.setPins(16, 17); // Pins 16 (RX) and 17 (TX)

  // Initialize Blynk
  Blynk.begin(auth, ssid, pass);
  
  // Initialize Ultrasonic sensors
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);

  // Initialize LED and BUZZER_PIN
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  //digitalWrite(LED_PIN, LOW);
  //digitalWrite(BUZZER_PIN_PIN, HIGH);

  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  Serial.println("Setup completed");
}

void checkGPS() {
  if (gps.charsProcessed() < 10) {
    Serial.println(F("No GPS detected: check wiring."));
    Blynk.virtualWrite(V5, "GPS ERROR"); 
  }
}

void loop() {
  // Blynk and timer operations
  Blynk.run();
  timer.run();

  // MPU6050 sensor readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  float X1 = a.acceleration.x;
  float Y1 = a.acceleration.y;
  float Z1 = a.acceleration.z;
  float X2 = g.gyro.x;
  float Y2 = g.gyro.y;
  float Z2 = g.gyro.z;

  Serial.print("Acceleration X: "); Serial.print(X1);
  Serial.print(", Y: "); Serial.print(Y1);
  Serial.print(", Z: "); Serial.println(Z1);
  
  Serial.print("Rotation X: "); Serial.print(X2);
  Serial.print(", Y: "); Serial.print(Y2);
  Serial.print(", Z: "); Serial.println(Z2);

  float arr[6] = {X1, Y1, Z1, X2, Y2, Z2};
  int siren = check(arr);
  Serial.print(": OUTPUT : "); Serial.println(siren);
  Blynk.virtualWrite(V4, siren > 0 ? HIGH : LOW);

  // GPS data processing
  while (GPS_Serial.available() > 0) {
    gps.encode(GPS_Serial.read());
  }
  
  if (gps.location.isUpdated()) {
    lati = gps.location.lat();
    longi = gps.location.lng();
    loc = "https://maps.google.com/maps/place/" + String(lati, 5) + ", " + String(longi, 5);
    Serial.println(loc);

    speed = gps.speed.mps();
    Serial.print("Speed: "); Serial.println(speed);

    alti = gps.altitude.meters();
    Serial.print("Altitude: "); Serial.println(alti);

    int hour = gps.time.hour();
    int min = gps.time.minute();
    int sec = gps.time.second();

    min += 30;
    if (min > 59) {
      min -= 60;
      hour += 1;
    }
    hour += 5;
    if (hour > 23) hour -= 24;

    String date_time = String(gps.date.day()) + "/" + String(gps.date.month()) + "/" + String(gps.date.year()) + " : " + String(hour) + "." + String(min) + "." + String(sec) + (hour < 12 ? "AM" : "PM");
    Serial.println(date_time);
    
  }

  // Ultrasonic sensor readings and BUZZER_PIN/LED control
  long duration1,duration2,duration3;
  digitalWrite(TRIG_PIN1,LOW);
  digitalWrite(TRIG_PIN2,LOW);
  digitalWrite(TRIG_PIN3,LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN1, LOW);
  duration1 = pulseIn(ECHO_PIN1, HIGH);

  digitalWrite(TRIG_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN2, LOW);
  duration2 = pulseIn(ECHO_PIN2, HIGH);

  digitalWrite(TRIG_PIN3, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN3, LOW);
  duration3 = pulseIn(ECHO_PIN3, HIGH);
  int distance1,distance2,distance3;
  distance1=duration1* 0.034 / 2;
  distance2=duration2* 0.034 / 2;
  distance3=duration3* 0.034 / 2;
  
  Blynk.virtualWrite(V1,distance1);
  Blynk.virtualWrite(V2,distance2);
  Blynk.virtualWrite(V3,distance3);
  Blynk.virtualWrite(V5, loc);
  Serial.print("Distance1: ");Serial.println(distance1);
  Serial.print("Distance2: ");Serial.println(distance2);
  Serial.print("Distance3: ");Serial.println(distance3);
  delay(1000);
  Serial.println("CM");

  if (distance1 < 20 || distance2 < 20 || distance3 < 20) {
    tone(BUZZER_PIN,(distance1||distance2||distance3)*400); //set the BUZZER_PIN as an output
    delay(50);
    digitalWrite(LED_PIN, HIGH);
    delay(50);
    digitalWrite(LED_PIN,LOW);
  } else if ((distance1 >= 20 && distance1 < 40) || (distance2 >= 20 && distance2 < 40) || (distance3 >= 20 && distance3 < 40)) {
    tone(BUZZER_PIN,2000); //set the BUZZER_PIN as an output
    delay(500);
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    digitalWrite(LED_PIN, LOW);
  } else if ((distance1 >= 40 && distance1 < 70) || (distance2 >= 40 && distance2 < 70) || (distance3 >= 40 && distance3 < 70)) {
    tone(BUZZER_PIN,5000); //set the BUZZER_PIN as an output
    delay(2000);
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    digitalWrite(LED_PIN, LOW);
  } else {
    noTone(BUZZER_PIN); //stop the tone
    delay(500); 
    digitalWrite(LED_PIN,LOW);
  }

  delay(500);
}

int check(float arr[]) {
  if (m == 0) {
    for (int i = 0; i < 6; i++) temp[i] = arr[i];
    m = 1;
    return 0;
  } else {
    if (abs(arr[0] - temp[0]) > 8 || abs(arr[1] - temp[1]) > 8 || abs(arr[2] - temp[2]) > 8) {
      if (abs(arr[3] - temp[3]) > 1.2 || abs(arr[4] - temp[4]) > 1.2 || abs(arr[5] - temp[5]) > 1.2) {
        for (int i = 0; i < 6; i++) temp[i] = arr[i];
        return 1;
      }
      return 0;
    } else {
      return 0;
    }
  }
}