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

Copyright 2007 Virginia Polytechnic Institute and State University

This file is part of the OSSIE FrameAssembler_metadata.

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

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


#include <string>
#include <iostream>
#include "FrameAssembler_metadata.h"

FrameAssembler_metadata_i::FrameAssembler_metadata_i(const char *uuid, omni_condition *condition) : 
    Resource_impl(uuid), component_running(condition)
{
    dataIn_0 = new standardInterfaces_i::complexShort_p("SymbolsIn");
    dataOut_0 = new standardInterfaces_i::complexShort_u("FrameSymbolsOut");

    num_frames_sent_app_1 = 0;
    num_frames_sent_app_2 = 0;
    num_frames_sent_app_5 = 0;
    num_frames_sent_app_6 = 0;

    //Create the thread for the writer's processing function 
    processing_thread = new omni_thread(Run, (void *) this);

    //Start the thread containing the writer's processing function 
    processing_thread->start();

}

FrameAssembler_metadata_i::~FrameAssembler_metadata_i(void)
{   
    delete dataIn_0;
    delete dataOut_0;
}

// Static function for omni thread
void FrameAssembler_metadata_i::Run( void * data )
{
    ((FrameAssembler_metadata_i*)data)->ProcessData();
}

CORBA::Object_ptr FrameAssembler_metadata_i::getPort( const char* portName ) throw (
    CORBA::SystemException, CF::PortSupplier::UnknownPort)
{
    DEBUG(3, FrameAssembler_metadata, "getPort() invoked with " << portName)
    
    CORBA::Object_var p;

    p = dataIn_0->getPort(portName);

    if (!CORBA::is_nil(p))
        return p._retn();

    p = dataOut_0->getPort(portName);

    if (!CORBA::is_nil(p))
        return p._retn();

    // Requested port not recognized
    throw CF::PortSupplier::UnknownPort();
}

void FrameAssembler_metadata_i::start() throw (CORBA::SystemException, 
    CF::Resource::StartError)
{
    DEBUG(3, FrameAssembler_metadata, "start() invoked")
}

void FrameAssembler_metadata_i::stop() throw (CORBA::SystemException, CF::Resource::StopError) 
{  
    DEBUG(3, FrameAssembler_metadata, "stop() invoked")
}

void FrameAssembler_metadata_i::releaseObject() throw (CORBA::SystemException,
    CF::LifeCycle::ReleaseError)
{
    DEBUG(3, FrameAssembler_metadata, "releaseObject() invoked")
    
    component_running->signal();
}

void FrameAssembler_metadata_i::initialize() throw (CF::LifeCycle::InitializeError,
    CORBA::SystemException)
{
    DEBUG(3, FrameAssembler_metadata, "initialize() invoked")
}

void FrameAssembler_metadata_i::configure(const CF::Properties& props)
throw (CORBA::SystemException,
    CF::PropertySet::InvalidConfiguration,
    CF::PropertySet::PartialConfiguration)
{
    DEBUG(3, FrameAssembler_metadata, "configure() invoked")
    
    std::cout << "props length : " << props.length() << std::endl;

    for (unsigned int i = 0; i <props.length(); i++) {
        std::cout << "Property id : " << props[i].id << std::endl;

        if (strcmp(props[i].id, "DCE:c15e70e4-96e4-4cc8-b1f0-06cb301f6c2e") == 0) {
            // Modulation type
            const char * prop_str;
            props[i].value >>= prop_str;

            // Set appropriate modulation function
            if ( strcmp(prop_str, "BPSK") == 0 ) {
                ConfigureModulationScheme(SigProc::BPSK);
            } else if ( strcmp(prop_str, "QPSK") == 0 ) {
                ConfigureModulationScheme(SigProc::QPSK);
            } else if ( strcmp(prop_str, "8PSK") == 0 ) {
                ConfigureModulationScheme(SigProc::PSK8);
            } else if ( strcmp(prop_str, "16QAM") == 0 ) {
                ConfigureModulationScheme(SigProc::QAM16);
            } else if ( strcmp(prop_str, "4PAM") == 0 ) {
                ConfigureModulationScheme(SigProc::PAM4);
            } else {
                // unknown property
                std::cerr << "ERROR: FrameAssembler_metadata::configure() unknown mod. scheme "
                          << prop_str << std::endl;
                throw CF::PropertySet::InvalidConfiguration();
            }
            ///\todo catch exception thrown by FrameAssembler_metadataDSP::ConfigureModulationScheme()

        } else if (strcmp(props[i].id, "DCE:06b798ba-5412-4bbe-bf0b-8ca665d25b83") == 0) {
            // Frame size
            CORBA::UShort simple_temp;
            props[i].value >>= simple_temp;

            ConfigureFrameSize(simple_temp);

        } else {
            // unknown property
            std::cerr << "ERROR: FrameAssembler_metadata::configure() unknown property" << std::endl;
            throw CF::PropertySet::InvalidConfiguration();
        }
    }

}

