/*
  GPS Fake using custom chip
  by Anderson Costa

  GPS NMEA 0183 Messaging Protocol 101
  https://docs.arduino.cc/learn/communication/gps-nmea-data-101

  NMEA Generator
  https://www.nmeagen.org/
*/

#include <SoftwareSerial.h>
#include "NMEA.h"

#define MODULO_CENTRAL          0xFF // Define o ID do modulo central
#define TIPO_MODULOS            0x00 // Define tipo como todos os modulos
#define TIPO_MODULO_GPS         0x03 // Define como modulo GPS
#define ID_MODULO_ADDR          0x01 // Endereço do modulo na EEPROM
#define ID_CENTRAL_ADDR         0x02 // Endereço da central na EEPROM
#define COMANDO_ID_MODULO       0x05 // Define o ID do modulo ADUBO
#define COMANDO_SETA_ID_MODULO  0x06 // Define comando para mudar o ID do módulo
#define COMANDO_RESET           0x07 // Comando de reset do módulo
#define COMANDO_DESLIGA         0x08 // Comando para desativar o modo plantio
#define COMANDO_LIGA            0x09 // Comando para ativar o modo plantio
#define COMANDO_VELOCIDADE_GPS  0x0A // Comando velocidade do GPS
#define CAN_MODULO_ORIGEM          0 // Define a posição do módulo origem da rede
#define CAN_DESTINO                1 // Define a posição do módulo destino da rede
#define CAN_COMANDO                2 // Define a posição do comando da rede
#define CAN_DADOS_1                3 // Define a posição 1 dos dados da CAN
#define CAN_DADOS_2                4 // Define a posição 2 dos dados da CAN
#define CAN_DADOS_3                5 // Define a posição 3 dos dados da CAN
#define CAN_DADOS_4                6 // Define a posição 4 dos dados da CAN
#define CAN_DADOS_5                7 // Define a posição 5 dos dados da CAN
#define CAN_DADOS_6                8 // Define a posição 6 dos dados da CAN

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

// // Defina os pinos para o Software Serial (RX e TX)
const int gpsRxPin = 2;  // Pino RX do GPS, conecte ao TX do GPS
const int gpsTxPin = 3;  // Pino TX do GPS, não é usado neste exemplo

// Cria o objeto SoftwareSerial
SoftwareSerial gpsSerial(gpsRxPin, gpsTxPin);

union {
  char bytes[4];
  float valor;
} velocidadeGPS;

float latitude;
float longitude;

char serialData;

NMEA gps(ALL); // Cria uma conexão de dados GPS com o tipo de sentença GPRMC

uint8_t data[8]; // Data field

void setup() {
  Serial.begin(115200); // Inicializa o Serial padrão
  gpsSerial.begin(9600); // Inicializa o Software Serial para leitura de dados GPS
  Serial.println("Dados recebidos do NMEA:");
}

void loop() {
  // Aguarda dados na porta serial do GPS (Software Serial)
  while (gpsSerial.available()) {
    // Recebe dados do GPS do Software Serial
    serialData = static_cast<char>(gpsSerial.read());
    // Verifica se a sentença do GPS é válida
    Serial.print(serialData);

    if (gps.decode(serialData)) {
      // Checks if GPS status is 'A'
      if ((gps.gprmc_status() == 'A')) {
        // Receives GPS speed in km/h
        velocidadeGPS.valor = gps.gprmc_speed(KMPH);

        data[CAN_MODULO_ORIGEM] = TIPO_MODULO_GPS;
        data[CAN_DESTINO] = MODULO_CENTRAL; // Envia para o modulo central
        data[CAN_COMANDO] = COMANDO_VELOCIDADE_GPS;
        memcpy(data + CAN_DADOS_1, &velocidadeGPS.valor, sizeof(velocidadeGPS.valor));

        latitude = gps.gprmc_latitude();
        longitude = gps.gprmc_longitude();

        // Show Latitude
        Serial.print(" Latitude: ");
        Serial.println(latitude, 8);

        // Show Longitude
        Serial.print("Longitude: ");
        Serial.println(longitude, 8);

        // Show Speed ​​in km/h
        Serial.print("    Speed: ");
        Serial.print(velocidadeGPS.valor);
        Serial.print(" Km/h");
        Serial.print(" CAN Send:");

        for (uint8_t i = 0; i < 8; i++) {
          Serial.print(" ");
          Serial.print(data[i], DEC);
        }

        Serial.println();

      } else {
        velocidadeGPS.valor = 0;
      }

      // if (gps.type() == GPGGA) {
          Serial.println();
          Serial.print("Número de satélites: ");
          Serial.print(gps.gpgga_satellites());
      // }
    }
  }
}

void convertCoordinatesToCartesian(float latitude, float longitude) {
  // Converte Coordenadas Geográficas para Plano Cartesiano
  float latRadius = latitude  * (PI) / 180; // Convert from Degrees to Radians
  float lonRadius = longitude * (PI) / 180;

  int earthRadius = 6371; // Radius in km

  float posX = earthRadius * cos(latRadius) * cos(lonRadius);
  float posY = earthRadius * cos(latRadius) * sin(lonRadius);

  Serial.print("        X: ");  Serial.println(posX);
  Serial.print("        Y: ");  Serial.println(posY);
}