root/experimental/components/Packetizer/trunk/Packetizer-metadata/src/PacketizerDSP.cpp @ 5520

/****************************************************************************

Copyright 2007 Virginia Polytechnic Institute and State University

This file is part of the OSSIE Packetizer.

OSSIE Packetizer 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 2 of the License, or
(at your option) any later version.

OSSIE Packetizer 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 OSSIE Packetizer; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

****************************************************************************/


#include <iostream>
#include "PacketizerDSP.h"
#include <math.h>

// Default constructor
PacketizerBase::PacketizerBase()
{
    // Initialize P/N synchronization code block
    pnSyncCode = pn_code_63;

    // Initialize P/N synchronization shift register
    pn_sync_code = binary_sequence_create();
    binary_sequence_initialize(pn_sync_code, PN_SYNC_CODE_LENGTH);
    for (unsigned int i=0; i<PN_SYNC_CODE_LENGTH; i++)
        binary_sequence_push(pn_sync_code, pnSyncCode[i] & 0x01);

    // Initialize P/N synchronization code block
    pnControlCode = pn_code_7;

    // Initialize P/N control code shift register
    pn_control_code = binary_sequence_create();
    binary_sequence_initialize(pn_control_code, PN_CONTROL_CODE_LENGTH);
    for (unsigned int i=0; i<PN_CONTROL_CODE_LENGTH; i++)
        binary_sequence_push(pn_control_code, pnControlCode[i] & 0x01);

    packet_header_length = NUM_CONTROL_CODES*PN_CONTROL_CODE_LENGTH;

    // initialize buffers
    buf1 = new unsigned char[MAX_PACKET_SIZE];
    buf2 = new unsigned char[MAX_PACKET_SIZE];
    buf3 = new unsigned char[MAX_PACKET_SIZE];
    buf_len = MAX_PACKET_SIZE;

    // 4-bit RS (15,11) code for 12 blocks
    //    132 4-bit symbols (uncoded)
    // -> 180 4-bit symbols (coded)
    // symsize = 4;
    // nn = (1<<symsize)-1 = 15
    // nroots = 4;
    // kk = nn-nroots = 11
    rs_4_15_11 = init_rs_char(4,0x13,1,1,4,0);

    // 6-bit RS (63,55) punctured to (35,27)
    //    27 symbols (uncoded)
    // -> 35 symbols (coded)
    // symsize = 6;
    // npad = 28;
    // nn = (1<<symsize)-1-npad = 63-28 = 35
    // nroots = 8;
    // kk = nn-nroots = 35-8 = 27
    rs_6_35_27 = init_rs_char(6,0x43,1,1,8,28);

    src_id = 0;
    dst_id = 0;
    port_id = 0;
    app_id = 0;
    packet_id = 0;
}

// Destructor
PacketizerBase::~PacketizerBase()
{
    binary_sequence_destroy(pn_sync_code);
    free_rs_char(rs_4_15_11);
    free_rs_char(rs_6_35_27);

    delete [] buf1;
    delete [] buf2;
    delete [] buf3;
}

PacketEncoder::PacketEncoder()
{
    // Initialize control block pointers
    controlBlock = new char*[NUM_CONTROL_CODES];

    // Default control block codes to be all positive
    for (unsigned int i=0; i<NUM_CONTROL_CODES; i++)
        controlBlock[i] = pnControlCode;
}

PacketDecoder::PacketDecoder()
{
    // Initialize shift registers for receiver
    pn_sync_buffer    = binary_sequence_create();
    pn_control_buffer = binary_sequence_create();
    pn_eom_buffer     = binary_sequence_create();
    binary_sequence_initialize( pn_sync_buffer,    PN_SYNC_CODE_LENGTH );
    binary_sequence_initialize( pn_control_buffer, PN_CONTROL_CODE_LENGTH );
    binary_sequence_initialize( pn_eom_buffer,     PN_SYNC_CODE_LENGTH );

    op_mode = FIND_HEADER;
}