void FrameAssembler_metadata_i::ConfigureModulationSchemeOnMetaData() {
    switch (metadata->modulation_scheme.scheme) {
    case standardInterfaces::ModulationScheme::PSK:
        switch (metadata->modulation_scheme.M) {
        case 2:
            ConfigureModulationScheme(SigProc::BPSK);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to BPSK");
            break;
        case 4:
            ConfigureModulationScheme(SigProc::QPSK);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to QPSK");
            break;
        case 8:
            ConfigureModulationScheme(SigProc::PSK8);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to 8PSK");
            break;
        default:
            std::cerr << "FrameAssembler_metadata-metadata: Unsupported PSK modulation depth" << std::endl;
            throw 0;
        }
        break;
    case standardInterfaces::ModulationScheme::QAM:
        switch (metadata->modulation_scheme.M) {
        case 4:
            ConfigureModulationScheme(SigProc::QPSK);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to QPSK (4QAM)");
            break;
        case 16:
            ConfigureModulationScheme(SigProc::QAM16);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to 16QAM");
            break;
        default:
            std::cerr << "FrameAssembler_metadata-metadata: Unsupported QAM modulation depth" << std::endl;
            throw 0;
        }
        break;
    case standardInterfaces::ModulationScheme::PAM:
        switch (metadata->modulation_scheme.M) {
        case 4:
            ConfigureModulationScheme(SigProc::PAM4);
            DEBUG(5, FrameAssembler_metadata, "Frame mod set to 4PAM");
            break;
        default:
            std::cerr << "FrameAssembler_metadata-metadata: Unsupported PAM modulation depth" << std::endl;
            throw 0;
        }
        break;
    case standardInterfaces::ModulationScheme::UNKNOWN:
    default:;
    }
}




