/*********************************************************************************
 *
 *  lora_aprs_node_pico is a LoRa APRS KISS modem with additional PE1RXF telemetry
 *  capabilities. It runs on a Raspberry Pi Pico.
 *
 * (C)2023 M.T. Konstapel https://meezenest.nl/mees
 *
 * This file is part of lora_aprs_node_pico.
 *
 *    lora_aprs_node_pico is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    lora_aprs_node_pico is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with lora_aprs_node_pico. If not, see <https://www.gnu.org/licenses/>.
 *
 **********************************************************************************/
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <stdarg.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "hardware/flash.h"
#include "LoRa-RP2040.h"
#include "Config.h"
#include "kiss.h"
#include "hardware/claim.h"
#include "i2c_sensor.h"

KissClass Kiss;
struct ax25_frame AX25Frame;        //defined in kiss.h
struct aprs_frame AprsFrame;        //defined in kiss.h
struct kiss_tx_frame KissTxFrame;   //defined in kiss.h

bool NO_I2C_AVAILABLE=false;

bool startRadio();
void getPacketData(int packetLength);
int compare_strings(uint8_t a[], uint8_t b[]);
bool is_message_for_me (uint8_t data[], uint8_t mycall[]);

/* declaration for receive functions */
uint16_t decode_packet ();

/* declaration for transmit functions */
void ComposeAprsFrame(uint8_t payload[]);
void ComposeAprsFrameFromKiss();
bool TransmitRequest = false;
void transmit();

const uint PowerSupply24VControl = 6;
const uint PowerSupply12VControl = 5;
const uint PowerSupply5VControl  = 4;
const uint RelayOffControl       = 2;
const uint RelayOnControl        = 3;

// We're going to use a region 512k from the start of flash as non volatile storage for our settings.
// We can access this at XIP_BASE + 512k.
#define FLASH_TARGET_OFFSET (512 * 1024)

const uint8_t *flash_target_contents = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET);

void log_out(const char *fmt, ...)
{
        char buf[256];
        va_list args;
        
        if (Status.KissMode == OFF) {
            va_start(args, fmt);

            vsnprintf(buf, sizeof buf, fmt, args);

            va_end( args);
            
            printf("%s", buf);
        }

}

/*
 * Saves settings in struct AprsSettings to FLASH memory
 * Struct AprsSettings should be exactly a multiple of FLASH_PAGE_SIZE in size.
 * 
 * Returns: 0 when successfull
 *          1 when error
 */

uint8_t SaveSettingsToFlash(void)
{
    uint32_t ints = save_and_disable_interrupts();
        
    // First we erase the FLASH sector we need. After that we can store new values.
    // Note that a whole number of sectors must be erased at a time.
    // Sector size is 4kB, so this is way bigger than the 256 bytes PAGE_SIZE for storing the settings.
    // We can therefore store up to 16 blocks of 256 bytes per sector
    log_out("Erasing FLASH region...");
    flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE);
    log_out("done\n");
    log_out("Writing settings to FLASH...");    
    flash_range_program(FLASH_TARGET_OFFSET, (uint8_t*)&AprsSettings, sizeof(AprsSettings) );
    log_out("done\n");
    restore_interrupts (ints);

    return(0);
}

void ShowSettings(void)
{
    uint64_t tmp;

    log_out("LoRa APRS KISS TNC with support for PE1RXF telemetry.\n");
    log_out(" Firmware version : %s\n",AprsSettings.FirmwareVersion);
    log_out(" Size of struct   : %u.\n\n", sizeof(AprsSettings));
    
    log_out("APRS settings\n");
    log_out("  My call         : %s\n", AprsSettings.MyCall);
    log_out("  Server call     : %s\n", AprsSettings.ServerCall);
    log_out("  Destination     : %s\n", AprsSettings.Destination);
    log_out("  Path 1          : %s\n", AprsSettings.Path1);
    log_out("  Path 2          : %s\n\n", AprsSettings.Path2);
    
    log_out("LoRa settings\n");
    log_out("  Frequency       : %u\n", AprsSettings.loraFrequency);    
    log_out("  SpreadingFactor : %i\n", AprsSettings.loraSpreadingFactor);    
    log_out("  Preamble        : %i\n", AprsSettings.loraPreamble);    
    log_out("  CodingRate      : %i\n", AprsSettings.loraCodingRate);    
    log_out("  TxPower         : %i\n", AprsSettings.loraTxPower);    
    log_out("  PaSelect        : %i\n", AprsSettings.loraPaSelect);    
    log_out("  Bandwidth       : %u\n\n", AprsSettings.loraBandwidth);

    log_out("Telemetry settings\n");
    if (AprsSettings.TelemetryPeriod == 0xFFFFFFFFFFFFFFFF)
        tmp=0;
    else
        tmp = AprsSettings.TelemetryPeriod/60000000;

    log_out("  Period          : %u\n", (uint32_t)tmp);
}

