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

Copyright 2008 Virginia Polytechnic Institute and State University

This file is part of the OSSIE DataStreamerSink.

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

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


#include "DataStreamerSink.h"

DataStreamerSink_i::DataStreamerSink_i(const char *uuid, omni_condition *condition) : 
    Resource_impl(uuid), component_running(condition) 
{
    samplesOut = new standardInterfaces_i::complexShort_u("samples_out");
    dataIn = new standardInterfaces_i::complexShort_p("data_in");

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

}

DataStreamerSink_i::~DataStreamerSink_i(void)
{   
    delete samplesOut;
    delete dataIn;
}

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

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

    p = samplesOut->getPort(portName);

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

    p = dataIn->getPort(portName);

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

    /*exception*/
    throw CF::PortSupplier::UnknownPort();
}

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

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

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

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

void DataStreamerSink_i::configure(const CF::Properties& props)
throw (CORBA::SystemException,
    CF::PropertySet::InvalidConfiguration,
    CF::PropertySet::PartialConfiguration)
{
    DEBUG(3, DataStreamerSink, "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:d2f9ae84-17b4-11dd-a212-00123f63025f") == 0) {
            // modulation scheme
        } else if (strcmp(props[i].id, "DCE:d704c6ee-17b4-11dd-a1ba-00123f63025f") == 0) {
            // modulation depth
        } else {
            // unknown property
            std::cerr << "ERROR: DataStreamerSink::configure() unknown property \""
               << props[i].id << "\"" << std::endl;
            throw CF::PropertySet::InvalidConfiguration();
        }
    }
}