// Destructor
PacketEncoder::~PacketEncoder()
{
    delete [] controlBlock;
}

// Destructor
PacketDecoder::~PacketDecoder()
{
    binary_sequence_destroy( pn_sync_buffer );
    binary_sequence_destroy( pn_control_buffer );
    binary_sequence_destroy( pn_eom_buffer );
}

void PacketizerBase::SetBuffer(unsigned char *_b1, unsigned char *_b2, unsigned int _len)
{
    buf1 = _b1;
    buf2 = _b2;
    buf_len = _len;
}

// Returns the necessary buffer size for processing given the current
// packet configuration
unsigned int PacketizerBase::RequiredBufferSize(unsigned int input_length)
{
    switch (packet_type) {
    case PACKET_RAW_400:
        return 512;
    case PACKET_RS_512:
        return 1024;
    default:
        ///\todo calculate buffer size based on coding rate, etc.
        std::cerr << "ERROR! PacketizerBase::RequiredBufferSize() "
                  << "Unsupported packet type: " << packet_type << std::endl;
        throw 0;
    }

    // should never get to this point, but include the return statement
    // to keep the compiler happy
    return 0;
}

//
void PacketizerBase::CheckBuffers(unsigned int input_length)
{
    if (buf1 == NULL || buf2 == NULL) {
        std::cerr << "ERROR! PacketizerBase:" << std::endl
                  << "  => buffers not yet initialized!" << std::endl;
        throw 0;
    }

    // ensure buffer size is sufficient
    unsigned int output_length_req = RequiredBufferSize(input_length);

    // ensure output buffer is sufficiently long
    if ( output_length_req > buf_len ) {
        std::cerr << "ERROR! PacketizerBase:" << std::endl
                  << "  => required output length (" << output_length_req
                  << ") exceeds available buffer size (" << buf_len << ")" << std::endl;
        throw 0;
    }
}


//
void PacketEncoder::ConfigurePacketType( PacketType type )
{
    switch (type) {
    case PACKET_RAW_400:
        ConfigureControl( 0x00 );
        dynamic_packet_size = false;
        packet_subheader_length = 21;
        packet_data_length = 400;
        packet_data_length_encoded = 400;
        break;
    case PACKET_RS_512:
        ConfigureControl( 0x01 );
        dynamic_packet_size = false;
        packet_subheader_length = 213;
        packet_data_length = 512;
        packet_data_length_encoded = 720;
        fec_inner = RS_4_15_11;
        fec_outer = FEC_NONE;
        crc = CRC_CHECKSUM_16;
        break;
    default:
        std::cerr << "ERROR! PacketEncoder::ConfigurePacketType() "
                  << "Unsupported packet type: " << type << std::endl;
        //throw 0;
        ConfigurePacketType(PACKET_RAW_400);
        return;
    }

    packet_type = type;
}


//
void PacketizerBase::ConfigurePacketType( PacketType type )
{
    switch (type) {
    case PACKET_RAW_400:
        dynamic_packet_size = false;
        packet_subheader_length = 21;
        packet_data_length = 400;
        packet_data_length_encoded = 400;
        break;
    case PACKET_RS_512:
        dynamic_packet_size = false;
        packet_subheader_length = 213;
        packet_data_length = 512;
        packet_data_length_encoded = 720;
        fec_inner = RS_4_15_11;
        fec_outer = FEC_NONE;
        crc = CRC_CHECKSUM_16;
        break;
    default:
        std::cerr << "ERROR! PacketizerBase::ConfigurePacketType() "
                  << "Unsupported packet type: " << type << std::endl;
        //throw 0;
        ConfigurePacketType(PACKET_RAW_400);
        return;
    }

    packet_type = type;
}


// Completely constructs and encodes a packet from raw data bits
bool PacketEncoder::CreatePacket(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int encoded_packet_length;
    unsigned int nr;
    EncodePacketData(
        input,
        input_length,
        num_read,
        buf3,
        buf_len,
        encoded_packet_length);

    AssemblePacket(
        buf3,
        encoded_packet_length,
        nr,
        output,
        output_length,
        num_written);

    return true;
}


