// Licence: MIT
#include <Arduino.h>
#include <RadioLib.h>
#include <Wire.h>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <SD.h>
#include "compression.h"

// Date Type Codes defined in aprs434.github.io
constexpr int DATA_TYPE_CODE_GEOLOCATION = 0;
constexpr int DATA_TYPE_CODE_STATUS_REPORT = 1;

// TODO: read these settings from the SD Card
constexpr char CALLSIGN[] = "HB9EGM";
constexpr int SSID = 12;
constexpr int PATH_CODE = 2; // metropolitan mobile
constexpr char SYMBOL_TABLE_IDENTIFIER = '/';
constexpr char SYMBOL_CODE_BICYCLE = 'b';
constexpr long REPORT_TX_INTERVAL = 15000;

constexpr long TEXT_TX_INTERVAL = 47000;
constexpr char TEXT_REPORT[] = "mpb.li/git/lora-aprs-hb9egm"; // Max length=28

#if 0
File myFile;
constexpr int SD_CS = 17;
#endif

// SX1278 has the following connections:
// NSS pin:   PD14 (Arduino 10)
// DIO0 pin:  PF3 (Arduino 8)
// RESET pin: PF15 (Arduino 9)
RFM96 radio = new Module(10, 8, 9);
HardwareSerial SerialGNSS(PD6, PD5);
SFE_UBLOX_GNSS gnss;

long lastGnssPoll = 0;
long lastPositionReport = 0;
long lastTextReport = 0;

constexpr size_t MAX_REPORT_LEN = 32;
size_t report_len = 0;
uint8_t report[MAX_REPORT_LEN];

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);

#if 0
    pinMode(SD_CS, OUTPUT);

    if (!SD.begin(SD_CS)) {
        Serial.println("SD init failed!");
        return;
    }
#endif

    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);

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

    if (radio.setBandwidth(125.0) == RADIOLIB_ERR_INVALID_BANDWIDTH) {
        Serial.println(F("Selected bandwidth is invalid for this module!"));
        while (true);
    }

    if (radio.setSpreadingFactor(12) == RADIOLIB_ERR_INVALID_SPREADING_FACTOR) {
        Serial.println(F("Selected spreading factor is invalid for this module!"));
        while (true);
    }

    if (radio.setCodingRate(5) == RADIOLIB_ERR_INVALID_CODING_RATE) {
        Serial.println(F("Selected coding rate is invalid for this module!"));
        while (true);
    }

    // NOTE: value 0x34 is reserved for LoRaWAN networks and should not be used
    if (radio.setSyncWord(0x14) != RADIOLIB_ERR_NONE) {
        Serial.println(F("Unable to set sync word!"));
        while (true);
    }

    if (radio.setOutputPower(16) == RADIOLIB_ERR_INVALID_OUTPUT_POWER) {
        Serial.println(F("Selected output power is invalid for this module!"));
        while (true);
    }

    if (radio.setPreambleLength(8) == RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH) {
        Serial.println(F("Selected preamble length is invalid for this module!"));
        while (true);
    }

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

