#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"

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[]);
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 256k from the start of flash as non volatile storage for our settings.
// We can access this at XIP_BASE + 256k.
#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)
{
    log_out("LoRa APRS remote switcher with build in KISS TNC.\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", AprsSettings.loraBandwidth); 
}

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));
   
   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;
   
   sleep_ms(5000);
   
   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");

}

void ProcessSerialInput(char string[])
{
    uint8_t cnt;
    uint8_t position=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");
            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);
            }
        }
        else {
            print_help();
        }
    }
    else
    {
        print_help();
    }
}
void ReadUSBSerial(void)
{
    static char strg[100];
    int chr;
    static int lp = 0;
    
    // 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
			
			ProcessSerialInput(strg);
            
			break;
		}

		chr = getchar_timeout_us(0);
	}
}

int main() {
    
    uint16_t ServerCommand = 0;
    uint16_t TxDelay = 0;
  
   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 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);
                  
              // 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;
      }
      
      /* 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;
}

/* Encode 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 
 */
uint16_t decode_packet ()
{
    int position = 0;
    int cnt = 0;
    int number_of_digipeaters = 0;
    char valid_apsr_data = false;
    
    uint8_t aprs_source_address[10];
    uint8_t aprs_digi_path[255];
    uint8_t aprs_data_field[255];
    uint8_t aprs_message[255];
    uint8_t aprs_digis[10][10];
    uint8_t aprs_acknowledge_number[255];
    bool aprs_acknowledge_request = false;
    uint16_t aprs_server_command = 0;
    
    memset(aprs_source_address, 0, sizeof(aprs_source_address));
    memset(aprs_digi_path, 0, sizeof(aprs_digi_path));
    memset(aprs_data_field, 0, sizeof(aprs_data_field));
    memset(aprs_message, 0, sizeof(aprs_message));
    memset(aprs_digis, 0, sizeof(aprs_digis));
    memset(aprs_acknowledge_number, 0, sizeof(aprs_acknowledge_number));
   
    // Extract from address
    cnt = 0;    
    while( rxBuffer[position] != 0 )
    {
        aprs_source_address[cnt++] = rxBuffer[position];
        if ( rxBuffer[position] == '>' && position < 10 ) {
            aprs_source_address[cnt-1] = 0;
            valid_apsr_data = true;
                position++;
            break;
        }
        position++;
    }
    
    if (valid_apsr_data == true)
    {
        // Extract digi path
        valid_apsr_data = false;
        cnt = 0;
        while( rxBuffer[position] != 0 )
        {
            aprs_digi_path[cnt++] = rxBuffer[position];
            if ( rxBuffer[position] == ':' ) {
                aprs_digi_path[cnt-1] = 0;
                valid_apsr_data = true;
                    position++;
                break;
            }
            position++;
        }
        
    }

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

            if ( is_message_for_me(aprs_data_field, AprsSettings.MyCall) ) 
            {
               
                // Extract aprs message from data field
                position=11;
                while( aprs_data_field[position] != 0 )
                {
                    aprs_message[position-11] = aprs_data_field[position];
                    position++;
                }
                
                // Extract command and acknowledge number (if present)
                cnt = 0;
                position = 0;
                while( aprs_message[position] != 0 )
                {
                    if ( aprs_message[position] == '{' ) {
                        
                        aprs_acknowledge_number[cnt++] = 'a';
                        aprs_acknowledge_number[cnt++] = 'c';
                        aprs_acknowledge_number[cnt++] = 'k';
                        aprs_acknowledge_request = true;
                    }
                    // Calculate server command
                    if (!aprs_acknowledge_request) {
                        aprs_server_command = 10*aprs_server_command + aprs_message[position]-48;
                    }
                    
                    position++;
                    if (aprs_acknowledge_request) {
                        aprs_acknowledge_number[cnt++] = aprs_message[position];
                    }
                }
                aprs_acknowledge_number[cnt] = 0;
            }
            
        }
        
        log_out("Source address: %s\nDigipeaters (%u): %s %s %s %s\nData: %s\n", aprs_source_address, number_of_digipeaters+1, aprs_digis[0], aprs_digis[1], aprs_digis[2], aprs_digis[3], aprs_data_field);
        if (aprs_message[0])
        {
            log_out("Message from server: %s (command %u)\n", aprs_message, aprs_server_command);
            if (aprs_acknowledge_request) {
                ComposeAprsFrame(aprs_acknowledge_number);
                log_out("Acknowledge request: %s\n", aprs_acknowledge_number);
            }
            
        }

    }    
    else
        log_out("Error decoding APRS frame.");
    
    return (aprs_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 transmit() {
  uint16_t position = 0;
  
  LoRa.beginPacket();
  while( txBuffer[position] != 0 )
  {
    LoRa.write(txBuffer[position]);
    position++;
  }
  LoRa.endPacket();

  LoRa.receive();
}