// Completely finds and decodes a packet from raw data
bool PacketDecoder::ExtractPacket(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int nr(0); // samples read from input buffer on each function
    unsigned int nw(0); // samples written to output buffer on each function
    bool success;

    // reset counter return values
    num_read = 0;
    num_written = 0;

    //
    switch (op_mode) {
    case FIND_HEADER:
        //
        DEBUG_PRINTF(2, "MODE 0: EXTRACT_HEADER\n");
        success = FindHeader(input, input_length, nr);
        num_read += nr;

        if ( success ) {
            DEBUG_PRINTF(2, "  :: header found!\n");
            op_mode = EXTRACT_HEADER;
        } else {
            return false;
        }
    case EXTRACT_HEADER:
        //

        DEBUG_PRINTF(2, "MODE 1: EXTRACT_HEADER\n");

        success = ExtractHeader(
                    input + num_read,
                    input_length - num_read,
                    nr,
                    buf1 + num_header_bits_read,
                    buf_len - num_header_bits_read,
                    nw);
        num_read += nr;

        if ( success ) {
            DEBUG_PRINTF(2, "  :: header extracted!\n");
            op_mode = DECODE_HEADER;
        } else {
            return false;
        }

    case DECODE_HEADER:
        //

        DEBUG_PRINTF(2, "MODE 2: DECODE_HEADER\n");

        success = DecodeHeader(buf1, buf_len);

        if ( success ) {
            DEBUG_PRINTF(2, "  :: header decoded!\n");
            op_mode = EXTRACT_SUBHEADER;
        } else {
            return false;
        }
    case EXTRACT_SUBHEADER:
        //

        DEBUG_PRINTF(2, "MODE 3: EXTRACT_SUBHEADER\n");

        // try to extract subheader and put into external buffer 1
        success = ExtractSubheader(
                    input + num_read,
                    input_length - num_read,
                    nr,
                    buf1 + num_subheader_bits_read,
                    buf_len - num_subheader_bits_read,
                    nw);
        num_read += nr;

        if ( success ) {
            DEBUG_PRINTF(2, "  :: subheader extracted!\n");
            op_mode = DECODE_SUBHEADER;
        } else {
            return false;
        }
    case DECODE_SUBHEADER:
        //

        DEBUG_PRINTF(2, "MODE 4: DECODE_SUBHEADER\n");

        success = DecodeSubheader(buf1, buf_len);

        if ( success ) {
            DEBUG_PRINTF(2, "  :: subheader decoded!\n");
            op_mode = EXTRACT_PACKET_DATA;
        } else {
            DEBUG_PRINTF(2, "  COULD NOT DECODE SUBHEADER!\n");
            op_mode = EXTRACT_HEADER;
            return false;
        }

    case EXTRACT_PACKET_DATA:
        //

        DEBUG_PRINTF(2, "MODE 5: EXTRACT_PACKET_DATA\n");

        // try to extract subheader and put into external buffer 1
        success = ExtractPacketData(
                    input + num_read,
                    input_length - num_read,
                    nr,
                    buf1 + num_packet_bits_read,
                    buf_len - num_packet_bits_read,
                    nw);
        num_read += nr;

        if ( success ) {

            DEBUG_PRINTF(2, "  :: packet data extracted!\n");

            op_mode = DECODE_PACKET_DATA;
        } else {
            return false;
        }
    case DECODE_PACKET_DATA:
        //

        DEBUG_PRINTF(2, "MODE 6: DECODE_PACKET_DATA\n");

        success = DecodePacketData(
                buf1,
                packet_data_length_encoded,
                nr,
                output,
                output_length,
                num_written);
        
        op_mode = FIND_HEADER;

        if ( success ) {

            DEBUG_PRINTF(2, "  :: packet data decoded!\n");

            return true;
        } else {
            DEBUG_PRINTF(2, "  COULD NOT DECODE PACKET DATA!\n");
            return false;
        }
    default:;
    }

    return false;
}



