#include "kiss.h"

/*
 * KISS encoder uses two steps:
 * 
 * Step 1: encodes the LoRa APRS frame into an AX.25 frame. DONE
 * Step 2: encapsulates the AX.25 frame into a KISS frame. DONE
 * 
 *  struct aprs_frame {
 *      uint8_t source_address[10];
 *      uint8_t digi_path[255];
 *      uint8_t data_field[255];
 *      uint8_t message[255];
 *      uint8_t digis[10][10];
 *      uint8_t acknowledge_number[255];
 *      bool acknowledge_request = false;
 *      uint16_t server_command = 0;
 *      uint16_t number_of_digipeaters = 0;
 *      
 *      uint8_t valid_apsr_data = false;
 *  };
 *
 */
uint16_t KissClass::EncodeFrame(struct aprs_frame *aprsframe, struct ax25_frame *ax25frame)
{
    uint8_t *encoded_call;
    uint16_t cnt=0;
    uint16_t position = 0;
    uint8_t digi_cnt = 0;
    uint8_t escaped_string_cnt=0;
    
    //printf("Encode KISS");
    
    // Destination call
    //printf("Destination: ");
    encoded_call = EncodeCall(aprsframe->digis[0]);
    cnt = 0;
    while (cnt < 7)
    {
        ax25frame->complete[position] = *(encoded_call+cnt);
        //printf("0x%X ", *(encoded_call+cnt));
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf("\n");
    
    // Source call
    //printf("Source: ");
    cnt=0;
    encoded_call = EncodeCall(aprsframe->source_address);
    while (cnt < 7)
    {
        ax25frame->complete[position] = *(encoded_call+cnt);
        //printf("0x%X ", *(encoded_call+cnt));
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf("\n");
    
    // No digipeaters in path, so destination is the last call and therefore the 'last' flag should be set
    if (aprsframe->number_of_digipeaters == 0)
    {
        ax25frame->complete[position-1] |= 0x01;
    }
    else
    {

        // Path (digipeaters)
        //printf("Path: ");
        cnt=0;
        digi_cnt = 1;       // Start at position 1 as position 0 contains destination (which we already encoded)
        while (aprsframe->number_of_digipeaters-- != 0) {
            
            encoded_call = EncodeCall(aprsframe->digis[digi_cnt]);            
            while (cnt < 7) {
                ax25frame->complete[position] = *(encoded_call+cnt);
                //printf("0x%X ", *(encoded_call+cnt));
                //printf("0x%X ", ax25frame->complete[position]);
                cnt++;
                position++;
            }
            digi_cnt++;
            cnt = 0;
            
        }
        //printf("\n");
        
        // Set 'last' flag
        ax25frame->complete[position-1] |= 0x01;
    }
    
    // Add control fields
    ax25frame->complete[position++] = 0x03;
    ax25frame->complete[position++] = 0xF0;
    
    // All the dificult bits are done, now we just add the payload.
    //printf("Data:");
    cnt = 0;
    while (aprsframe->data_field[cnt] != 0 && cnt < 256) {
        ax25frame->complete[position] = aprsframe->data_field[cnt];
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf( "\n");
    
    // Store length of AX25 frame
    ax25frame->lenght = position;
    
    // Encapsulate AX.25 frame in KISS frame (including escaping FEND codes)    
    putchar(FEND);
    putchar(CMD_DATA);
    escaped_string_cnt=0;
    ax25frame->encoded_kiss_frame[escaped_string_cnt++] = FEND;
    ax25frame->encoded_kiss_frame[escaped_string_cnt++] = CMD_DATA;
    cnt=0;
    position=ax25frame->lenght;
    while (position-- != 0)
    {
        // Escape FESC and TFEND
        if (ax25frame->complete[cnt] == FEND) {
            putchar(FESC);
            putchar(TFEND);
            ax25frame->encoded_kiss_frame[escaped_string_cnt++] = FESC;
            ax25frame->encoded_kiss_frame[escaped_string_cnt++] = TFEND;
        } else if (ax25frame->complete[cnt] == FESC) {
            putchar(FESC);
            putchar(TFESC);
            ax25frame->encoded_kiss_frame[escaped_string_cnt++] = FESC;
            ax25frame->encoded_kiss_frame[escaped_string_cnt++] = TFESC;
        } else {
            putchar(ax25frame->complete[cnt]);
            ax25frame->encoded_kiss_frame[escaped_string_cnt++] = ax25frame->complete[cnt];
        }
        cnt++;
    }
    putchar(FEND);
    ax25frame->encoded_kiss_frame[escaped_string_cnt++] = FEND;
    //printf("\n");
    

    return 0;
}

/*
 * Decodes a KISS frame from the usb serial port
 * 
 * Input : string starting with FEND and ending with FEND
 * Output: filled struct AprsFrame
 * Return: 0 = OK, 1 = ERROR, 2 = EXIT KISS MODE
 */
uint16_t KissClass::DecodeFrame(uint8_t string[], struct kiss_tx_frame *kisstxframe)
{
    uint16_t position  =0 ;
    uint16_t loop_counter = 0;
    uint16_t cnt  =0 ;
    uint16_t ssid = 0;
    uint16_t last_digi = 0;
    uint16_t digi_cnt = 0;
    
    //printf("Decode KISS");
    
    kisstxframe->valid_data = false;
    
    // Not a valid frame
    if (string[position] != FEND)
        return 1;
    
    position++;
    
    // Is data frame: we de-escape the string and put it back in the same string with a NULL at the end as terminator.
    if (string[position] == CMD_DATA)
    {
        position++;
        while (string[position] != FEND)
        {
            // De-escape codes
            if (string[position] == FESC && string[position+1] == TFEND)
            {
                string[cnt] = FEND;
                position++;
            }
            else if (string[position] == FESC && string[position+1] == TFESC)
            {
                string[cnt] = FESC;
                position++;
            }
            else {
                string[cnt] = string[position];
            }
            cnt++;
            position++;
        }
       string[cnt] = 0;             //Terminate string
        
    }
    // Is command to exit KISS MODE
    else if (string[position] == CMD_EXIT_KISS) {
        return 2;
    }
    
    // If we are here we have a valid AX.25 frame in 'string' which is decapsulated from its KISS frame
    /*
    0-5 destination
    6 destination ssid
    7-12 source
    13 desitination sidd
    
    vanaf byte 13 LAST FLAG zoeken
    
    03
    f0
    payload tot aan NULL
    */
    
    // Decode destination call
    position = 0;
    cnt = 0;
    loop_counter = 0;
    while (loop_counter < 6)
    {
        kisstxframe->digis[0][cnt] = string[position] >> 1;
        // Remove space
        if (kisstxframe->digis[0][cnt] == ' ')
            cnt--;
        position++;
        loop_counter++;
        cnt++;
        //printf("%u ", position);
    }
    
    // Extract SSID
    //printf("position: %u\n", position);
    ssid = (string[position] & 0b00011110) >> 1;
    //printf("DIGI SSID: %u (%u)\n", ssid, position);
    
    // Place SSID behind call or not if it is zero
    if (ssid != 0) {
        kisstxframe->digis[0][cnt++] = '-';
        if (ssid < 10)
        {
            kisstxframe->digis[0][cnt++] = ssid + 48;
        }
        else
        {
            kisstxframe->digis[0][cnt++] = '1';
            kisstxframe->digis[0][cnt++] = (ssid%10)+48;
        }
    }
    // Is has-been-repeated flag set?
    if(string[position] & 0b10000000) {
        kisstxframe->digis[0][cnt] = '*';
        cnt++;
    }
    kisstxframe->digis[0][cnt] = 0;   //terminate string
    position++;
    
    // Decode source call
    cnt = 0;
    loop_counter=0;
    while (loop_counter < 6)
    {
        kisstxframe->source_address[cnt] = string[position] >> 1;
        // Remove space
        if (kisstxframe->source_address[cnt] == ' ')
            cnt--;
        position++;
        loop_counter++;
        cnt++;
        //printf("%u ", position);
    }
    
    // Extract SSID
    ssid = (string[position] & 0b00011110) >> 1;
    //printf("sources SSID: %u (%u)\n", ssid, position);
    
   // Place SSID behind call or not if it is zero
    if (ssid != 0) {
        kisstxframe->source_address[cnt++] = '-';
        if (ssid < 10)
        {
            kisstxframe->source_address[cnt++] = ssid + 48;
        }
        else
        {
            kisstxframe->source_address[cnt++] = '1';
            kisstxframe->source_address[cnt++] = (ssid%10)+48;
        }
    }
    // Is has-been-repeated flag set?
    if(string[position] & 0b10000000) {
        kisstxframe->source_address[cnt] = '*';
        cnt++;
    }
    kisstxframe->source_address[cnt] = 0;   //terminate string
    
    // Check LAST flag
    if ( (string[position] & 0b00000001))
        last_digi = 1;
    
    position++;
     
    // Decode digi path
    digi_cnt=1;
    while (last_digi == 0) {
        
        cnt = 0;
        loop_counter=0;
        while (loop_counter < 6)
        {
            kisstxframe->digis[digi_cnt][cnt] = string[position] >> 1;
            // Remove space
            if (kisstxframe->digis[digi_cnt][cnt] == ' ')
                cnt--;
            position++;
            loop_counter++;
            cnt++;
            //printf("%u ", position);
        }
        
        // Extract SSID
        ssid = (string[position] & 0b00011110) >> 1;
        //printf("DIGI  SSID: %u (%u)\n", ssid, position);
        
        // Place SSID behind call or not if it is zero
        if (ssid != 0) {
            kisstxframe->digis[digi_cnt][cnt++] = '-';
            if (ssid < 10)
            {
                kisstxframe->digis[digi_cnt][cnt++] = ssid + 48;
            }
            else
            {
                kisstxframe->digis[digi_cnt][cnt++] = '1';
                kisstxframe->digis[digi_cnt][cnt++] = (ssid%10) + 48;
            }
        }
     
        // Is has-been-repeated flag set?
        if(string[position] & 0b10000000) {
            kisstxframe->digis[digi_cnt][cnt] = '*';
            cnt++;
        }
        kisstxframe->digis[digi_cnt][cnt] = 0;   //terminate string
        
        // Check LAST flag (also ends when more than 10 digis)
        if ( (string[position] & 0b00000001) || digi_cnt > 8)
            last_digi = 1;
    
        digi_cnt++;
        position++;
    }
    kisstxframe->number_of_digipeaters = digi_cnt-1;
    //printf("Digipeaters: %u \n",kisstxframe->number_of_digipeaters );
    
    //Skip the two control fields
    position++;
    position++;
   
    //Rest of string up to NULL is payload
    cnt = 0;
    kisstxframe->data_field[cnt++] = ':';
    while (string[position] != 0 && position < 512)
    {
        kisstxframe->data_field[cnt] = string[position];
        position++;
        cnt++;
    }
    kisstxframe->data_field[cnt] = 0;     //terminate string
    
    kisstxframe->valid_data = true;
    
    
    return 0;
}

/*
 * Encodes call according to AX.25 specs.
 * 
 * input : string with call and ssid as readable characters
 * output: pointer to memory location (7 bytes with the encoded call)
 * 
 */
uint8_t * KissClass::EncodeCall(uint8_t string[])
{
    uint8_t position = 0;
    uint8_t cnt = 0;
    uint8_t repeat_flag = 0;
    
    static uint8_t call[7] = { 0,0,0,0,0,0,0} ;
    uint8_t ssid = 0;
    
    // if asterix is pressent in string, than the message has been repeated, so set the 'has been repeated' flag in the ssid register
    // At the same time delete the asterix and place a NULL (string terminator) at its place. As the asterix should be the last character in the string, this should work.
    position = 0;
    while( string[position] != 0)
    {
        if (string[position] == '*') {
            repeat_flag = 1;
            string[position] == 0;
            break;
        }
        
        position++;
    }
    
    position = 0;
    // extract call
    while( string[position] != 0 || cnt < 6)
    {
        if ( string[position] == '-') {
            position++;
            break;
        } else {
            call[cnt] = string[position] << 1;            
        }
        cnt++;
        position++;
    }
    // pad with spaces to a length of 6
    while( cnt < 6 )
    {
        call[cnt++] = ' ' << 1;
    }
    
    // extract ssid
    cnt=0;
    while( string[position] != 0 )
    {
        if (string[position] >= 48 && string[position] <= 57) {
            ssid = 10*ssid + string[position]-48;
            position++;
        }
    }
    
    ssid = ssid << 1;
    ssid |= 0b01100000;
    
    if (repeat_flag == 1)            
        ssid |= 0b10000000;
    
    // add encoded ssid to encoded call array
    call[6] = ssid;
    
    /*
    //printf("SSID: 0x%X\n", ssid);
    
    cnt = 0;
    while (cnt < 7)
    {
        //printf("0x%X ", call[cnt++]);
    }
    //printf("\n");
    */
    
    return call;
    
}