uint8_t ReadSettingsFromFlash(void)
{
    // If byte zero of flash contains 0x5A we assume the data to be valid, otherwise we fill the flash with default values.
    if (flash_target_contents[0] != 0x5A)
    {
        log_out( "No valid data found in FLASH memory. Using default values.\n" );
        memset(AprsSettings.FillerData, 0, sizeof(AprsSettings.FillerData));
        SaveSettingsToFlash();
        
    } else {
        // Read settings stored in flash memory
        log_out("Found valid settings in FLASH memory.\n");
    }
    memcpy((uint8_t*)&AprsSettings, flash_target_contents, sizeof(AprsSettings));
    ShowSettings();
}

void setup(void)
{
    /* Among others, this initializes the USB-serial port at 115200bps 8N1 */
   stdio_init_all();

   // Buffers
   memset(rxBuffer, 0, sizeof(rxBuffer));
   memset(txBuffer, 0, sizeof(txBuffer));
   Status.TelemetryString[0] = '0';
   Status.TelemetryString[1] = 0;
   
   gpio_init(PowerSupply24VControl);
   gpio_init(PowerSupply12VControl);
   gpio_init(PowerSupply5VControl);
   gpio_init(RelayOffControl);
   gpio_init(RelayOnControl);
   
   gpio_set_dir(PowerSupply24VControl, GPIO_OUT);
   gpio_set_dir(PowerSupply12VControl, GPIO_OUT);
   gpio_set_dir(PowerSupply5VControl, GPIO_OUT);
   gpio_set_dir(RelayOffControl, GPIO_OUT);
   gpio_set_dir(RelayOnControl, GPIO_OUT);
   
   gpio_put(PowerSupply24VControl, 0);
   Status.PowerSupply24V = OFF;
   gpio_put(PowerSupply12VControl, 0);
   Status.PowerSupply12V = OFF;
   gpio_put(PowerSupply5VControl, 1);
   Status.PowerSupply5V = OFF;
   
   gpio_put(RelayOffControl, 1);
   sleep_ms(250);
   gpio_put(RelayOffControl, 0);
   gpio_put(RelayOnControl, 0);
   Status.ControlRelay = OFF;

   // Without a delay, the serial port does not work...
   sleep_ms(5000);

   log_out("Trying to connect to AM2315 I2C sensor...");
   // Initialize I2C
   if (initAM2315()) {
       NO_I2C_AVAILABLE = true;
       log_out("not found.\n");
       log_out("Trying to continue without I2C\n");
   } else {
       NO_I2C_AVAILABLE = false;
       log_out("connected.\n");
   }

   ReadSettingsFromFlash();
   
   startRadio();
   
}

void print_help(void)
{
    log_out("Unknown command.\n\n");
    log_out("kiss\n");
    log_out("   Enter KISS mode.\n");
    log_out("save\n");
    log_out("   Save settings to flash.\n");
    log_out("read <flash/ram>\n");
    log_out("   Read settings from FLASH or RAM.\n");
    log_out("mycall/servercall/destination/path1/path2\n");
    log_out("   APRS settings.\n");
    log_out("freq/spread/pre/rate/power/pa/band.\n");
    log_out("   LoRa settings.\n");
    log_out("period.\n");
    log_out("   Telemetry settings.\n\n");

}

/*
 * Reads a string and converts it to its 32 bit value if it contains a number,
 * else it returns 0xFFFFFFFF
 */
uint32_t ConvertStringToValue(char string[])
{
    uint16_t position = 0;
    uint32_t value = 0;

    // Extract value from string (if present)    
    while( string[position] != 0 )
    {
        // Is character a number?
        if (string[position] >= 48 && string[position] <= 57) {
            value = 10*value + string[position]-48;
        } else {
            return 0xFFFFFFFF;
        }
        position++;
        
    }
    return value;
}

