#include <Wire.h>
#include <MechaQMC5883.h>
#include<TinyGPS++.h>
#include<SoftwareSerial.h>
#include <WiFi.h>
#include <IOXhop_FirebaseESP32.h>


float latc,logc;
float latd,logd;
float bearing;
float heading;

const int rp = 10;
const int rn = 11;
const int lp = 8;
const int ln = 9;



#define FIREBASE_HOST "rover-a642c.firebaseio.com" 
#define FIREBASE_AUTH "I4lIsthExTTjueCw2XjRWuP381NnQjzws2f5fWxj" 

#define WIFI_SSID "BANG-LAW WALA" 
#define WIFI_PASSWORD "home@home" 

SoftwareSerial GPS_SoftSerial(16,17);

TinyGPSPlus gps;

MechaQMC5883 qmc;

void setup() 
{
 pinMode(rp,OUTPUT);
 pinMode(rn,OUTPUT);
 pinMode(ln,OUTPUT);
 pinMode(lp,OUTPUT);
 Wire.begin();
 Serial.begin(9600);
 GPS_SoftSerial.begin(9600); 
 qmc.init();
 // connect to wifi. 
   WiFi.begin(WIFI_SSID, WIFI_PASSWORD); 
   Serial.print("connecting"); 
   while (WiFi.status() != WL_CONNECTED) { 
     Serial.print("."); 
     delay(500); 
   } 
   Serial.println(); 
   Serial.print("connected: "); 
   Serial.println(WiFi.localIP()); 
   Firebase.begin(FIREBASE_HOST, FIREBASE_AUTH); 
}

void loop() 
{
  String latd = Firebase.getString("Latitude");
  String logd = Firebase.getString("Longitude");
  //Convert String to float
  Serial.println(latd);
  Serial.println(logd);
  headingcal();
  delay(200);
  //gpsdata();
  smartDelay(1000);
  unsigned long start;
  double lat_val, lng_val, alt_m_val;
  bool loc_valid, alt_valid;
  lat_val = gps.location.lat(); 
  loc_valid = gps.location.isValid(); 
  lng_val = gps.location.lng();
  alt_m_val = gps.altitude.meters(); 
  alt_valid = gps.altitude.isValid(); 
  
  if (!loc_valid)
  {
    Serial.print("Latitude : ");
    Serial.println("*****");
    Serial.print("Longitude : ");
    Serial.println("*****");
    delay(100);
  }
  else
  {
    Serial.println("GPS READING: ");
    //    DegMinSec(lat_val);
    Serial.print("Latitude in Decimal Degrees : ");
    Serial.println(lat_val, 6);

    //n  DegMinSec(lng_val); 
    Serial.print("Longitude in Decimal Degrees : ");
    Serial.println(lng_val, 6);
    delay(100);
  }
  latc=lat_val;
  logc=lng_val;
  //Convert Float to String and than send data to firebase

  String latcc,logcc;
  latcc = String(latc);
  logcc = String(logc);
  Serial.println(latcc);
  Serial.println(logcc);
  Firebase.setString("Latitude1", latcc);
  Firebase.setString("Longitude1", logcc); 
  delay(200);
  gpsheading();
  delay(200);
  steering();
  delay(200);
}

/*
static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do
  {
    while (GPS_SoftSerial.available()) 
    gps.encode(GPS_SoftSerial.read());
  } while (millis() - start < ms);
}*/