// Sets pointers in controlBlock to appropriate control codes
void PacketEncoder::ConfigureControl( unsigned char id )
{
    for (unsigned int i=NUM_CONTROL_CODES; i>0; i--) {
        controlBlock[i-1] = ( id & 0x01 ) ? pn_code_7 : pn_code_7_inv;
        id >>= 1;
    }
}

// Assemble packet and store in output buffer
bool PacketEncoder::AssemblePacket(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int i;
    unsigned int output_index(0);

    // check output size
    unsigned int output_length_req = RequiredBufferSize(input_length);

    // ensure output buffer is sufficiently long
    if ( output_length_req > output_length ) {
        std::cerr << "ERROR! PacketEncoder::AssemblePacket():" << std::endl
                  << "  => required output length (" << output_length_req
                  << ") exceeds buffer size (" << output_length << ")" << std::endl;
        throw 0;
    }
    
    // ensure output packet does not exceed the maximum allowable
    if ( output_length_req > MAX_PACKET_SIZE ) {
        std::cerr << "ERROR! PacketEncoder::AssemblePacket():" << std::endl
                  << "  => required output length (" << output_length_req 
                  << ") exceeds maximum size (" << MAX_PACKET_SIZE << ")" << std::endl;
        throw 0;
    }

    // write P/N sync code
    memcpy(output+output_index, pnSyncCode, PN_SYNC_CODE_LENGTH);
    output_index += PN_SYNC_CODE_LENGTH;

    // write control codes
    for (i=0; i<NUM_CONTROL_CODES; i++) {
        memcpy(output+output_index, controlBlock[i], PN_CONTROL_CODE_LENGTH);
        output_index += PN_CONTROL_CODE_LENGTH;
    }

    //unsigned int nr;
    unsigned int nw;

    EncodeSubheader(output+output_index, output_length-output_index, nw);
    output_index += nw;
    memmove(&output[output_index], input, input_length*sizeof(unsigned char));
    //EncodePacketData(input, input_length, nr, output+output_index, output_length-output_index, nw);
    output_index += input_length;

    num_written = output_index;
    num_read = packet_data_length; ///\todo num_read = input_length ???

    return true;
}

// Encode subheader
bool PacketEncoder::EncodeSubheader(
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    ///\todo use function pointer for this

    switch (packet_type) {
    case PACKET_RAW_400:
        return EncodeSubheader_PACKET_RAW_400(output, output_length, num_written);
    case PACKET_RS_512:
        return EncodeSubheader_PACKET_RS_512(output, output_length, num_written);
    default:
        std::cerr << "ERROR! PacketEncoder::EncodeSubheader(): unsupported packet type" << std::endl;
        throw 0;
    }

}

// Encode subheader for PACKET_RAW_400
bool PacketEncoder::EncodeSubheader_PACKET_RAW_400(
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int i;
    unsigned int N(0);

    ///\todo use reed-solomon codec to encode subheader data
    // configure sub-header here
    if ( packet_id > 7 )
        printf("WARNING! PacketEncoder::EncodeSubheader_PACKET_RAW_400() cannot encode packet_id > 7\n");

    for (i=0; i<3; i++) {
        if ( (packet_id >> (2-i)) & 0x01 )
            memcpy( output+N, pn_code_7, PN_CONTROL_CODE_LENGTH);
        else
            memcpy( output+N, pn_code_7_inv, PN_CONTROL_CODE_LENGTH);

        N += PN_CONTROL_CODE_LENGTH;
    }
    num_written = N;
    return true;
}

