// Licence: MIT
// This file is a bit of a mess, because first it contained aprs434 compressed
// code running on STM32H7, and now it contains traditional APRS-over-LoRa for STM32F1
#include <Arduino.h>

#if defined(SEMIHOSTING)
#include <SemihostingStream.h>
#endif

#include <RadioLib.h>
#include <Wire.h>
#include <TinyGPS++.h>
#include <SD.h>
#include "compression.h"
#include "utils.h"

// Pin mapping: outputs
constexpr int LED_STATUSn = PB8;
constexpr int LED_TXn = PB9;
constexpr int EN_PA = PB3;
constexpr int EN_RX = PB4;
constexpr int LORA_RESET = PB11;

// Pin mapping: inputs
constexpr int BTN1n = PC13;
constexpr int BTN2n = PC14;
constexpr int BTN3n = PC15;
constexpr int ENCODER_SW_1 = PB12;
constexpr int ENCODER_SW_2 = PB13;
constexpr int ENCODER_SW_4 = PB14;
constexpr int ENCODER_SW_8 = PB15;

constexpr int BAT_MEAS = PA0;

// TODO: read these settings from the SD Card
constexpr char CALLSIGN_SSID_PATH[] = "HB9EGM-7>APZEGM";
constexpr char SYMBOL_TABLE_IDENTIFIER = '/';
constexpr char SYMBOL_CODE_BICYCLE = 'b';
constexpr char SYMBOL_CODE_FOOT = '[';
constexpr long REPORT_TX_INTERVAL = 181000;

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


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

// LoRa module has the following connections:
// NSS pin:   PA4
// DIO0 pin:  PB10
// RESET pin: PB11
RFM96 radio = new Module(PA4, PB10, PB11);

HardwareSerial serialGNSS(PA10, PA9);
TinyGPSPlus    gps;

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

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

#if defined(SEMIHOSTING)
SemihostingStream sh;
#endif

static char letterize(int x) {
    return (char) x + 65;
}

void setup() {
    Wire.begin();
    SerialUSB.begin();
    pinMode(LED_STATUSn, OUTPUT);
    pinMode(LED_TXn, OUTPUT);
    digitalWrite(LED_TXn, HIGH);

    digitalWrite(EN_PA, LOW);
    pinMode(EN_PA, OUTPUT);
    digitalWrite(EN_RX, LOW);
    pinMode(EN_RX, OUTPUT);

    pinMode(ENCODER_SW_1, INPUT_PULLUP);
    pinMode(ENCODER_SW_2, INPUT_PULLUP);
    pinMode(ENCODER_SW_4, INPUT_PULLUP);
    pinMode(ENCODER_SW_8, INPUT_PULLUP);
    pinMode(BTN1n, INPUT);

#if 0
    pinMode(SD_CS, OUTPUT);

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

    serialGNSS.begin(9600);

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

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

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

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

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

    if (radio.setOutputPower(20) == RADIOLIB_ERR_INVALID_OUTPUT_POWER) {
#if defined(SEMIHOSTING)
        sh.println(F("Selected output power is invalid for this module!"));
#endif
        while (true);
    }

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

    // radio.setRfSwitchPins(4, 5);

    digitalWrite(LED_STATUSn, HIGH);
}

static void handle_radio_error(int state)
{
#if defined(SEMIHOSTING)
    if (state == RADIOLIB_ERR_NONE) {
        sh.print(F(" RFM OK Datarate: "));
        sh.print(radio.getDataRate());
        sh.println(F(" bps"));
    }
    else if (state == RADIOLIB_ERR_PACKET_TOO_LONG) {
        sh.println(F(" too long!"));
    }
    else if (state == RADIOLIB_ERR_TX_TIMEOUT) {
        sh.println(F(" timeout!"));

    }
    else {
        sh.print(F(" failed, code "));
        sh.println(state);
    }
#endif
}

void append_position_report()
{
    float Tlat, Tlon;
    float Tspeed=0, Tcourse=0;
    Tlat    = gps.location.lat();
    Tlon    = gps.location.lng();
    Tcourse = gps.course.deg();
    Tspeed  = gps.speed.knots();

    uint32_t aprs_lat, aprs_lon;
    aprs_lat = 900000000 - Tlat * 10000000;
    aprs_lat = aprs_lat / 26 - aprs_lat / 2710 + aprs_lat / 15384615;
    aprs_lon = 900000000 + Tlon * 10000000 / 2;
    aprs_lon = aprs_lon / 26 - aprs_lon / 2710 + aprs_lon / 15384615;

    char helper_base91[] = {"0000\0"};
    int i;
    ax25_base91enc(helper_base91, 4, aprs_lat);
    for (i=0; i<4; i++) {
        report[report_len++] = helper_base91[i];
    }
    ax25_base91enc(helper_base91, 4, aprs_lon);
    for (i=0; i<4; i++) {
        report[report_len++] = helper_base91[i];
    }

    report[report_len++] = SYMBOL_CODE_FOOT;

    // Do not encode altitude, only course and speed
    ax25_base91enc(helper_base91, 1, (uint32_t)Tcourse/4 );
    report[report_len++] = helper_base91[0];
    ax25_base91enc(helper_base91, 1, (uint32_t)(log1p(Tspeed)/0.07696));
    report[report_len++] = helper_base91[0];
    report[report_len++] = 0x47;
}

