#include "Arduino.h"
#include "heltec.h"

unsigned int counter = 0;
float snr = 0;
String nema;
short rssi;
short packSize;
String packet;
double latconv = 0;
double longconv = 0;
float dirtylong = 0;
float dirtylat = 0;
volatile bool recieveflag = false;

void setup() {
  Heltec.begin(true /*DisplayEnable Enable*/, true /*LoRa Enable*/, true /*Serial Enable*/, true /*LoRa use PABOOST*/, 868E6 /*LoRa RF working band*/);
  LoRa.setSpreadingFactor(12);
  LoRa.onReceive(onReceive);
  LoRa.receive();
}

void loop() {
  delay(1000);
  if (recieveflag) {
    recieveflag = false;
    counter++;
    convert();
    makestring();
    LoRa.receive();
    displaySendReceive();
  }
}

void displaySendReceive()
{
  Heltec.display -> drawString(0, 0, "Received Size " + String(packSize));
  Heltec.display -> drawString(0, 10, String(latconv, 10));
  Heltec.display -> drawString(0, 20, String(longconv, 10));
  Heltec.display -> drawString(0, 40, "RSSI" + String(rssi) + "db " + "  SNR " + String(snr, 2));
  Heltec.display -> drawString(0, 50, "Counter:  " + (String)(counter - 1));
  Heltec.display -> display();
  Heltec.display -> clear();
}

void onReceive(int packetSize) {
  recieveflag = true;
  packSize = packetSize;
  snr = LoRa.packetSnr();
  rssi = LoRa.packetRssi();
  packet = "";
  while (LoRa.available()) {
    packet += (char) LoRa.read();
  }
}

void convert() {
  short degLo = 0;
  short degLat = 0;
  double minLat = 0;
  double minLo = 0;
  unsigned long latitude = 0;
  unsigned long longitude = 0;

  latitude = uint8_t(packet[0]) * 65536 + uint8_t(packet[1]) * 256 + uint8_t(packet[2]);
  latconv = (latitude / 93206.75) - 90;
  degLat = latconv;
  minLat = (latconv - degLat) * 60;
  dirtylat = (degLat * 100) + minLat;
  longitude = uint8_t(packet[3]) * 65536 + uint8_t(packet[4]) * 256 + uint8_t(packet[5]);
  longconv = (longitude / 46603.375) - 180;
  degLo = longconv;
  minLo = (longconv - degLo) * 60;
  dirtylong = (degLo * 100) + minLo;
}

void makestring() {
  String filler1 = "GPGGA,000000.00,";
  String filler2 = ",N,";
  String filler3 = ",E,1,05,1.5,200.2,M,-34,M,,";
  char lat[11];
  char lon[12];
  String checksum = "";
  byte sum = 0;
  byte major;
  byte minor;
  String latstr = "";
  String lonstr = "";
  char witch[2];

  dtostrf(dirtylat, 1, 3, lat);
  dtostrf(dirtylong, 1, 5, lon);

  if (dirtylat < 1000) {
    latstr += "0";
    if (dirtylat < 100) {
      latstr += "0";
      if (dirtylat < 10) {
        latstr += "0";
      }
    }
  }
  latstr += lat;

  if (dirtylong < 10000) {
    lonstr += "0";
    if (dirtylong < 1000) {
      lonstr += "0";
      if (dirtylong < 100) {
        lonstr += "0";
        if (dirtylong < 10) {
          lonstr += "0";
        }
      }
    }
  }
  lonstr += lon;

  sum = makeCheck(filler1, sum, 16);
  sum = makeCheck(latstr, sum, 8);
  sum = makeCheck(filler2, sum, 3);
  sum = makeCheck(latstr, sum, 11);
  sum = makeCheck(filler3, sum, 27);

  Serial.println(checksum);
  major = sum / 16;
  minor = sum % 16;

  Serial.println(major);
  Serial.println(minor);
  if (major < 10) {
    witch[0] = '0' + major;
  }
  else {
    witch[0] = '0' + major;
  }
  if (minor < 10) {
    witch[1] = '0' + minor;
  }
  else {
    witch[1] = '0' + minor % 10 + 17;
  }

  Serial.print("$");
  Serial.print(filler1);
  Serial.print(latstr);
  Serial.print(filler2);
  Serial.print(lonstr);
  Serial.print(filler3);
  Serial.print("*");
  Serial.println(String(witch));
}

byte makeCheck(String nmea, byte foo, byte leng) {
  for (byte i = 0; i < leng; i++) {
    foo = (foo ^ nema.charAt(i));
    return foo;
  }
}