// Encode subheader for PACKET_RS_512
bool PacketEncoder::EncodeSubheader_PACKET_RS_512(
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int nw;    // num samples written from each function

    // 1. push 16 8-bit symbols into buf1
    // variable     num bits
    // ----         ----
    // packet_id    32
    // src_id        8
    // dst_id        8
    // app_id        8
    // port_id       8
    // packet_len   16
    // expansion    48
    buf1[0] = (packet_id >> 0)  & 0xFF;
    buf1[1] = (packet_id >> 8)  & 0xFF;
    buf1[2] = (packet_id >> 16) & 0xFF;
    buf1[3] = (packet_id >> 24) & 0xFF;

    buf1[4] = (src_id >> 0 ) & 0xFF;

    buf1[5] = (dst_id >> 0 ) & 0xFF;

    buf1[6] = (app_id >> 0 ) & 0xFF;

    buf1[7] = (port_id >> 0 ) & 0xFF;

    buf1[8] = (packet_data_length >> 0 ) & 0xFF;
    buf1[9] = (packet_data_length >> 8 ) & 0xFF;

    buf1[10] = 7;
    buf1[11] = 7;
    buf1[12] = 7;
    buf1[13] = 7;
    buf1[14] = 7;
    buf1[15] = 7;

    // 2. compute CRC and append to end of message
    FastCrc crc(0x04c11db7);
    for (unsigned int i=0; i<16; i++)
        crc.PutByte(buf1[i]);
    UINT32 crc_code = crc.Done();

    buf1[16] = (crc_code >> 0 )  & 0xFF;
    buf1[17] = (crc_code >> 8 )  & 0xFF;
    buf1[18] = (crc_code >> 16 ) & 0xFF;
    buf1[19] = (crc_code >> 24 ) & 0xFF;

    // 3. repack 20 8-bit symbols (+2 bits) into 27 6-bit symbols
    SigProc::repack_bytes(buf1, 8, 20, buf2, 2, buf_len, &nw);
    buf2[640] = 0;
    SigProc::repack_bytes(buf2, 2, 641, buf1, 6, buf_len, &nw);
    
    // 4. use 6-bit RS (63,55) punctured to (35,27)
    //    27 symbols (uncoded)
    // -> 35 symbols (coded)
    int symsize = 6;
    int npad = 28;
    int nn = (1<<symsize)-1-npad;    // 63-28 = 35
    int nroots = 8;
    int kk = nn-nroots;             // 35-8 = 27
    encode_rs_char(rs_6_35_27, buf1, &buf1[kk]);

    // 5. unpack to output buffer
    SigProc::repack_bytes(buf1, 6, 35, output, 1, output_length, &num_written);
    num_written += 3; // pad to 213
    return true;
}

bool PacketDecoder::FindHeader(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_bits_read)
{
    rxy = 0;
    int correlator_threshold(60);
    unsigned int i(0);
    
    for (i=0; i<input_length; i++) {
        // push bit into shift register
        binary_sequence_push( pn_sync_buffer, input[i] & 0x01 );

        // correlate with P/N sync code
        rxy = binary_sequence_correlate( pn_sync_buffer, pn_sync_code );

        if ( abs(rxy) > correlator_threshold ) {
            DEBUG_PRINTF(4, "\nPN sync code found!!\n\n");
            invert_bits = ( rxy < 0 ) ? true : false;
            num_bits_read = i+1;
            num_header_bits_read = 0;
            return true;
        }
    }

    num_bits_read = input_length;
    return false;

}


bool PacketDecoder::ExtractHeader(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int i;
    num_written = 0;
    num_read = 0;

    for (i=0; i<input_length; i++) {
        output[i] = invert_bits ? (~input[i]) & 0x01 : input[i];
        num_header_bits_read++;
        num_written++;
        num_read++;

        if ( num_header_bits_read == packet_header_length ) {
            // header data have been extracted
            num_subheader_bits_read = 0;
            return true;
        }
    }

    return false;
}