void ProcessSerialInput(char string[])
{
    uint8_t cnt;
    uint8_t position=0;
    uint32_t tmp = 0;
    
    char command[100];
    char parameter[100];
    
    //log_out("You wrote - %s (%u).\n", string, strlen(string));
    
    // Command cannot be any shorter than 3 characters
    if (strlen(string) > 2) {
        
        // Extract command (part before space)
        cnt = 0;    
        while( string[position] != 0 )
        {
            command[cnt++] = string[position];
            if ( string[position] == ' ' ) {
                command[cnt-1] = 0;  // terminate command string
                position++;
                break;
            }
            position++;
        }

        // Extract parameter (part after space)
        cnt = 0;    
        while( string[position] != 0 )
        {
            parameter[cnt++] = string[position++];
        }
        parameter[cnt] = 0; //terminate string

        //log_out("Command - %s.\n", command);
        //log_out("Parameter - %s.\n", parameter);
        
        // Read settings
        if (strcmp(command, "read") == 0) {
            if (strcmp(parameter, "flash") == 0) {
                ReadSettingsFromFlash();
            }
            else if (strcmp(parameter, "ram") == 0) {
                ShowSettings();
            }
        }
        // Save settings to FLASH
        else if (strcmp(command, "save") == 0)
            SaveSettingsToFlash();
        
        // Enter KISS mode
        else if (strcmp(command, "kiss") == 0) {
            log_out("Entering KISS mode.\n");
            log_out("You can exit KISS mode via KISS command <0xC0 0xFF 0xC0>\n");
            // disable CRLF convertion, so we can write raw data to the USB port in KISS mode
            stdio_set_translate_crlf(&stdio_usb, false);
            //fwrite(buf, 1, sizeof(buf), stdout);  // seems not to work
            //putchar(49);

            Status.KissMode = ON;
        }

        // Set mycall (cannot be longer than 9 characters)
        else if (strcmp(command, "mycall") == 0) {
            if (sizeof(AprsSettings.MyCall) > strlen(parameter)) {
                position = 0;
                while( parameter[position] != 0 )
                {
                    AprsSettings.MyCall[position] = parameter[position];
                    position++;
                }
                AprsSettings.MyCall[position] = 0;
                log_out("MyCall set to %s.\n", AprsSettings.MyCall);
            }
        }
        // Set servercall (cannot be longer than 9 characters)
        else if (strcmp(command, "servercall") == 0) {
            if (sizeof(AprsSettings.ServerCall) > strlen(parameter)) {
                position = 0;
                while( parameter[position] != 0 )
                {
                    AprsSettings.ServerCall[position] = parameter[position];
                    position++;
                }
                AprsSettings.ServerCall[position] = 0;
                log_out("ServerCall set to %s.\n", AprsSettings.ServerCall);
            }
        }
        // Set path 1 (cannot be longer than 9 characters)
        else if (strcmp(command, "path1") == 0) {
            // Set path to nothing
            if (parameter[0] == '0') {
                AprsSettings.Path1[0] = 0;
                log_out("Path1 cleared.\n");
            }
            else if (sizeof(AprsSettings.Path1) > strlen(parameter)) {
                position = 0;
                while( parameter[position] != 0 )
                {
                    AprsSettings.Path1[position] = parameter[position];
                    position++;
                }
                AprsSettings.Path1[position] = 0;
                log_out("Path1 set to %s.\n", AprsSettings.Path1);
            }
        }
        // Set path 2 (cannot be longer than 9 characters)
        else if (strcmp(command, "path2") == 0) {
            // Set path to nothing
            if (parameter[0] == '0') {
                AprsSettings.Path2[0] = 0;
                log_out("Path2 cleared.\n");
            }
            else if (sizeof(AprsSettings.Path2) > strlen(parameter)) {
                position = 0;
                while( parameter[position] != 0 )
                {
                    AprsSettings.Path2[position] = parameter[position];
                    position++;
                }
                AprsSettings.Path2[position] = 0;
                log_out("Path2 set to %s.\n", AprsSettings.Path2);
            }
        }
        // Set destination (cannot be longer than 9 characters)
        else if (strcmp(command, "dest") == 0) {
            if (sizeof(AprsSettings.Destination) > strlen(parameter)) {
                position = 0;
                while( parameter[position] != 0 )
                {
                    AprsSettings.Destination[position] = parameter[position];
                    position++;
                }
                AprsSettings.Destination[position] = 0;
                log_out("Destination set to %s.\n", AprsSettings.Destination);
            }
        }
        // Set lora frequency (limited between 420MHz and 450MHz)
        else if (strcmp(command, "freq") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 420000000 || tmp > 450000000)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraFrequency = tmp;
                log_out("LoRa frequency set to %u.\n", AprsSettings.loraFrequency);
            }
        }
        // Set lora spreading factor (can be between 6 and 12)
        else if (strcmp(command, "spread") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 6 || tmp > 12)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraSpreadingFactor = (uint16_t)tmp;
                log_out("LoRa spreading factor set to %u.\n", AprsSettings.loraSpreadingFactor);
            }
        }
        // Set lora preamble (can be between 6 and 0xFFFF)
        else if (strcmp(command, "pre") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 6 || tmp > 0xFFFF)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraPreamble = (uint16_t)tmp;
                log_out("LoRa preamble set to %u.\n", AprsSettings.loraPreamble);
            }
        }
        // Set lora coding rate (can be between 5 and 8)
        else if (strcmp(command, "rate") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 5 || tmp > 8)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraCodingRate = (uint16_t)tmp;
                log_out("LoRa coding rate set to %u.\n", AprsSettings.loraCodingRate);
            }
        }
        // Set lora tx power (can be between 2 and 17)
        else if (strcmp(command, "power") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 2 || tmp > 17)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraTxPower = (uint16_t)tmp;
                log_out("LoRa tx power set to %u.\n", AprsSettings.loraTxPower);
            }
        }
        // Set lora bandwidth (can be between 7800 and 500000)
        else if (strcmp(command, "band") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp < 7800 || tmp > 5000000)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraBandwidth = tmp;
                log_out("LoRa bandwidth set to %u.\n", AprsSettings.loraBandwidth);
            }
        }
        // Set lora pa to +20dBm (can be either 0 or 1)
        else if (strcmp(command, "pa") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp != 0 && tmp != 1)
                log_out("ERROR: that is not a valid value.\n");
            else {
                AprsSettings.loraPaSelect = (uint16_t)tmp;
                log_out("LoRa PA set to %u.\n", AprsSettings.loraPaSelect);
            }
        }
        // Set telemetry interval (can be between 0 and 6)
        else if (strcmp(command, "period") == 0) {
            tmp = ConvertStringToValue(parameter);
            if (tmp == 0xFFFFFFFF || tmp > 6)
                log_out("ERROR: that is not a valid value.\n");
            else {
                if (tmp==0) {
                    AprsSettings.TelemetryPeriod = 0xFFFFFFFFFFFFFFFF;
                } else {
                    AprsSettings.TelemetryPeriod = tmp * 6e+8;
                }
                log_out("Telemtry period set to %u minutes.\n", tmp*10);
            }
        }
        // Restart radio
        else if (strcmp(command, "restart") == 0)
            if (strcmp(parameter, "lora") == 0) {
                log_out("Re-");
                startRadio();
            }

        else {
            print_help();
        }
    }
    else
    {
        print_help();
    }
}
void ReadUSBSerial(void)
{
    static char strg[512];
    int chr;
    static int lp = 0;
    uint16_t tmp;
    
    if (Status.KissMode == OFF) {
        // Read serial port (USB) - non-blocking!
        chr = getchar_timeout_us(0);
        while(chr != PICO_ERROR_TIMEOUT)
        {
            log_out("%c", chr);
            
            strg[lp++] = chr;
            if(chr == CR || lp == (sizeof(strg) - 1))
            {
                strg[lp-1] = 0;	//terminate string by overwriting <CR> with NULL
                //log_out("You wrote - %s\n", strg);
                lp = 0;		//reset string buffer pointer
                log_out("\n");
                
                ProcessSerialInput(strg);
                
                break;
            }

            chr = getchar_timeout_us(0);
        }
    }
    // We are in KISS mode
    else
    {
        // Read serial port (USB) - non-blocking!
        chr = getchar_timeout_us(0);
        while(chr != PICO_ERROR_TIMEOUT)
        {
            strg[lp++] = chr;
            // Receive buffer buffer full
            if( lp == (sizeof(strg) - 1)) {
                lp=0;
            }
            // Received FEND (=begin or end frame)
            if(chr == FEND)
            {
                // Valid KISS frame received
                if (strg[0] == FEND && lp > 1)
                {
                    tmp = Kiss.DecodeFrame((uint8_t *) strg, &KissTxFrame);
                    if ( tmp == 2)
                    {
                        //exit KISS MODE
                        stdio_set_translate_crlf(&stdio_usb, true);
                        Status.KissMode = OFF; 
                    }
                    // Valid KISS data frame, so lets send it
                    else if (tmp == 0)
                    {
                        ComposeAprsFrameFromKiss();
                    }
                    lp = 0;		//reset string buffer pointer
                }
                // We received a FEND byte,so we are probably between two KISS frames. Let's assume the latest FEND is the beginning of a new frame
                else
                {
                    strg[0] = chr;
                    lp = 1;         // set string buffer pointer to second position
                }

                break;
            }

            chr = getchar_timeout_us(0);
        }
    }
}