void DataStreamerSink_i::ProcessData()
{
    DEBUG(3, DataStreamerSink, "ProcessData() invoked")

    PortTypes::ShortSequence I_out, Q_out;

    PortTypes::ShortSequence *I_in(NULL), *Q_in(NULL);
    CORBA::UShort I_in_length, Q_in_length;

    // bookkeeping variables
    unsigned int i;
    float I1, Q1, I2, Q2;
    float q, q_hat;
    float * ptr_i, * ptr_q;

    // objects

    // operational mode
    enum {SEEK_HEADER, EXTRACT_HEADER, EXTRACT_FRAME} op_mode = SEEK_HEADER;

    // AGC
    sigprocc_agc agc;
    sigprocc_agc_init(&agc);
    sigprocc_agc_set_target(&agc, 1.0f);
    sigprocc_agc_set_bandwidth(&agc, 1e-3f);

    // Costas loop filter, NCO
    sigprocc_lf2 costas_loop_filter;
    sigprocc_lf2_init(&costas_loop_filter);
    sigprocc_lf2_set_bandwidth(&costas_loop_filter, 0.025f);

    sigprocc_nco costas_loop_nco;
    sigprocc_nco_set_frequency(&costas_loop_nco, 0.0f);
    sigprocc_nco_set_phase(&costas_loop_nco, 0.0f);

    // Symbol timing recovery, MF, dMF, buffers
    bool enable_symbol_output = false;
    unsigned int k=4;
    unsigned int m=5;
    float beta = 0.35f;
    unsigned int Npfb=16;
    polyphase_filterbank* mf = polyphase_filterbank_create_prototype(
        PULSE_SHAPE_RRCOS, false, k, m, beta, Npfb);
    polyphase_filterbank* dmf = polyphase_filterbank_create_prototype(
        PULSE_SHAPE_RRCOS, true, k, m, beta, Npfb);
    unsigned int h_len = 2*k*m;
    circular_buffer * buf_i = circular_buffer_create(h_len);
    circular_buffer * buf_q = circular_buffer_create(h_len);
    float mf_i;     // matched filter input (in-phase)
    float mf_q;     // matched filter output (quadrature)
    float dmf_i;    // derivative MF output (in-phase)
    float dmf_q;    // derivative MF output (quadrature)
    unsigned int b=0;   // filter bank select
    unsigned int v=0;

    // Phase recovery modem, filter, NCO
    modem * frame_sync_demod = modem_create(MOD_BPSK, 1);

    sigprocc_lf2 pll_filter;
    sigprocc_lf2_init(&pll_filter);
    sigprocc_lf2_set_bandwidth(&pll_filter, 0.025f);

    sigprocc_nco pll_nco;
    sigprocc_nco_set_frequency(&pll_nco, 0.0f);
    sigprocc_nco_set_phase(&pll_nco, 0.0f);

    // Frame header PN sequence
    unsigned int s;
    msequence frame_sync_code = msequence_default[8];
    binary_sequence * frame_sync_code_sequence;
    frame_sync_code_sequence = binary_sequence_create(frame_sync_code.n);
    binary_sequence_init_msequence(frame_sync_code_sequence, &frame_sync_code);

    int frame_sync_code_threshold = frame_sync_code.n / 2;

    binary_sequence * frame_header_buffer;
    frame_header_buffer = binary_sequence_create(frame_sync_code.n);
    int rxy;

    // Frame data
    unsigned int num_symbols_extracted = 0;
    unsigned int frame_length = 256;
    modem * demodulator = modem_create(MOD_QAM, 4);
    unsigned int num_bit_errors = 0;
    unsigned int s_gen;
    msequence data_source = msequence_default[12];

    while (true) {
        dataIn->getData(I_in, Q_in);

        I_in_length = I_in->length();
        Q_in_length = Q_in->length();

        // define length of output
        I_out.length(frame_length);
        Q_out.length(frame_length);

        printf("DataStreamerSink got %u samples\n", I_in_length);

        for (i=0; i<I_in_length; i++) {
            I1 = (float) (*I_in)[i];
            Q1 = (float) (*Q_in)[i];

            I1 /= 4e3f;
            Q1 /= 4e3f;

            //------------------------------------------------
            // AGC
            //------------------------------------------------
#if 1
            sigprocc_agc_apply_gain(&agc, I1, Q1, &I2, &Q2);

            // Costas Loop (BPSK header)
            sigprocc_nco_mix_down(&costas_loop_nco, I2, Q2, &I1, &Q1);
            if (op_mode != EXTRACT_FRAME) {
                // demodulate
                demodulate(frame_sync_demod, I1, Q1, &s);

                // compute phase error
                get_demodulator_phase_error(frame_sync_demod, &q);

                // filter phase error
                sigprocc_lf2_advance(&costas_loop_filter, q, &q_hat);

                // adjust NCO frequency
                sigprocc_nco_set_frequency(&costas_loop_nco, q_hat);

                // update NCO
                sigprocc_nco_step(&costas_loop_nco);
            }
#else
            I2 = I1;
            Q2 = Q1;
#endif
            // push values onto buffer
            circular_buffer_push_front(buf_i, I2);
            circular_buffer_push_front(buf_q, Q2);

            //------------------------------------------------
            // Symbol timing recovery
            //------------------------------------------------
            v++;
            if (v==k) {
                enable_symbol_output = true;
                v=0;
            } else {
                enable_symbol_output = false;
            }

            if (enable_symbol_output) {
                // compute (derivative) matched filter outputs
                ptr_i = circular_buffer_getpointer(buf_i);
                ptr_q = circular_buffer_getpointer(buf_q);
                mf_i  = polyphase_filterbank_compute_output(mf, b, ptr_i) / k;
                mf_q  = polyphase_filterbank_compute_output(mf, b, ptr_q) / k;
                dmf_i = polyphase_filterbank_compute_output(dmf, b, ptr_i) / k;
                dmf_q = polyphase_filterbank_compute_output(dmf, b, ptr_q) / k;

                switch (op_mode) {
                    case SEEK_HEADER:

                        if (mf_i > 0) binary_sequence_push(frame_header_buffer, 1);
                        else          binary_sequence_push(frame_header_buffer, 0);

                        // correlate with header pn sequence
                        rxy = binary_sequence_correlate(
                                frame_header_buffer, frame_sync_code_sequence);

                        if (abs(rxy) > frame_sync_code_threshold) {
                            printf("HEADER FOUND! rxy=%f\n",
                                    (float)rxy / (float)(frame_sync_code.n));
                            printf("mf_i: %f, mf_q: %f\n", mf_i, mf_q);

                            op_mode = EXTRACT_HEADER;

                            num_symbols_extracted = 0;
                            num_bit_errors = 0;

                            // reset data source generator
                            msequence_reset(&data_source);

                            if (rxy < 0) {
                                printf("  >> INVERTED HEADER\n");
                                // todo: adjust costas_loop_nco by pi
                            }
                        }

                        break;
                    case EXTRACT_HEADER:
                        op_mode = EXTRACT_FRAME;
                        
                    case EXTRACT_FRAME:
                        I_out[num_symbols_extracted] = (short) (mf_i * 10000);
                        Q_out[num_symbols_extracted] = (short) (mf_q * 10000);

                        // demodulate and count bit errors
                        demodulate(demodulator, mf_i, mf_q, &s);
                        s_gen = msequence_generate_symbol(&data_source, demodulator->m);
                        num_bit_errors += count_bit_errors(s, s_gen);

                        num_symbols_extracted++;

                        if (num_symbols_extracted==frame_length) {
                            printf("  FRAME EXTRACTED\n");
                            op_mode = SEEK_HEADER;
                            samplesOut->pushPacket(I_out, Q_out);
                            // print bit errors:
                            printf("  bit errors: %u / %u\n\n",
                                num_bit_errors, frame_length*(demodulator->m));
                        }
                        break;
                    default:
                        printf("unexpected operational mode\n");
                        throw (0);
                }

                //------------------------------------------------
                // Phase recovery (demodulation)
                //------------------------------------------------

                //------------------------------------------------
                // Frame header detect, extract
                //------------------------------------------------

                //------------------------------------------------
                // Count bit errors
                //------------------------------------------------
            }
        }

        dataIn->bufferEmptied();
    }

    // free up the memory
    free_binary_sequence(frame_sync_code_sequence);
    free_binary_sequence(frame_header_buffer);
    free_polyphase_filterbank(mf);
    free_polyphase_filterbank(dmf);
}