// decode control codes to describe packet type
bool PacketDecoder::DecodeHeader(
    unsigned char* input,
    unsigned int input_length)
{
    // At this point the P/N sync code header has been found and
    // the data should be completely stored in the input buffer

    unsigned int new_packet_type(0);
    for (unsigned int i=0; i<NUM_CONTROL_CODES; i++) {
        for (unsigned int j=0; j<PN_CONTROL_CODE_LENGTH; j++) {
            binary_sequence_push( pn_control_buffer, input[j+i*PN_CONTROL_CODE_LENGTH] & 0x01 );
        }

        rxy = binary_sequence_correlate( pn_control_buffer, pn_control_code );

        new_packet_type <<= 1;
        new_packet_type |= ( rxy > 0 ) ? 0x01 : 0x00;
    }

    ConfigurePacketType( (PacketType) new_packet_type );
    num_subheader_bits_read = 0;
    return true;
}


bool PacketDecoder::ExtractSubheader(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int i;
    num_written = 0;
    num_read = 0;

    for (i=0; i<input_length; i++) {
        output[i] = invert_bits ? (~input[i]) & 0x01 : input[i];
        num_subheader_bits_read++;
        num_written++;
        num_read++;

        if ( num_subheader_bits_read == packet_subheader_length ) {
            // subheader data have been extracted
            num_packet_bits_read = 0;
            return true;
        }
    }

    return false;
}

//
bool PacketDecoder::DecodeSubheader(
    unsigned char* input,
    unsigned int input_length)
{
    ///\todo use function pointer instead of switch statement

    switch (packet_type) {
    case PACKET_RAW_400:
        return PacketDecoder::DecodeSubheader_PACKET_RAW_400(input, input_length);
    case PACKET_RS_512:
        return PacketDecoder::DecodeSubheader_PACKET_RS_512(input, input_length);
    default:;
    }

    return true;
}

//
bool PacketDecoder::DecodeSubheader_PACKET_RAW_400(
    unsigned char* input,
    unsigned int input_length)
{
    ///\todo use reed-solomon codec to decode subheader data
    packet_id = 0;
    for (unsigned int i=0; i<3; i++) {
        // shift 7 bits into buffer
        for (unsigned int j=0; j<PN_CONTROL_CODE_LENGTH; j++) {
            binary_sequence_push( pn_control_buffer, input[j+i*PN_CONTROL_CODE_LENGTH] & 0x01 );
        }

        rxy = binary_sequence_correlate( pn_control_buffer, pn_control_code );

        packet_id <<= 1;
        packet_id |= ( rxy > 0 ) ? 0x01 : 0x00;
    }
    return true;
}

bool PacketDecoder::DecodeSubheader_PACKET_RS_512(
    unsigned char* input,
    unsigned int input_length)
{
    unsigned int nw;    // num samples written from each function
    
    // 1. pack 210 bits into 35 6-bit symbols to buf2
    SigProc::repack_bytes(input, 1, 210, buf2, 6, buf_len, &nw);

    // 2. use RS 6-bit (63,55) code, punctured to (35,27)
    int symsize = 6;
    int npad = 28;
    //int nn = (1<<symsize)-1-npad;   // 63-28 = 35
    int nroots = 8;
    //int kk = nn-nroots;             // 35-8 = 27
    int nerrors(0);
    nerrors = decode_rs_char(rs_6_35_27, buf2, 0, 0);

    // 3. repack 27 6-bit symbols (-2 bits) to 20 8-bit symbols
    SigProc::repack_bytes(buf2, 6, 27, buf1, 2, buf_len, &nw);
    SigProc::repack_bytes(buf1, 2, 80, buf2, 8, buf_len, &nw);
    
    // 4. decode and compare CRC
    FastCrc crc(0x04c11db7);
    for (unsigned int i=0; i<16; i++)
        crc.PutByte( buf2[i] );
    UINT32 crc_code = crc.Done();
    UINT32 crc_rx(0);
    for (unsigned int i=0; i<4; i++) {
        crc_rx <<= 8;
        crc_rx |= 0xFF & (UINT32) (buf2[20-i-1]);
    }
    crc_rx = 0;
    crc_rx |= (buf2[16] << 0);
    crc_rx |= (buf2[17] << 8);
    crc_rx |= (buf2[18] << 16);
    crc_rx |= (buf2[19] << 24);
    if (crc_rx != crc_code) {
        printf("ERROR! PACKET_RS_512 subheader did not pass cyclic redundancy check\n");
        //return false;
    } else {
    }

    // 5. decode 16 8-bit symbols...
    // variable     num bits
    // ----         ----
    // packet_id    32
    // src_id        8
    // dst_id        8
    // app_id        8
    // port_id       8
    // packet_len   16
    // expansion    48
    packet_id = 0;
    packet_id = 0;
    packet_id |= (buf2[0] << 0);
    packet_id |= (buf2[1] << 8);
    packet_id |= (buf2[2] << 16);
    packet_id |= (buf2[3] << 24);

    src_id = 0;
    src_id |= buf2[4];

    dst_id = 0;
    dst_id |= buf2[5];

    app_id = 0;
    app_id |= buf2[6];

    port_id = 0;
    port_id |= buf2[7];

    packet_data_length = 0;
    packet_data_length |= buf2[8];
    packet_data_length |= buf2[9];

    return true;
}