int main() {
    
   uint16_t ServerCommand = 0;
   uint16_t TxDelay = 0;

   uint64_t TelemetryDelay = 0;
   uint16_t TelemetryManualRequest = 0;

   int16_t humidity=0;
   int16_t temperature=0;

   char tmp_string[68];
   uint16_t cnt;

   setup();
    
   while (1) {
      int packetSize = LoRa.parsePacket();
      if (packetSize) {
         // received a packet
         log_out("Received packet (RSSI = %idBm)\n",LoRa.packetRssi());

         getPacketData(packetSize);
         
         // Check APRS header: must be 0x3C 0xFF 0x01
         if (rxBuffer[0] == 0x3C && rxBuffer[1] == 0xFF && rxBuffer[2] == 0x01 ) {
             // Shift array three places to left (= remove APRS header)
             for (int cnt = 3; cnt < packetSize; cnt++) {
                 rxBuffer[cnt-3] = rxBuffer[cnt];
             }
             rxBuffer[packetSize-3] = 0;
             log_out("%s\n", rxBuffer);
             ServerCommand = decode_packet();
         } else {
             log_out("ERROR: No or corrupted APRS frame.\n");
         }
      }
      
      if (ServerCommand) {
          
          switch(ServerCommand) {
              
              case 1 :
                  ComposeAprsFrame(AprsSettings.FirmwareVersion);
                  break;

              // Send telemetry data
              case 3:
                  // Force a telemtry transmission
                  TelemetryManualRequest = 1;
                  break;
              // Send telemetry data description fields
              case 4:
                  ComposeAprsFrame(Status.TelemetryDescriptionString);
                  break;
              case 5:
                  ComposeAprsFrame(Status.TelemetryUnitString);
                  break;
              // Send status of output pins
              case 6 :
                  if (Status.PowerSupply24V == ON)
                      Status.StatusString[4] = '1';
                  else
                      Status.StatusString[4] = '0';

                  if (Status.PowerSupply12V == ON)
                      Status.StatusString[3] = '1';
                  else
                      Status.StatusString[3] = '0';
                  
                  if (Status.PowerSupply5V == ON)
                      Status.StatusString[2] = '1';
                  else
                      Status.StatusString[2] = '0';
                  
                  if (Status.ControlRelay == ON)
                      Status.StatusString[1] = '1';
                  else
                      Status.StatusString[1] = '0';
                  
                  ComposeAprsFrame(Status.StatusString);
                  
                  break;
                  
              // Send description digital outputs
              case 7 :
                  ComposeAprsFrame(Status.DescriptionString);
                  break;

              // Time between telemetry transmissions in microseconds. Data is volatile! eg. not stored in FLASH, default after power on is 10 minutes
              // Interval telemetry data: never send data
              case 20:
                  AprsSettings.TelemetryPeriod = 0xFFFFFFFFFFFFFFFF;
                  break;
              // Interval telemetry data: 10 minutes
              case 21:
                  AprsSettings.TelemetryPeriod = 6e+8;
                  break;
              // Interval telemetry data: 20 minutes
              case 22:
                  AprsSettings.TelemetryPeriod = 12e+8;
                  break;
              // Interval telemetry data: 30 minutes
              case 23:
                  AprsSettings.TelemetryPeriod = 18e+8;
                  break;
              // Interval telemetry data: 40 minutes
              case 24:
                  AprsSettings.TelemetryPeriod = 24e+8;
                  break;
              // Interval telemetry data: 50 minutes
              case 25:
                  AprsSettings.TelemetryPeriod = 30e+8;
                  break;
              // Interval telemetry data: 60 minutes
              case 26:
                  AprsSettings.TelemetryPeriod = 36e+8;
                  break;

              // Switch off 24V power supply
              case 30 :
                  gpio_put(PowerSupply24VControl, 0);
                  Status.PowerSupply24V = OFF;
                  break;
              // Switch on 24V power supply
              case 31 :
                  gpio_put(PowerSupply24VControl, 1);
                  Status.PowerSupply24V = ON;
                  break;
              // Switch off 12V power supply
              case 32 :
                  gpio_put(PowerSupply12VControl, 0);
                  Status.PowerSupply12V = OFF;
                  break;
              // Switch on 12V power supply
              case 33 :
                  gpio_put(PowerSupply12VControl, 1);
                  Status.PowerSupply12V = ON;
                  break;
              // Switch off 5V power supply
              case 34 :
                  gpio_put(PowerSupply5VControl, 1);
                  Status.PowerSupply5V = OFF;
                  break;
              // Switch on 5V power supply
              case 35 :
                  gpio_put(PowerSupply5VControl, 0);
                  Status.PowerSupply5V = ON;
                  break;
              // Switch off relay
              case 36 :
                  gpio_put(RelayOffControl, 1);
                  sleep_ms(250);
                  gpio_put(RelayOffControl, 0);
                  Status.ControlRelay = OFF;
                  break;
              // Switch on 24V relay
              case 37 :
                  gpio_put(RelayOnControl, 1);
                  sleep_ms(250);
                  gpio_put(RelayOnControl, 0);
                  Status.ControlRelay = ON;
                  break;

              default :
                  break;
          }
          
          ServerCommand = 0;
      }

      /* Periodically send telemetry data */
      if ( (time_us_64() - TelemetryDelay > AprsSettings.TelemetryPeriod) || TelemetryManualRequest ){

        // Reset appropriate flag
        if (TelemetryManualRequest)
            TelemetryManualRequest = 0;
        else
            TelemetryDelay = time_us_64();

        // Read humidity and temperature
        if (!NO_I2C_AVAILABLE)
            ReadAM2315(&humidity, &temperature);

        // Format telemetry string
        sprintf(tmp_string, "%.1f,%.1f", (float)temperature/10, (float)humidity/10);
        // Copy string (including NULL terminator) to final destination (can not be done without temporary string due to char/uint8_t issues)
        cnt=0;
        while ( cnt <= strlen(tmp_string) ) {
            Status.TelemetryString[cnt]=tmp_string[cnt];
            cnt++;
        }

        // Print results
        log_out("Sending telemetry: Temp: %.1f, Hum: %.1f\r\n", (float)temperature/10, (float)humidity/10);

        ComposeAprsFrame(Status.TelemetryString);

      }

      /* A message is ready to be send */
      if (TransmitRequest) {
          
          if ( TxDelay == 0 ) {
            // Generate pseudo random value between 0-1024
            TxDelay = time_us_32()&0x3FF;
          }
          
          /* If TxDelay times out: send message */
          if ( TxDelay-- == 1 ) {
              transmit();
              TransmitRequest = false;
          }
      }

      // Read serial input and process it
      ReadUSBSerial();
  }    
  
  return 0;
}