void FrameAssembler_metadata_i::ProcessData()
{
    DEBUG(3, FrameAssembler_metadata, "ProcessData() invoked")

    PortTypes::ShortSequence I_out_header, Q_out_header;
    PortTypes::ShortSequence I_out_data, Q_out_data;
    PortTypes::ShortSequence I_out_0, Q_out_0;

    PortTypes::ShortSequence *I_in_0(NULL), *Q_in_0(NULL);
    unsigned int N_in(0);
    unsigned int j(0);
    unsigned int blockSize(512);

    // Prepare preamble phasing pattern, ramp up/down
    short * I_phasing_pattern = new short[512];
    short * Q_phasing_pattern = new short[512];
    PortTypes::ShortSequence I_out_phasing_pattern, Q_out_phasing_pattern;
    I_out_phasing_pattern.length(256);
    Q_out_phasing_pattern.length(256);
    PortTypes::ShortSequence I_out_ramp_up, Q_out_ramp_up;
    I_out_ramp_up.length(256);
    Q_out_ramp_up.length(256);
    PortTypes::ShortSequence I_out_ramp_down, Q_out_ramp_down;
    I_out_ramp_down.length(256);
    Q_out_ramp_down.length(256);

    AssemblePhasingPattern(I_phasing_pattern, Q_phasing_pattern);
    for (unsigned int i=0; i<256; i++) {
        I_out_phasing_pattern[i] = I_phasing_pattern[i];
        Q_out_phasing_pattern[i] = Q_phasing_pattern[i];
        
        I_out_ramp_up[i] = (short) (I_phasing_pattern[i] * (0.5 - 0.5*cos( PI*i/256 )));
        Q_out_ramp_up[i] = (short) (Q_phasing_pattern[i] * (0.5 - 0.5*cos( PI*i/256 )));
        
        I_out_ramp_down[i] = 1 - I_out_ramp_up[i];
        Q_out_ramp_down[i] = 1 - Q_out_ramp_up[i];
    }

    // control
    PortTypes::ShortSequence I_out_control, Q_out_control;
    I_out_control.length(1024);
    Q_out_control.length(1024);
    for (unsigned int i=0; i<256; i++) {
        I_out_control[i] = (i%2 == 0) ? -BPSK_LEVEL : BPSK_LEVEL;
        Q_out_control[i] = 0;
    }

    delete [] I_phasing_pattern;
    delete [] Q_phasing_pattern;


    short * I_header = new short[512];
    short * Q_header = new short[512];

    while( true ) {

        numFrameSymbolsAssembled = 0;

        while ( numFrameSymbolsAssembled < (frameSize-1) ) {
            // Get data from port
            //dataIn_0->getData(I_in_0, Q_in_0, metadata);
            dataIn_0->getData(I_in_0, Q_in_0);
            // Read input data length
            N_in = I_in_0->length();
            DEBUG(1, FrameAssembler_metadata, "got " << N_in << " samples");


/* HV
            if (metadata->app_id==0) {
                // control packet; configure
                DEBUG(1, FrameAssembler_metadata, "configuring end-of-frame control (app_id = 0)");
                if (N_in != 1024) {
                    std::cerr << "ERROR! FrameAssembler_metadata: control packet is wrong length!"
                              << std::endl;
                    throw 0;
                }

                for (unsigned int i=0; i<N_in; i++) {
                    I_out_control[i] = (*I_in_0)[i];
                    Q_out_control[i] = (*Q_in_0)[i];
                }
                dataIn_0->bufferEmptied();
                continue;
            }
*/

            if ( numFrameSymbolsAssembled==0 ) {
                // first packet
                ConfigureModulationScheme(SigProc::QPSK);
                
                /* HV
                ConfigureModulationSchemeOnMetaData();

                if (metadata->app_id==5) {
                    DEBUG(1, FrameAssembler_metadata, "configuring special control frame (app_id = 5)");
                    ConfigureFrameType(FRAME_TYPE_CONTROL);
                    // control frame
                    if (N_in != frameSize) {
                        std::cerr << "ERROR! FrameAssembler_metadata: control packet (app_id=5) is wrong length!"
                                  << std::endl;
                        throw 0;
                    }
                    num_frames_sent_app_5++;
                } else if (metadata->app_id==6) {
                    DEBUG(1, FrameAssembler_metadata, "configuring special beacon frame (app_id = 6)");
                    ConfigureFrameType(FRAME_TYPE_CONTROL);
                    // control frame
                    if (N_in != frameSize) {
                        std::cerr << "ERROR! FrameAssembler_metadata: control packet (app_id=6) is wrong length!"
                                  << std::endl;
                        throw 0;
                    }
                    num_frames_sent_app_6++;
                } else if (metadata->app_id==1) {
                    // audio data
                    ConfigureFrameType(FRAME_TYPE_DATA);
                    ///todo configure for BPSK
                    if (metadata->modulation_scheme.M == 4)
                        ConfigureFrameSize(8192);   // QPSK
                    else
                        ConfigureFrameSize(4096);   // 16-QAM
                    num_frames_sent_app_1++;
                } else if (metadata->app_id==2) {
                    // text data
                    ConfigureFrameType(FRAME_TYPE_DATA);
                    ConfigureFrameSize(512);
                    num_frames_sent_app_2++;
                } else {
                    std::cerr << "ERROR! FrameAssembler_metadata: Unknown app_id: "
                              << metadata->app_id << std::endl;
                    throw 0;
                }
                */
                
                 //HV audio data
                 ConfigureFrameType(FRAME_TYPE_DATA);
                ///todo configure for BPSK
                 //if (metadata->modulation_scheme.M == 4)
                   //     ConfigureFrameSize(8192);   // QPSK
                    //else
                        ConfigureFrameSize(4096);   // 16-QAM
                    num_frames_sent_app_1++;

                I_out_data.length(frameSize);
                Q_out_data.length(frameSize);
            }

            DEBUG(1, FrameAssembler_metadata, "Assembling " << N_in << " symbols (max: " << frameSize);

            for (unsigned int i=0; i<N_in; i++) {
                //
                I_out_data[numFrameSymbolsAssembled] = (*I_in_0)[i];
                Q_out_data[numFrameSymbolsAssembled] = (*Q_in_0)[i];

                numFrameSymbolsAssembled++;
            }

            // empty input buffer; release semaphore
            dataIn_0->bufferEmptied();

        }

        // ASSEMBLE_PREAMBLE
        DEBUG(4, FrameAssembler_metadata, "Assembling preamble")
 //HV       metadata->port_id = 0;

        // push ramp-up preamble phasing pattern
        DEBUG(7, FrameAssembler_metadata, "pushing ramp up phasing " << I_out_ramp_up.length() << " frame samples")
//        dataOut_0->pushPacket(I_out_ramp_up, Q_out_ramp_up, *metadata);
        dataOut_0->pushPacket(I_out_ramp_up, Q_out_ramp_up);

        // push preamble
        unsigned int phasing_pattern_length;
        /* HV
        if (metadata->app_id==6)
            phasing_pattern_length = 16;
        else if (metadata->app_id==1)
            phasing_pattern_length = 32;
        else if (metadata->app_id==0)
            phasing_pattern_length = 8;
        else if (metadata->app_id==5)
            phasing_pattern_length = 32;
        else
            phasing_pattern_length = 8;
            */
            
            phasing_pattern_length = 32;

        for (unsigned int i=0; i<phasing_pattern_length; i++) {
            DEBUG(7, FrameAssembler_metadata, "pushing phasing " << I_out_phasing_pattern.length() << " frame samples")
            //dataOut_0->pushPacket(I_out_phasing_pattern, Q_out_phasing_pattern, *metadata);
            dataOut_0->pushPacket(I_out_phasing_pattern, Q_out_phasing_pattern);
        }

        // ASSEMBLE_HEADER
        DEBUG(4, FrameAssembler_metadata, "Assembling header")
        // copy header
        I_out_header.length(512);
        Q_out_header.length(512);
        AssembleHeader( I_header, Q_header );
        for (unsigned int i=0; i<512; i++) {
            I_out_header[i] = I_header[i];
            Q_out_header[i] = Q_header[i];
        }

        // push data
        DEBUG(7, FrameAssembler_metadata, "pushing header " << I_out_header.length() << " frame samples")
        //dataOut_0->pushPacket(I_out_header, Q_out_header, *metadata);
        dataOut_0->pushPacket(I_out_header, Q_out_header);


        // ASSEMBLE_FRAME
        // break output into smaller pieces
        
        I_out_0.length(blockSize);
        Q_out_0.length(blockSize);

        j = 0;
        for ( unsigned int i=0; i<frameSize; i++ ) {
            I_out_0[j] = I_out_data[i];
            Q_out_0[j] = Q_out_data[i];
            j++;
            
            if ( j==blockSize || i==(frameSize-1) ) {
                I_out_0.length(j);
                Q_out_0.length(j);
                DEBUG(7, FrameAssembler_metadata, "pushing frame " << I_out_0.length() << " frame samples")
                //dataOut_0->pushPacket(I_out_0, Q_out_0, *metadata);
                dataOut_0->pushPacket(I_out_0, Q_out_0);
                j=0;
            }
        }

        // ASSEMBLE_EOM_CODE:
        // push phasing pattern
        ///\todo push control packet (BPSK)
        if (frameType != FRAME_TYPE_CONTROL) {
            // No need to push additional control information as internal frame data
            // are already control
            DEBUG(7, FrameAssembler_metadata, "pushing control " << I_out_control.length() << " frame samples")
      //HV      metadata->port_id = 1;//127;
            //dataOut_0->pushPacket(I_out_control, Q_out_control, *metadata);
      //HV no EOM      dataOut_0->pushPacket(I_out_control, Q_out_control);
      //HV      metadata->port_id = 0;
        }

        // push ramp-down phasing pattern
        DEBUG(7, FrameAssembler_metadata, "pushing ramp down phasing " << I_out_ramp_up.length() << " frame samples")
//        dataOut_0->pushPacket(I_out_ramp_down, Q_out_ramp_down, *metadata);
        dataOut_0->pushPacket(I_out_ramp_down, Q_out_ramp_down);

        if (ossieDebugLevel >= 4) {
            std::cout << "Frame count:  "
                      << "app_1(" << num_frames_sent_app_1 << ")  "
                      << "app_2(" << num_frames_sent_app_2 << ")  "
                      << "app_5(" << num_frames_sent_app_5 << ")  "
                      << "app_6(" << num_frames_sent_app_6 << ")  "
                      << "total: " << num_frames_sent_app_1+num_frames_sent_app_2
                      << std::endl;
        }
    }

    delete [] I_header;
    delete [] Q_header;
}