bool PacketDecoder::ExtractPacketData(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    bool packet_extracted;

    if (dynamic_packet_size) {
        packet_extracted = 
            ExtractPacketDataDynamicLength(input, input_length, num_read, output, output_length, num_written);
    } else {
        packet_extracted = 
            ExtractPacketDataFixedLength(input, input_length, num_read, output, output_length, num_written);
    }

    return packet_extracted;
}

bool PacketDecoder::ExtractPacketDataDynamicLength(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    ///\todo look for eom code

    num_written = 0;
    num_read = input_length;
    return false;
}


bool PacketDecoder::ExtractPacketDataFixedLength(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int i;
    num_read = 0;
    num_written = 0;

    for (i=0; i<input_length; i++) {
        output[i] = invert_bits ? (~input[i]) & 0x01 : input[i];
        num_packet_bits_read++;
        num_read++;
        num_written++;

        if (num_packet_bits_read == packet_data_length_encoded) {
            // packet data have been extracted
            num_packet_bits_read = 0;
            return true;
        }
    }
    
    return false;
}


//
bool PacketEncoder::EncodePacketData(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    bool success;

    // CheckBuffers();

    switch (packet_type) {
    case PACKET_RAW_400:
        success = EncodePacketData_PACKET_RAW_400(
            input, input_length, num_read, output, output_length, num_written);
        return success;
    case PACKET_RS_512:
        success = EncodePacketData_PACKET_RS_512(
            input, input_length, num_read, output, output_length, num_written);
        return success;
    default:;
        std::cerr << "ERROR! PacketEncoder::EncodePacketData() " << std::endl
                  << "  => Unsupported packet type: " << packet_type << std::endl;
        throw 0;
    }
    
    return false;
}


bool PacketEncoder::EncodePacketData_PACKET_RAW_400(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    // nothing to do; just copy data
    memcpy( output, input, packet_data_length );
    num_read = packet_data_length;
    num_written = packet_data_length_encoded;
    return true;
}


