#include <Arduino.h>
#include <RadioLib.h>
#include <Wire.h>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>

// SX1278 has the following connections:
// NSS pin:   10
// DIO0 pin:  2
// RESET pin: 9
// DIO1 pin:  3
RFM96 radio = new Module(10, 2, 9, 3);

HardwareSerial SerialGNSS(PD6, PD5);
SFE_UBLOX_GNSS gnss;
long lastGnssPoll = 0;

static char letterize(int x) {
    return (char) x + 65;
}
static char* get_mh(double lat, double lon, int size) {
    static char locator[11];
    double LON_F[]={20,2.0,0.083333,0.008333,0.0003472083333333333};
    double LAT_F[]={10,1.0,0.0416665,0.004166,0.0001735833333333333};
    int i;
    lon += 180;
    lat += 90;

    if (size <= 0 || size > 10) size = 6;
    size/=2; size*=2;

    for (i = 0; i < size/2; i++){
        if (i % 2 == 1) {
            locator[i*2] = (char) (lon/LON_F[i] + '0');
            locator[i*2+1] = (char) (lat/LAT_F[i] + '0');
        } else {
            locator[i*2] = letterize((int) (lon/LON_F[i]));
            locator[i*2+1] = letterize((int) (lat/LAT_F[i]));
        }
        lon = fmod(lon, LON_F[i]);
        lat = fmod(lat, LAT_F[i]);
    }
    locator[i*2]=0;
    return locator;
}

void setup() {
    Serial.begin(9600);
    pinMode(LED_BUILTIN, OUTPUT);

    Wire.begin();

    SerialGNSS.begin(9600);
    if (!gnss.begin(SerialGNSS)) {
        while (true) {
            Serial.println(F("Check GNSS"));
            delay(4000);
        }
    }

    //gnss.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial

    Serial.print(F("Version: "));
    byte versionHigh = gnss.getProtocolVersionHigh();
    Serial.print(versionHigh);
    Serial.print(".");
    byte versionLow = gnss.getProtocolVersionLow();
    Serial.println(versionLow);

#if 0
    Serial.print(F("[RFM] Init "));
    int state = radio.begin();
    if (state == RADIOLIB_ERR_NONE) {
        Serial.println(F("success!"));
    } else {
        Serial.print(F("failed, code "));
        Serial.println(state);
        while (true);
    }

    // radio.setRfSwitchPins(4, 5);
#endif
}

void loop()
{
    const auto now = millis();
    if (now - lastGnssPoll > 1000) {
        lastGnssPoll = now;

        long latitude = gnss.getLatitude();
        Serial.print(F("Lat: "));
        Serial.print(latitude);

        long longitude = gnss.getLongitude();
        Serial.print(F(" Long: "));
        Serial.print(longitude);

        char* locator = get_mh((double)latitude/1e7, (double)longitude/1e7, 6);
        Serial.print(F(" Loc: "));
        Serial.print(locator);

        long altitude = gnss.getAltitude();
        Serial.print(F(" Alt: "));
        Serial.print(altitude);
        Serial.print(F(" (mm)"));

        long speed = gnss.getGroundSpeed();
        Serial.print(F(" Speed: "));
        Serial.print(speed);
        Serial.print(F(" (mm/s)"));

        long heading = gnss.getHeading();
        Serial.print(F(" Heading: "));
        Serial.print(heading);
        Serial.print(F(" (degrees * 10^-5)"));

        byte SIV = gnss.getSIV();
        Serial.print(F(" SIV: "));
        Serial.print(SIV);

        Serial.println();
    }
}

void loop2()
{
    digitalWrite(LED_BUILTIN, HIGH);
    delay(1000);

    Serial.print(F("[RFM] Transmitting packet ... "));

    int state = radio.transmit("Hello World!");

    if (state == RADIOLIB_ERR_NONE) {
        Serial.println(F(" success!"));
        Serial.print(F("[RFM] Datarate:\t"));
        Serial.print(radio.getDataRate());
        Serial.println(F(" bps"));

    } else if (state == RADIOLIB_ERR_PACKET_TOO_LONG) {
        Serial.println(F("too long!"));

    } else if (state == RADIOLIB_ERR_TX_TIMEOUT) {
        Serial.println(F("timeout!"));

    } else {
        Serial.print(F("failed, code "));
        Serial.println(state);
    }

    digitalWrite(LED_BUILTIN, LOW);
    delay(1000);
}