/*
 * Initializes the LoRa module with the parameters set in config.h
 */
bool startRadio()
{
  // override the default CS, reset, and IRQ pins (optional)
  // LoRa.setPins(7, 6, 1); // set CS, reset, IRQ pin
  log_out("Starting LoRa radio");
  if (!LoRa.begin(AprsSettings.loraFrequency)) {
    log_out("  [ FAILED ]\n");
    while(1);
  }
  else {
    LoRa.setPreambleLength(AprsSettings.loraPreamble);
    LoRa.setSignalBandwidth(AprsSettings.loraBandwidth);
    LoRa.setTxPower(AprsSettings.loraTxPower,AprsSettings.loraPaSelect);
    LoRa.setSpreadingFactor(AprsSettings.loraSpreadingFactor);
    LoRa.setCodingRate4(AprsSettings.loraCodingRate);
    LoRa.enableCrc();
    log_out("  [ DONE ]\n");
  }
}

void getPacketData(int packetLength)
{
  int position = 0;
  
  while (packetLength--) {
    rxBuffer[position++] = LoRa.read();
  }
  rxBuffer[position] = 0;
}

/* Decode LoRa APRS frame: extract source call, digipeater path and data field. At the same time, check for data corruption
 * 
 * If frame is a message from the server it returns the command from this server 
 * 
 * The resulting fields are stored in struct AprsFrame and given to the kiss-routines for further processing (when in kiss-mode)
 */