static void handle_radio_error(int state)
{
    if (state == RADIOLIB_ERR_NONE) {
        Serial.print(F(" RFM OK Datarate: "));
        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);
    }
}

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

        // longitude and latitude are in degrees*1e7
        const long latitude = gnss.getLatitude();
        Serial.print(F("Lat: "));
        Serial.print(latitude);

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

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

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

        const long speed = gnss.getGroundSpeed();
        Serial.print(F(" Speed: "));
        Serial.print(speed);
        Serial.print(F(" (mm/s)"));
        // 1m/s = 900/463 knots
        const double speed_kn = speed / 1000.0 * 900.0 / 463.0;

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

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

        Serial.print(F(" TX in: "));
        Serial.print(REPORT_TX_INTERVAL - (now - lastPositionReport));

        Serial.println();

        if (now - lastPositionReport > REPORT_TX_INTERVAL) {
            lastPositionReport = now;

            digitalWrite(LED_BUILTIN, HIGH);

            // Encode Compressed Geolocation Frame according to aprs434.github.io
            const uint32_t callsign_EEEE = encodeCallsign(CALLSIGN);

            report_len = 0;
            // Callsign encoded as CCCC
            report[report_len++] = (callsign_EEEE >> 24) & 0xFF;
            report[report_len++] = (callsign_EEEE >> 16) & 0xFF;
            report[report_len++] = (callsign_EEEE >> 8) & 0xFF;
            report[report_len++] = callsign_EEEE & 0xFF;

            // D SSID Path Code and Data Type Code
            report[report_len++] = SSID * 16 + PATH_CODE * 4 + DATA_TYPE_CODE_GEOLOCATION;

            // / Symbol Table Identifier
            report[report_len++] = SYMBOL_TABLE_IDENTIFIER;

            // XXXX Base91 Longitude
            // YYYY Base91 Latitude
            uint32_t aprs_lat = 900000000 - latitude;
            aprs_lat = aprs_lat / 26 - aprs_lat / 2710 + aprs_lat / 15384615;
            uint32_t aprs_lon = 900000000 + longitude / 2;
            aprs_lon = aprs_lon / 26 - aprs_lon / 2710 + aprs_lon / 15384615;

            char tmp_base91[5];
            ax25_base91enc(tmp_base91, 4, aprs_lat);
            for (int i=0; i<4; i++) {
                report[report_len++] = tmp_base91[i];
            }
            ax25_base91enc(tmp_base91, 4, aprs_lon);
            for (int i=0; i<4; i++) {
                report[report_len++] = tmp_base91[i];
            }

            // $ Symbol Code
            report[report_len++] = SYMBOL_CODE_BICYCLE;

            // cs Course and Speed
            ax25_base91enc(tmp_base91, 1, heading * 10000 / 4);
            report[report_len++] = tmp_base91[0];

            ax25_base91enc(tmp_base91, 1, (uint32_t)(log1p(speed_kn) / 0.07696));
            report[report_len++] = tmp_base91[0];

            Serial.print(F("TX: "));
            // This is the old custom report I used before adopting aprs434.github.io,
            // it is just callsign SPACE locator sent as ASCII
            Serial.print(CALLSIGN);
            Serial.print(F(" "));
            Serial.println(locator);

            // Debug print
            Serial.println(F("Bytes: "));
            for (int i = 0; i < report_len; i++) {
                Serial.print((uint32_t)report[i]);
            }
            Serial.println(F(""));

            int state = radio.transmit(report, report_len);
            handle_radio_error(state);

            digitalWrite(LED_BUILTIN, LOW);
        }

        if (now - lastTextReport > TEXT_TX_INTERVAL) {
            lastTextReport = now;
            digitalWrite(LED_BUILTIN, HIGH);

            // Encode Compressed Status Report Frame according to aprs434.github.io
            const uint32_t callsign_EEEE = encodeCallsign(CALLSIGN);

            report_len = 0;
            // Callsign encoded as CCCC
            report[report_len++] = (callsign_EEEE >> 24) & 0xFF;
            report[report_len++] = (callsign_EEEE >> 16) & 0xFF;
            report[report_len++] = (callsign_EEEE >> 8) & 0xFF;
            report[report_len++] = callsign_EEEE & 0xFF;

            // D SSID Path Code and Data Type Code
            report[report_len++] = SSID * 16 + PATH_CODE * 4 + DATA_TYPE_CODE_STATUS_REPORT;

            // t(t...) text
            const auto bignum = encodetttt(TEXT_REPORT);
            for (int i = 0; i <= 512; i += 8) {
                auto v = static_cast<uint8_t>((bignum >> (512-i)) & 0xFF);
                if (v) {
                    report[report_len++] = v;
                }
            }

            Serial.print(F("TX length "));
            Serial.print(report_len);
            Serial.print(F(" bytes: "));
            Serial.println(TEXT_REPORT);

            int state = radio.transmit(report, report_len);
            handle_radio_error(state);

            digitalWrite(LED_BUILTIN, LOW);
        }
    }
}