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

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

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
PacketizerDSP::PacketizerDSP() :
    buf1(NULL),
    buf2(NULL),
    buf_len(0)
{
    // 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);

    // 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;

    // 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 );

}

// Destructor
PacketizerDSP::~PacketizerDSP()
{
    delete [] controlBlock;
    binary_sequence_destroy(pn_sync_code);

    binary_sequence_destroy( pn_sync_buffer );
    binary_sequence_destroy( pn_control_buffer );
    binary_sequence_destroy( pn_eom_buffer );
}

void PacketizerDSP::SetBuffer(char *_b1, 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 PacketizerDSP::RequiredBufferSize(unsigned int input_length)
{
    switch (packet_type) {
    case PACKET_RAW_400:
        return 512;
    default:
        ///\todo calculate buffer size based on coding rate, etc.
        std::cerr << "ERROR! PacketizerDSP::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 PacketizerDSP::ConfigurePacketType( PacketType type )
{
    switch (type) {
    case PACKET_RAW_400:
        ConfigureControl( 0x00 );
        dynamic_packet_size = false;
        packet_subheader_length = 21;
        packet_data_length = 400;
        break;
    default:
        std::cerr << "ERROR! PacketizerDSP::ConfigurePacketType() "
                  << "Unsupported packet type: " << type << std::endl;
        throw 0;
    }

    packet_type = type;
}


// Sets pointers in controlBlock to appropriate control codes
void PacketizerDSP::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
void PacketizerDSP::AssemblePacket(
    char* input,
    unsigned int input_length,
    char* output,
    unsigned int &output_length,
    PacketType type)
{
    unsigned int i;
    output_index = 0;

    ConfigurePacketType( type );

    // 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! PacketizerDSP::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! PacketizerDSP::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;
    }

    switch (type) {
    case PACKET_RAW_400:

        // configure sub-header here
        for (i=0; i<3; i++) {
            memcpy( output+output_index, pn_code_7_inv, PN_CONTROL_CODE_LENGTH);
            output_index += PN_CONTROL_CODE_LENGTH;
        }
        // output_index should be at this point 112;

        // no error correction coding; copy data to output buffer
        memcpy( output+output_index, input, 400 );
        output_index += 400;

        output_length = output_index;

        break;
    default:
        std::cerr << "ERROR! PacketizerDSP::AssemblePacket(): unsupported packet type" << std::endl;
        throw 0;
    }
}

bool PacketizerDSP::ExtractPacketHeader(
    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 );

        //printf("rxy(%d) = %d;\n", i, rxy);
        if ( abs(rxy) > correlator_threshold ) {
            //printf("\nPN sync code found!!\n\n");
            invert_bits = ( rxy < 0 ) ? true : false;
            num_bits_read = i+1;
            num_control_codes_extracted = 0;
            num_control_bits_read = 0;
            new_packet_type = 0;
            return true;
        }
    }

    num_bits_read = input_length;
    return false;

}


bool PacketizerDSP::ExtractPacketType(
    char * input,
    unsigned int input_length,
    unsigned int &num_bits_read)
{
    unsigned int i;

    // Extract control codes to describe packet type.
    // At this point the P/N sync code header has been found.
    for (i=0; i<input_length; i++) {
        // push bit into shift register
        binary_sequence_push( pn_control_buffer, input[i] & 0x01 );
        num_control_bits_read++;

        if ( num_control_bits_read==PN_CONTROL_CODE_LENGTH ) {
            // correlate with control code
            rxy = binary_sequence_correlate( pn_control_buffer, pn_control_code );

            // store result in new packet type variable
            new_packet_type <<= 1;
            if (invert_bits)
                new_packet_type |= ( rxy > 0 ) ? 0x00 : 0x01;
            else
                new_packet_type |= ( rxy > 0 ) ? 0x01 : 0x00;

            num_control_bits_read = 0;
            num_control_codes_extracted++;

            if ( num_control_codes_extracted == NUM_CONTROL_CODES ) {
                // try to configure the new packet type
                ConfigurePacketType( (PacketType) new_packet_type );
                num_bits_read = i+1;
                num_subheader_bits_read = 0;
                return true;
            }
        }
    }
    
    num_bits_read = input_length;
    return false;
}


bool PacketizerDSP::ExtractPacketSubheader(
    char * input,
    unsigned int input_length,
    char * output,
    unsigned int &output_length)
{
    unsigned int i;

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

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

    return false;
}

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

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

    return packet_extracted;
}

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

    return false;
}


bool PacketizerDSP::ExtractPacketDataFixedLength(
    char * input,
    unsigned int input_length,
    char * output,
    unsigned int &output_length)
{
    unsigned int i;

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

        if (num_packet_bits_read == packet_data_length) {
            // packet data have been extracted
            output_length = i+1;
            return true;
        }
    }
    
    return false;
}


///
void PacketizerDSP::EncodePacketData(
    char * input,
    unsigned int input_length,
    char * output,
    unsigned int &output_length)
{
    if (buf1 == NULL || buf2 == NULL) {
        std::cerr << "ERROR! PacketizerDSP::EncodePacketData() " << 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! PacketizerDSP::EncodePacketData():" << std::endl
                  << "  => required output length (" << output_length_req
                  << ") exceeds available buffer size (" << buf_len << ")" << std::endl;
        throw 0;
    }
    
    switch (packet_type) {
    case PACKET_RAW_400:
        // nothing to do; just copy data
        memcpy( output, input, packet_data_length );
        output_length = input_length;
        return;
    default:;
        std::cerr << "ERROR! PacketizerDSP::EncodePacketData() " << std::endl
                  << "  => Unsupported packet type: " << packet_type << std::endl;
        throw 0;
    }
}