#include <stdio.h>
#include <string.h>
#include <time.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "LoRa-RP2040.h"
#include "Config.h"
#include "KISS.h"

bool startRadio();
bool LoadSettings();
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;

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

   startRadio();
    
   while (1) {
      int packetSize = LoRa.parsePacket();
      if (packetSize) {
         // received a packet
         printf("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;
             printf("%s\n", rxBuffer);
             ServerCommand = decode_packet();
         } else {
             printf("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;
                  
              // 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;
          }
    }
  }    
  
  return 0;
}

/*
 * Load settings from EEPROM
 */
bool LoadSettings()
{
    printf("APRS settings:\n");
    printf("My call: %s\n", AprsSettings.MyCall);
    printf("Server call: %s\n", AprsSettings.ServerCall);
    
}
/*
 * 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
    
  printf("LoRa settings:\n");
  printf("loraFrequency       = %u\n", loraFrequency);    
  printf("loraSpreadingFactor = %i\n", loraSpreadingFactor);    
  printf("loraPreamble        = %i\n", loraPreamble);    
  printf("loraCodingRate      = %i\n", loraCodingRate);    
  printf("loraTxPower         = %i\n", loraTxPower);    
  printf("LoRaPaSelect        = %i\n", LoRaPaSelect);    
  printf("loraBandwidth       = %u\n", loraBandwidth);    
   
  printf("Starting LoRa radio");
  if (!LoRa.begin(loraFrequency)) {
    printf("  [ FAILED ]\n");
    while(1);
  }
  else {
    LoRa.setPreambleLength(loraPreamble);
    LoRa.setSignalBandwidth(loraBandwidth);
    LoRa.setTxPower(loraTxPower,LoRaPaSelect);
    LoRa.setSpreadingFactor(loraSpreadingFactor);
    LoRa.setCodingRate4(loraCodingRate);
    LoRa.enableCrc();
    printf("  [ 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;
            break;
        }
        position++;
    }
    
    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;
                break;
            }
            position++;
        }
        
    }

    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;
                }
                
                //position++;
                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;
            }
            
        }
        
        printf("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])
        {
            printf("Message from server: %s (command %u)\n", aprs_message, aprs_server_command);
            if (aprs_acknowledge_request) {
                ComposeAprsFrame(aprs_acknowledge_number);
                printf("Acknowledge request: %s\n", aprs_acknowledge_number);
            }
            
        }

    }    
    else
        printf("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; 
    printf("%s\n", txBuffer);
}

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

  LoRa.receive();
}