uint16_t decode_packet ()
{
    int position = 0;
    int cnt = 0;
    
    memset(AprsFrame.source_address, 0, sizeof(AprsFrame.source_address));
    memset(AprsFrame.digi_path, 0, sizeof(AprsFrame.digi_path));
    memset(AprsFrame.data_field, 0, sizeof(AprsFrame.data_field));
    memset(AprsFrame.message, 0, sizeof(AprsFrame.message));
    memset(AprsFrame.digis, 0, sizeof(AprsFrame.digis));
    memset(AprsFrame.acknowledge_number, 0, sizeof(AprsFrame.acknowledge_number));
    AprsFrame.acknowledge_request = false;
    AprsFrame.server_command = 0;
    AprsFrame.number_of_digipeaters = 0;
    AprsFrame.valid_apsr_data = false;
   
    // Extract from address
    cnt = 0;    
    while( rxBuffer[position] != 0 )
    {
        AprsFrame.source_address[cnt++] = rxBuffer[position];
        if ( rxBuffer[position] == '>' && position < 10 ) {
            AprsFrame.source_address[cnt-1] = 0;
            AprsFrame.valid_apsr_data = true;
                position++;
            break;
        }
        position++;
    }
    
    if (AprsFrame.valid_apsr_data == true)
    {
        // Extract digi path
        AprsFrame.valid_apsr_data = false;
        cnt = 0;
        while( rxBuffer[position] != 0 )
        {
            AprsFrame.digi_path[cnt++] = rxBuffer[position];
            if ( rxBuffer[position] == ':' ) {
                AprsFrame.digi_path[cnt-1] = 0;
                AprsFrame.valid_apsr_data = true;
                    position++;
                break;
            }
            position++;
        }
        
    }

    if (AprsFrame.valid_apsr_data == true)
    {
        // Extract data field
        cnt = 0;
        while( rxBuffer[position] != 0 )
        {
            AprsFrame.data_field[cnt++] = rxBuffer[position];
            position++;
        }
        AprsFrame.data_field[cnt] = 0;        
    }    
    
    if (AprsFrame.valid_apsr_data == true)
    {
        // Extract digis from digi-path
        cnt = 0;
        position = 0;
        AprsFrame.number_of_digipeaters = 0;
        while( AprsFrame.digi_path[position] != 0 )
        {
            AprsFrame.digis[AprsFrame.number_of_digipeaters][cnt++] = AprsFrame.digi_path[position];
            if ( AprsFrame.digi_path[position] == ',' && cnt < 10 ) {
                AprsFrame.digis[AprsFrame.number_of_digipeaters][cnt-1] = 0;
                if (++AprsFrame.number_of_digipeaters > 9) {
                    AprsFrame.valid_apsr_data = false;
                    break;
                }
                cnt = 0;
            }
            position++;
        }
        AprsFrame.digis[AprsFrame.number_of_digipeaters][cnt] = 0;    
    }
    
    if (AprsFrame.valid_apsr_data) {
        
        // Check if packet comes from our server and if so, check if it is a message for us.
        if ( !compare_strings(AprsFrame.source_address, AprsSettings.ServerCall) ) {

            if ( is_message_for_me(AprsFrame.data_field, AprsSettings.MyCall) ) 
            {
               
                // Extract aprs message from data field
                position=11;
                while( AprsFrame.data_field[position] != 0 )
                {
                    AprsFrame.message[position-11] = AprsFrame.data_field[position];
                    position++;
                }
                AprsFrame.message[position-11] = 0; // Terminate string
                
                // Extract command and acknowledge number (if present)
                cnt = 0;
                position = 0;
                while( AprsFrame.message[position] != 0 )
                {
                    if ( AprsFrame.message[position] == '{' ) {
                        
                        AprsFrame.acknowledge_number[cnt++] = 'a';
                        AprsFrame.acknowledge_number[cnt++] = 'c';
                        AprsFrame.acknowledge_number[cnt++] = 'k';
                        AprsFrame.acknowledge_request = true;
                    }
                    // Calculate server command
                    if (!AprsFrame.acknowledge_request) {
                        AprsFrame.server_command = 10*AprsFrame.server_command + AprsFrame.message[position]-48;
                    }
                    
                    position++;
                    if (AprsFrame.acknowledge_request) {
                        AprsFrame.acknowledge_number[cnt++] = AprsFrame.message[position];
                    }
                }
                AprsFrame.acknowledge_number[cnt] = 0;
            }
            
        }
        
        log_out("Source address: %s\nDigipeaters (%u): %s %s %s %s\nData: %s\n", AprsFrame.source_address, AprsFrame.number_of_digipeaters+1, AprsFrame.digis[0], AprsFrame.digis[1], AprsFrame.digis[2], AprsFrame.digis[3], AprsFrame.data_field);
        
        // If in KISS mode the struct AprsFrame is handed over to the KISS encoder
        if (Status.KissMode == ON) {
            Kiss.EncodeFrame(&AprsFrame, &AX25Frame);
            //if (Kiss.DecodeFrame(AX25Frame.encoded_kiss_frame, &KissTxFrame) == 0)
                //ComposeAprsFrameFromKiss();
        }
        
        if (AprsFrame.message[0])
        {
            log_out("Message from server: %s (command %u)\n", AprsFrame.message, AprsFrame.server_command);
            if (AprsFrame.acknowledge_request) {
                ComposeAprsFrame(AprsFrame.acknowledge_number);
                log_out("Acknowledge request: %s\n", AprsFrame.acknowledge_number);
            }
            
        }

    }    
    else
        log_out("Error decoding APRS frame.");
    
    return (AprsFrame.server_command);
}