static void init_report()
{
    report_len = 0;

    report[report_len++] = '<';
    report[report_len++] = 0xFF;
    report[report_len++] = 0x01;

    const char* p = CALLSIGN_SSID_PATH;
    while (*p) {
        report[report_len++] = *(p++);
    }
}

static int read_encoder()
{
    return 15 - (digitalRead(ENCODER_SW_1) +
        2 * digitalRead(ENCODER_SW_2) +
        4 * digitalRead(ENCODER_SW_4) +
        8 * digitalRead(ENCODER_SW_8));
}

void loop()
{
    while (serialGNSS.available() > 0) {
        gps.encode(serialGNSS.read());
    }

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

        const int encoder_value = read_encoder();
        const bool btn1 = digitalRead(BTN1n) == 0;
        digitalWrite(LED_TXn, not btn1);

        if (encoder_value == 1) {
            digitalWrite(EN_RX, HIGH);
            digitalWrite(EN_PA, LOW);
        }
        else if (encoder_value == 2) {
            digitalWrite(EN_RX, LOW);
            digitalWrite(EN_PA, HIGH);
        }
        else {
            digitalWrite(EN_RX, LOW);
            digitalWrite(EN_PA, LOW);
        }

        if (SerialUSB) {
            const long latitude = gps.location.lat();
            SerialUSB.print(F("Lat "));
            SerialUSB.print(latitude);

            const long longitude = gps.location.lng();
            SerialUSB.print(F(" Lon "));
            SerialUSB.print(longitude);

            const byte SIV = gps.satellites.value();
            SerialUSB.print(F(" SIV: "));
            SerialUSB.print(SIV);

            SerialUSB.print(F(" Enc: "));
            SerialUSB.print(encoder_value);

            SerialUSB.print(F(" BTN: "));
            SerialUSB.println((int)btn1);
        }
    }
}

void oldloop()
{
    while (serialGNSS.available() > 0) {
        gps.encode(serialGNSS.read());
    }

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

#if 0
        const long latitude = gps.location.lat();
        sh.print(F("Lat "));
        sh.print(latitude);

        const long longitude = gps.location.lng();
        sh.print(F(" Lon "));
        sh.print(longitude);

        const byte SIV = gps.satellites.value();
        sh.print(F(" SIV: "));
        sh.println(SIV);
#endif

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

            digitalWrite(LED_TXn, LOW);

            init_report();
            report[report_len++] = ':';
            report[report_len++] = '!';
            report[report_len++] = SYMBOL_TABLE_IDENTIFIER;
            append_position_report();
            int state = radio.transmit(report, report_len);
            handle_radio_error(state);

            digitalWrite(LED_TXn, HIGH);
        }
    }

    if (now - lastTextReport > TEXT_TX_INTERVAL) {
        lastTextReport = now;
        digitalWrite(LED_TXn, LOW);

        init_report();
        report[report_len++] = ':';
        report[report_len++] = '>';
        const char* p = TEXT_REPORT;
        while (*p) {
            report[report_len++] = *(p++);
        }

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

        digitalWrite(LED_TXn, HIGH);
    }
}

#if 0
// Date Type Codes defined in aprs434.github.io
constexpr int DATA_TYPE_CODE_GEOLOCATION = 0;
constexpr int DATA_TYPE_CODE_STATUS_REPORT = 1;
constexpr char CALLSIGN[] = "HB9EGM";
constexpr int SSID = 7;
constexpr int PATH_CODE = 2; // metropolitan mobile

void loop()
{
    while (serialGNSS.available() > 0) {
        gps.encode(serialGNSS.read());
    }

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

        // longitude and latitude are in degrees*1e7
        const long latitude = gps.location.lat();
        sh.print(F("Lat: "));
        sh.print(latitude);

        const long longitude = gps.location.lng();
        sh.print(F(" Long: "));
        sh.print(longitude);

        const long altitude = gps.altitude.feet();

        const int speed = gps.speed.kmph();
        sh.print(F(" Speed: "));
        sh.print(speed);
        sh.print(F(" (mm/s)"));
        // 1m/s = 900/463 knots
        const double speed_kn  = gps.speed.knots();

        const long heading = gps.course.deg();
        sh.print(F(" Heading: "));
        sh.print(heading);
        sh.print(F(" (degrees * 10^-5)"));

        const byte SIV = gps.satellites.value();
        sh.print(F(" SIV: "));
        sh.print(SIV);

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

        sh.println();

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

            digitalWrite(LED_STATUSn, 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_FOOT;

            // 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];

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

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

            digitalWrite(LED_STATUSn, LOW);
        }

        if (now - lastTextReport > TEXT_TX_INTERVAL) {
            lastTextReport = now;
            digitalWrite(LED_STATUSn, 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;
                }
            }

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

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

            digitalWrite(LED_STATUSn, LOW);
        }
    }
}
#endif