bool PacketEncoder::EncodePacketData_PACKET_RS_512(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int nr;
    unsigned int nw;

    // 1. pack data into symbols
    // 512 1-bit symbols -> 128 4-bit symbols
    SigProc::repack_bytes(input, 1, 512, buf2, 4, buf_len, &nw);

    // 2. add crc/checksum: 16-bit checksum
    unsigned int ones(0);
    for (unsigned int i=0; i<128; i++)
        ones += c_ones[(unsigned int) (buf2[i])];
    ones = ones % 65536;    // 2^16
    buf2[128] = ones & 0x0F;
    buf2[129] = (ones >> 4) & 0x0F;
    buf2[130] = (ones >> 8) & 0x0F;
    buf2[131] = (ones >> 12) & 0x0F;

    // 3. apply RS 4-bit (15,11) code for 12 blocks
    //    132 4-bit symbols (uncoded)
    // -> 180 4-bit symbols (coded)
    int symsize = 4;
    int nn = (1<<symsize)-1;    // 15
    int nroots = 4;
    int kk = nn-nroots;         // 11
    nw = 0;
    nr = 0;
    for (unsigned int i=0; i<12; i++) {
        // copy data from buf2 to buf1
        memmove(&buf1[nw], &buf2[nr], kk);

        // apply encoder
        encode_rs_char(rs_4_15_11, &buf1[nw], &buf1[nw+kk]);
        nr += kk;
        nw += nn;
    }
    
    // 4. unpack data: move 4-bit symbols from buf1 to 1-bit symbols in output
    SigProc::repack_bytes(buf1, 4, nw, output, 1, output_length, &num_written);
    num_read = 512;

    return true;
}

//
bool PacketDecoder::DecodePacketData(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    bool success;

    switch (packet_type) {
    case PACKET_RAW_400:
        success = DecodePacketData_PACKET_RAW_400(
            input, input_length, num_read, output, output_length, num_written);
        return success;
    case PACKET_RS_512:
        success = DecodePacketData_PACKET_RS_512(
            input, input_length, num_read, output, output_length, num_written);
        return success;
    default:;
        std::cerr << "ERROR! PacketDecoder::DecodePacketData() " << std::endl
                  << "  => Unsupported packet type: " << packet_type << std::endl;
        throw 0;
    }

    return false;
}

bool PacketDecoder::DecodePacketData_PACKET_RAW_400(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    // nothing to do; just copy data
    memcpy( output, input, packet_data_length );
    num_read = packet_data_length_encoded;
    num_written = packet_data_length;
    return true;
}

bool PacketDecoder::DecodePacketData_PACKET_RS_512(
    unsigned char* input,
    unsigned int input_length,
    unsigned int &num_read,
    unsigned char* output,
    unsigned int output_length,
    unsigned int &num_written)
{
    unsigned int nr;
    unsigned int nw;

    // 1. Pack data into 4-bit symbols, put into buf2
    SigProc::repack_bytes(input, 1, 720, buf2, 4, buf_len, &nw);
    
    // 2. Decode using RS 4-bit (15,11) code on 12 blocks
    //    180 4-bit symbols (coded)
    // -> 132 4-bit symbols (uncoded)
    int symsize = 4;
    int nn = (1<<symsize)-1;    // 15
    int nroots = 4;
    int kk = nn-nroots;         // 11
    nw = 0;
    nr = 0;
    int nerrors(0);
    int nerrors_total(0);
    for (unsigned int i=0; i<12; i++) {
        // apply decoder (no erasure knowledge)
        nerrors = decode_rs_char(rs_4_15_11, &buf2[nr], 0, 0);
        nerrors_total = nerrors_total + nerrors;

        // copy data from buf2 to buf1
        memmove(&buf1[nw], &buf2[nr], kk);

        nr += nn;
        nw += kk;
    }

    // 3. calculate and compare crc/checksum (16-bit)
    unsigned int checksum(0);
    // read four 4-bit symbols from end of buffer
    for (unsigned int i=0; i<4; i++) {
        checksum <<= 4;
        checksum |= buf1[nw-i-1];
    }
    unsigned int ones(0);
    for (unsigned int i=0; i<128; i++)
        ones += c_ones[(unsigned int) (buf1[i])];
    //printf("checksum: %d, ones: %d\n", checksum, ones);

    // 4. unpack data: move 4-bit symbols from buf1 to 1-bit symbols in output
    SigProc::repack_bytes(buf1, 4, nw-4, output, 1, output_length, &num_written);
    num_read = 720;

    if (checksum != ones) {
        //printf("ERROR! did not pass checksum!\n");
        return false;
    } else {
        return true;
    }
}