/* 
 * Checks if aprs datafield contains message and if message is for us
 * 
 * Returns: 0 if datafield contains no message for us
 *          1 if datafield contains a message for us 
 */
bool is_message_for_me (uint8_t data[], uint8_t mycall[])
{
    // A variable to iterate through the strings
    int i=0;

    if (data[0] == ':' && data[10] == ':')
    {
        while( i<9 && mycall[i] != 0 ) {
            if (data[i+1] != mycall[i]) {
                return (0);
            }
            i++;
        }
        return (1);
    }    
    return (0);
}

int compare_strings(uint8_t a[], uint8_t b[])
{
    // A variable to iterate through the strings
    int i = 0;

    while (a[i] == b[i]) 
    {
        // If either of the strings reaches the end
        // we stop the loop
        if (a[i] == '\0' || b[i] == '\0')
            break;
        i++;
    }

    // We check if both the strings have been compared
    // till the end or not
    // If the strings are compared till the end they are equal
    if (a[i] == '\0' && b[i] == '\0')
        return 0;
    else
    {
        if(a[i] == '\0') 
            return -1*(b[i]);
        else if(b[i] == '\0')
            return a[i];
        else
           return (a[i]-b[i]); 
    }
}

void ComposeAprsFrame(uint8_t payload[])
{
    uint16_t BufferPosition = 0;
    uint16_t cnt = 0;
    
    memset(txBuffer, 0, sizeof(txBuffer));
    
    // APRS header
    txBuffer[BufferPosition++] = '<';
    txBuffer[BufferPosition++] = 0xff;
    txBuffer[BufferPosition++] = 0x01;
    
    while ( AprsSettings.MyCall[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = AprsSettings.MyCall[cnt];
        cnt++;
    }
    txBuffer[BufferPosition++] = '>';
    
    cnt=0;
    while ( AprsSettings.Destination[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = AprsSettings.Destination[cnt];
        cnt++;
    }
    
    if ( AprsSettings.Path1[0] != 0)
        txBuffer[BufferPosition++] = ',';
    
    cnt=0;
    while ( AprsSettings.Path1[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = AprsSettings.Path1[cnt];
        cnt++;
    }

    if ( AprsSettings.Path2[0] != 0)
        txBuffer[BufferPosition++] = ',';

    cnt=0;
    while ( AprsSettings.Path2[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = AprsSettings.Path2[cnt];
        cnt++;
    }

    txBuffer[BufferPosition++] = ':';
    txBuffer[BufferPosition++] = ':';
    
    cnt=0;
    while ( AprsSettings.ServerCall[cnt] != 0 )
    {
        txBuffer[BufferPosition++] = AprsSettings.ServerCall[cnt];
        cnt++;
    }

    //Fill with spaces
    while ( cnt<9 )
    {
        txBuffer[BufferPosition++] = ' ';
        cnt++;

    }
    txBuffer[BufferPosition++] = ':';
    
    cnt=0;
    while ( payload[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = payload[cnt];
        cnt++;
    }
    
    // Set variable to indicate a send request
    TransmitRequest = true; 
    log_out("%s\n", txBuffer);
}

void ComposeAprsFrameFromKiss()
{
    uint16_t BufferPosition = 0;
    uint16_t cnt = 0;
    uint8_t digi_cnt=0;
    
    log_out( "Compose APRS from KISS frame");
    
    memset(txBuffer, 0, sizeof(txBuffer));
    
    // APRS header
    txBuffer[BufferPosition++] = '<';
    txBuffer[BufferPosition++] = 0xff;
    txBuffer[BufferPosition++] = 0x01;
    
    while ( KissTxFrame.source_address[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition] = KissTxFrame.source_address[cnt];
        BufferPosition++;
        cnt++;
    }
    txBuffer[BufferPosition++] = '>';
    
    digi_cnt=0;
    while(digi_cnt <= KissTxFrame.number_of_digipeaters) {
        cnt=0;
        
        while ( KissTxFrame.digis[digi_cnt][cnt] != 0 && BufferPosition<MTU )
        {
            txBuffer[BufferPosition++] = KissTxFrame.digis[digi_cnt][cnt];
            cnt++;
        }
        txBuffer[BufferPosition++] = ',';
        digi_cnt++;
    }
    
    // Overwrite last command with colon
    txBuffer[--BufferPosition] = ':';
    
    cnt=0;
    while ( KissTxFrame.data_field[cnt] != 0 && BufferPosition<MTU )
    {
        txBuffer[BufferPosition++] = KissTxFrame.data_field[cnt];
        cnt++;
    }
    
    // Set variable to indicate a send request
    TransmitRequest = true; 
    
    // Ready for next input KISS frame.
    KissTxFrame.valid_data = false;
    
    log_out("%s\n", txBuffer);
}

void transmit() {
  uint16_t position = 0;
  
  LoRa.beginPacket();
  while( txBuffer[position] != 0 )
  {
    LoRa.write(txBuffer[position]);
    position++;
  }
  LoRa.endPacket();

  LoRa.receive();
}