#define BLYNK_TEMPLATE_ID "TMPL3Q862KRXu"
#define BLYNK_TEMPLATE_NAME "GPS DATA"
#define BLYNK_AUTH_TOKEN "vZRcekL-b9lqLEkKIoGxalBaGHbv-HNZ"

//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 = 32; 
const int TRIG_PIN3 = 2;
const int ECHO_PIN3 = 4;

const int LED_PIN = 26;
const int BUZZER_PIN = 25;

//define sound speed in cm/uS
//define SOUND_SPEED 0.034
// Blynk synchronization

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

#define GPS_BAUDRATE 9600  // The default baudrate of NEO-6M is 9600
BlynkTimer timer;
Adafruit_MPU6050 mpu; //accelerometer object creation
TinyGPSPlus gps;  // the TinyGPS++ object
float lati, longi, speed;
int sats, alti;
String loc;
// Your WiFi credentials.
// Set password to "" for open networks.
char auth[] = BLYNK_AUTH_TOKEN; 
char ssid[] = "Wokwi-GUEST";  // type your wifi name
char pass[] = "";             // type your wifi password

// required for conditioning the gyroscope...
int check( float []);
int m=0;
float temp[6];




void setup() {
  Serial.begin(9600); //for serial monitor
  //Blynk and GPS setup
  Serial1.begin(GPS_BAUDRATE); //serial1 for GPS UART
  Blynk.begin(auth, ssid, pass);
  
  Serial.println(F("ESP32 - GPS module"));
  //timer.setInterval(5000L, checkGPS); // every 5s check if GPS is connected, only really needs to be done once
//Ultrasonic sensor setup
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite (LED_PIN, 1);
  digitalWrite (BUZZER_PIN, 1);
// Gyroscope sensor setup
Serial.println("Adafruit MPU6050 test!");
// Try to initialize MPU 6050!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  else{
  Serial.println("MPU6050 Found!");
  }
  // ACCELEROMETER PROPERTIES
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);


}


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

