#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. TODO
 * 
 *  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;
    
    // 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) {
            
            while (cnt < 7) {
                encoded_call = EncodeCall(aprsframe->digis[digi_cnt]);
                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;
    
    /*cnt=0;
    while (ax25frame->lenght-- != 0)
    {
        printf("0x%X ", ax25frame->complete[cnt]);
        cnt++;
    }
    printf("\n");
    */

    //fwrite(ax25frame->complete, 1, ax25frame->lenght, stdout);
    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;
    
    static uint8_t call[7] = { 0,0,0,0,0,0,0} ;
    uint8_t ssid = 0;
    
    // extract call
    while( string[position] != 0 || cnt < 6)
    {
        call[cnt++] = string[position] << 1;
        if ( string[position] == '-') {
            position++;
            break;
        }
        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++;
        }
    }
    
    // if asterix is pressent in string, than the message has been repeated, so set the 'has been repeated' flag in the ssid register
    position = 0;
    while( string[position] != 0)
    {
        if (string[position] == '*')
            ssid = 0b10000000;
        
        position++;
    }
    
    ssid = (ssid << 1) | 0b01100000;
    
    // 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;
    
}