//EMAIL:
//PASS:

//#include "NMEA.h"
#include <WiFi.h>
#include <WiFiClient.h>
#include <ThingsBoard.h>
#include <Arduino_MQTT_Client.h>
#include <TinyGPS++.h>
#include <HardwareSerial.h>
#include <HCSR04.h>

char ssid[] = "Wokwi-GUEST";
char pass[] = "";
#define server "thingsboard.cloud"
#define token "0SPrT9zKD6JatgtNIR49"

#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))

#define INC         18
#define IND         19
#define INA         13
#define INB         14
#define buzzer      4
#define TRIG        26
#define ECHO        25
#define PSWITCH     5

TinyGPSPlus gps;
HardwareSerial gpsSerial(2);
UltraSonicDistanceSensor SONOR(TRIG,ECHO);

float latitude;
float longitude; 
float speed;
bool found,start;
bool obstracle;

//NMEA gps(GPRMC);                            // Creates a GPS data connection with sentence type GPRMC
static const uint32_t GPSBaud = 9600;
constexpr uint16_t MAX_MESSAGE_SIZE = 256U;
WiFiClient espClient;
Arduino_MQTT_Client mqttClient(espClient);
ThingsBoard tb(mqttClient, MAX_MESSAGE_SIZE);

void setupWiFi(){
  Serial.print("\nConnecting to ");
  Serial.println(ssid);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, pass);

  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.print("\nConnected to ");
  Serial.println(ssid);
}

void connectToThingsBoard() {
  if (!tb.connected()) {
    Serial.println("Connecting to ThingsBoard server");
    
    if (!tb.connect(server, token)) {
      Serial.println("Failed to connect to ThingsBoard");
    } else {
      Serial.println("Connected to ThingsBoard");
    }
  }
}


void setup() 
{
  pinMode(INC,OUTPUT);
  pinMode(IND,OUTPUT);
  pinMode(INA,OUTPUT);
  pinMode(INB,OUTPUT);
  pinMode(PSWITCH,INPUT);
  pinMode(buzzer,OUTPUT);
  digitalWrite(buzzer,LOW);
  move('S');
  Serial.begin(115200);
  delay(500);
  gpsSerial.begin(9600, SERIAL_8N1, 16, 17);
  Serial.println();
  setupWiFi();
}

void loop() 
{
    if(digitalRead(PSWITCH))
    {
        while(!obstracle)
        {
            move('F');
            readGPS();
            sendData();
            checkObstracle();  
        } 
        while(obstracle)
        {
            move('S');
            move('T');
            Alert(2);
            checkObstracle(); 
        }
    }
    else
    {
       move('S');
    }
}

void sendData()
{
  if (!tb.connected()) 
   {
    connectToThingsBoard();
   }
   tb.sendTelemetryData("speed", speed);
   tb.sendTelemetryData("latitude", latitude);
   tb.sendTelemetryData("longitude", longitude);
   tb.loop();
   delay(500);
}

void readGPS()
{
  unsigned long start = millis();
  while (millis() - start < 1000) {
    while (gpsSerial.available() > 0) 
    {
      gps.encode(gpsSerial.read());
    }
    if (gps.location.isUpdated()) 
    {
      latitude=gps.location.lat();
      longitude=gps.location.lng();
      speed=gps.speed.kmph();
      Serial.print("LAT: ");
      Serial.println(latitude);
      Serial.print("LONG: "); 
      Serial.println(longitude);
      Serial.print("SPEED: "); 
      Serial.println(speed);
    }
  }
}

void checkObstracle()
{
  double distance=SONOR.measureDistanceCm();
  if(distance>2 && distance<50)
  {
   obstracle=HIGH;
  }
  else
  {
    obstracle=LOW;
  }
}

void move(char dir)
{
  if(dir=='F')
  {
    digitalWrite(INA,HIGH);
    digitalWrite(INB,LOW);
    digitalWrite(INC,LOW);
    digitalWrite(IND,HIGH);
  }
  if(dir=='R')
  {
    digitalWrite(INA,HIGH);
    digitalWrite(INB,LOW);
    digitalWrite(INC,HIGH);
    digitalWrite(IND,LOW);
  }
  if(dir=='S')
  {
    digitalWrite(INA,LOW);
    digitalWrite(INB,LOW);
    digitalWrite(INC,LOW);
    digitalWrite(IND,LOW);
  }
  if(dir=='T')
  {
    digitalWrite(INA,HIGH);
    digitalWrite(INB,LOW);
    digitalWrite(INC,LOW);
    digitalWrite(IND,HIGH);
  }
}

void Alert(uint8_t num)
{
  for(num;num>0;num--)
  {
    tone(buzzer, 100);
    delay(750);
    noTone(buzzer);
    delay(500);
  }
}