void loop() 
{

// Blynk server and GPS module code----


int hour, min, sec;
int pq;
String date_time;
 int siren;
  float X1,Y1,Z1, X2,Y2,Z2;
  float arr[6];
  

    //MPU6050 SENSOR RUNNING CODE----

 sensors_event_t a, g, temp;
 mpu.getEvent(&a, &g, &temp);
  /* PRINT ACCELARATION VALUES */
  X1= a.acceleration.x;
  Y1= a.acceleration.y;
  Z1= a.acceleration.z;
  Serial.print("Acceleration X: ");
  Serial.print(X1);
  Serial.print(", Y: ");
  Serial.print(Y1);
  Serial.print(", Z: ");
  Serial.print(Z1);
  Serial.println(" m/s^2");

// ROTATION VALUES PRINT
X2 =g.gyro.x;
Y2 = g.gyro.y;
Z2= g.gyro.z;


  Serial.print("Rotation X: ");
  Serial.print(X2);
  Serial.print(", Y: ");
  Serial.print(Y2);
  Serial.print(", Z: ");
  Serial.print(Z2);
  Serial.println(" rad/s");

//CONDITIONING FUNCTION CHECK()
arr[0]= X1;
arr[1]= Y1;
arr[2]= Z1;
arr[3]= X2;
arr[4]= Y2;
arr[5]= Z2;


siren = check(arr);
Serial.print(": OUTPUT : ");
Serial.println(siren);
if(siren>0)
 Blynk.virtualWrite(V3, HIGH);
else
Blynk.virtualWrite(V3, LOW);

//GPS MODULE CODE....//

  if (Serial1.available() > 0) {
    if (gps.encode(Serial1.read())) {

      if (gps.location.isValid()) {

      sats = gps.satellites.value();       //get number of satellites   
      lati= gps.location.lat();            // latitude
      longi= gps.location.lng();           // Longitude
      loc ="https://maps.google.com/maps/place/"+ String(lati,5)+", "+ String(longi,5); //form a google map URL 
      Serial.println(loc); 

      } 
      else {
        Serial.println(F("- location: INVALID"));
      }

      Serial.print(F("- speed: "));
      if (gps.speed.isValid()) {
         speed= 0.277778* gps.speed.kmph();
        Serial.print(speed);
        Serial.println(F("m/sec"));
      } else {
        Serial.println(F("INVALID"));
      }
      Serial.print(F("- altitude: "));
        if (gps.altitude.isValid())
        {
          alti = gps.altitude.meters();
          Serial.println(alti);
          // altitude checking..


        }
        

      Serial.print(F("- GPS date&time: "));
      if (gps.date.isValid() && gps.time.isValid()) {
        
        hour =gps.time.hour();
        min = gps.time.minute();
        sec= gps.time.second();
        // code for conversation of time to local IST
        min = min+30;  // local IST 5:30 extra
        if (min> 59)
        {
          min= min-60;
          hour = hour+1;
        } 
        hour = hour+5; //local IST
        if (hour> 23 )
          hour = hour-24;
        //checking for AM/PM
        if (hour< 12) // AM
        {
           date_time = String(gps.date.day())+"/"+ String(gps.date.month())+"/"+String(gps.date.year())+" : "+ String(hour)+"."+ String (min)+"."+String(gps.time.second())+"AM " ;
        }
        else
        date_time = String(gps.date.day())+"/"+ String(gps.date.month())+"/"+String(gps.date.year())+" : "+ String(hour)+"."+ String (min)+"."+String(gps.time.second())+"PM " ;


        Serial.println(date_time);


      } else {
        Serial.println(F("INVALID"));
      }

      Serial.println();
        Blynk.virtualWrite(V1, String(lati, 4));
        Blynk.virtualWrite(V2, String(longi, 4));
        //Blynk.virtualWrite(V3, sats);
        Blynk.virtualWrite(V4, speed);
        Blynk.virtualWrite(V5, loc);


    }
  }

 // if (millis() > 5000 && gps.charsProcessed() < 10)
   // Serial.println(F("No GPS data received: check wiring"));




  Blynk.run();
  timer.run();

// Ultrasonic sensor running code------

long duration1, duration2, duration3;
long distance1, distance2, distance3;

  // Sensor 1
  digitalWrite(TRIG_PIN1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN1, LOW);
  duration1 = pulseIn(ECHO_PIN1, HIGH);
  distance1 = duration1 * 0.034 / 2;

  // Sensor 2
  digitalWrite(TRIG_PIN2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN2, LOW);
  duration2 = pulseIn(ECHO_PIN2, HIGH);
  distance2 = duration2 * 0.034 / 2;

  // Sensor 3
  digitalWrite(TRIG_PIN3, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN3, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN3, LOW);
  duration3 = pulseIn(ECHO_PIN3, HIGH);
  distance3 = duration3 * 0.034 / 2;

  /*Serial.print("Distance 1 : ");
  Serial.print(distance1);
  Serial.print(" cm\tDistance 2 : ");
  Serial.print(distance2);
  Serial.print(" cm\tDistance 3 : ");
  Serial.print(distance3);
  Serial.println(" cm");
*/
  // Map the distance to buzzer frequency
  //int frequency = map(averageDistance, 5, 100, 500, 2000);
  //frequency = constrain(frequency, 200, 2000);

  // Check distances and activate buzzer if an obstacle is too close
  if (distance1 <20 || distance2 <20 || distance3 <20) 
  {
   // tone(BUZZER_PIN, frequency);
     digitalWrite (BUZZER_PIN, 0);
     delay(50);
     digitalWrite (BUZZER_PIN,1);
      digitalWrite(LED_PIN, 0);
      delay(50);
      digitalWrite (LED_PIN, 1);

  } 

 else if ((distance1>= 20 && distance1<40)||(distance2>=20 && distance2 <40)|| (distance3 >= 20 && distance3 <40)) 
  {
   // tone(BUZZER_PIN, frequency);
     digitalWrite (BUZZER_PIN, 0);
     delay(100);
     digitalWrite (BUZZER_PIN,1);
      digitalWrite(LED_PIN, 0);
      delay(100);
      digitalWrite (LED_PIN, 1);

  } 
   else if ((distance1>= 40 && distance1<70)||(distance2>=40 && distance2 <70)|| (distance3 >= 60 && distance3 <70)) 
   {
    digitalWrite (BUZZER_PIN, 0);
    delay(500);
    digitalWrite(BUZZER_PIN, 1);
    digitalWrite (LED_PIN, 1);
   }
else 
    digitalWrite(BUZZER_PIN, 1);
    digitalWrite (LED_PIN, 1);
  delay(500);
}


int check (float arr[])
{
if (m== 0)
{
  for (int i =0; i<6; i++)
  temp[i] = arr[i];
  m=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);
  
}

}