root/ossiedev/branches/mcarrick/ossiedev-trunk/components/Decimator/Decimator.cpp @ 9125

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

Copyright 2005,2006 Virginia Polytechnic Institute and State University

This file is part of the OSSIE Decimator.

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


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

#include <iostream>

#include <string.h>
#include <math.h>  // For energy debugging routine
#include "ossie/cf.h"

#include "Decimator.h"

    //#define PRINT_ENERGY 1

Decimator_i::Decimator_i(const char *uuid, omni_condition *condition) :
    Resource_impl(uuid),
    component_running(condition),
    calculateFilterCoefficients(true),
    M(1),
    sample_count(0),
    previous_length(0){
    
    
    // Create the port for output data
    dataOut = new standardInterfaces_i::complexShort_u("outData");

    // Create port for input Data
    dataIn = new standardInterfaces_i::complexShort_p("inData");

    // Start the run_decimation thread
    processing_thread = new omni_thread(do_run_decimation, (void *) this);
    processing_thread->start();

    filter_type=FIR;
    // Filter length is L
    len_h = 1;
    h = new float[len_h];

    h[0] = 1.0;

    fir_i_filter = new SigProc::fir_filter(h, len_h);
    fir_q_filter = new SigProc::fir_filter(h, len_h);
    iir_i_filter = NULL;
    iir_q_filter = NULL;

    //outFile = new std::ofstream("decimator_out.dat");
}

void Decimator_i::start() throw (CF::Resource::StartError, CORBA::SystemException)

{
    std::cout << "Start Decimator called" << std::endl;

}

void Decimator_i::stop() throw (CF::Resource::StopError, CORBA::SystemException)

{
    std::cout << "Stop decimator called" << std::endl;

}

CORBA::Object_ptr Decimator_i::getPort(const char* portName) throw(CF::PortSupplier::UnknownPort, CORBA::SystemException)

{
    std::cout << "Decimator getPort called with : " << portName << std::endl;

    CORBA::Object_var p;

    p = dataOut->getPort(portName);

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

    p = dataIn->getPort(portName);

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

    throw CF::PortSupplier::UnknownPort();
}

void Decimator_i::initialize() throw (CF::LifeCycle::InitializeError, CORBA::SystemException)

{
    std::cout << "Decimator Initialize called" << std::endl;

}

void Decimator_i::configure(const CF::Properties& props) throw (CORBA::SystemException, CF::PropertySet::InvalidConfiguration, CF::PropertySet::PartialConfiguration)

{
    std::cout << "decimator Configure called" << std::endl;

    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;
        
        // DecimateBy property, sets the decimator factor
        if (strcmp(props[i].id, "DCE:cea26b54-9d86-4b68-a761-14186efa9415") == 0) {
            CORBA::UShort D;
            props[i].value >>= D;
            M = D;
            std::cout << "Decimation factor set to " << M << std::endl;

        } else if (strcmp(props[i].id, "DCE:134e5dd8-c773-47af-a557-2837076358c4") == 0) {
            // filter property, Filter coefficients
            CORBA::FloatSeq *coeff_ptr;
            props[i].value >>= coeff_ptr;

            len_h = coeff_ptr->length();
            
            if (len_h < 2) {
                calculateFilterCoefficients = true;
            } else {
                calculateFilterCoefficients = false;

                delete []h;
                delete_filter_variables();

                h = new float[len_h];
                std::cout << "Decimator filter length = " << len_h << std::endl;
                for (unsigned int k = 0; k < len_h; k++) {
                    h[k] = (*coeff_ptr)[k];
                    DEBUG(3, Decimator, "Coeff[" << k << "] = " << h[k])
                }
                fir_i_filter = new SigProc::fir_filter(h, len_h);
                fir_q_filter = new SigProc::fir_filter(h, len_h);
            }
        } else if (strcmp(props[i].id, "DCE:537a42d9-2dd7-45b8-8dfb-f0999115ba71") == 0) {
           const char * prop_str;
            props[i].value >>= prop_str;
            if ( strcmp(prop_str, "FIR") == 0 ) {
               filter_type=FIR;
               DEBUG(3, Decimator, "Filtertype set to FIR")
            } else if ( strcmp(prop_str, "IIR") == 0 ){
               filter_type=IIR;
               DEBUG(3, Decimator, "Filtertype set to IIR")
            } else {
              std::cerr << "ERROR: Decimator::configure() unknown filter type" << std::endl;
               throw CF::PropertySet::InvalidConfiguration();
            };
        } else {
            std::cerr << "ERROR: Decimator::configure(): Unknown property "
                      << props[i].id << std::endl;
            throw CF::PropertySet::InvalidConfiguration();
        }
        
    }
    
         
         if (filter_type==FIR) {
         
                        if (calculateFilterCoefficients) {
                        // calculate filter coefficients dynamically
                        unsigned int m(2);      // filter delay (symbols)
                        float beta(0.5f);       // excess bandwidth factor
                        len_h = 2*2*M*m+1;      // filter length (samples)
                        delete [] h;
                        h = new float[len_h];
                        SigProc::DesignRRCFilter(2*M, m, beta, h);

                        // delete old filters
                        delete_filter_variables();

                        // print coefficients
                        for (unsigned int k=0; k<len_h; k++)
                            DEBUG(2, Decimator, " h[" << k << "] = " << h[k]);

                        fir_i_filter = new SigProc::fir_filter(h, len_h);
                        fir_q_filter = new SigProc::fir_filter(h, len_h);
                        }
                
         } else {
         
         
                        
                        float *b_f;                ///< feedforward filter coefficients
                        float *a_f;                ///< feedback filter coefficients
                        unsigned int f_order;      ///< filter order
                        // 10th-order Butterworth IIR filter, wc = (1/M)*pi rad,
                        f_order = 10;
                        b_f = new float[f_order+1];
                        a_f = new float[f_order+1];
                        // Delete old filters
                        delete_filter_variables();
                        SigProc::design_butter_lowpass_filter(f_order, 1.0/M, b_f, a_f);
                        iir_i_filter = new SigProc::iir_filter(a_f, f_order+1, b_f, f_order+1);
                        iir_q_filter = new SigProc::iir_filter(a_f, f_order+1, b_f, f_order+1);
                        DEBUG(3, Decimator, "IIR Filter generated")
                        delete b_f;
                        delete a_f;
         
         }

}
void Decimator_i::releaseObject() throw (CF::LifeCycle::ReleaseError, CORBA::SystemException)

{

    std::cout << "decimator releaseObject called" << std::endl;

    delete outFile;

    // Clear the component running semaphore so main shuts down everything
    component_running->signal();
}

void Decimator_i::run_decimation()
{
    std::cout << "run_decimation thread started" << std::endl;

/**
#ifdef LOG_DATA
    dump_data pre_decim("pre.dat", 1000, 1000);
    dump_data post_decim("post.dat", 1000, 1000);
#endif
**/
    
    unsigned int len=1024;
    unsigned int len_out=1024;
    PortTypes::ShortSequence I_out, Q_out;
    short * I_buf = new short[len];
    short * Q_buf = new short[len];
    bool output_sample(false);

    while (1) {
        PortTypes::ShortSequence *I_in(NULL), *Q_in(NULL);


        dataIn->getData(I_in, Q_in);

         len=I_in->length();
        
        DEBUG(5, Decimator, "Received "<<len<<" samples")

        if (len != previous_length) {
            len_out=(unsigned int)(1.0*len/M);
            I_out.length(len_out);
            Q_out.length(len_out);
            delete [] I_buf;
            delete [] Q_buf;
            I_buf = new short[len];
            Q_buf = new short[len];
            previous_length=len;
        }

//        I_out.length(len_out);
//        Q_out.length(len_out);

/**
#ifdef PRINT_ENERGY
        float E_in = 0, E_out = 0;
#endif
**/

if (filter_type==FIR) { 
        for (unsigned int i = 0; i < len; ++i) {

           

          
            
                sample_count = (++sample_count) % M;
                if (sample_count == 0) output_sample = true;
                fir_i_filter->do_work(output_sample, (*I_in)[i], I_buf[i]);
                fir_q_filter->do_work(output_sample, (*Q_in)[i], Q_buf[i] );

/**
#ifdef LOG_DATA
            pre_decim.write_data((float)(*I_in)[i], (float)(*Q_in)[i]);
#endif

#ifdef PRINT_ENERGY
            E_in  += (*I_in)[i] * (*I_in)[i] + (*Q_in)[i] * (*Q_in)[i];
            E_out += i_out * i_out + q_out * q_out;
#endif
**/
        }
        } else {
                for (unsigned int i = 0; i < len; ++i) {
             iir_i_filter->do_work( (*I_in)[i], I_buf[i]);
             iir_q_filter->do_work( (*Q_in)[i], Q_buf[i]);
             }
            
        }
        
        for (unsigned int i=0; i<len_out; i++) {
            I_out[i] = I_buf[M*i];
            Q_out[i] = Q_buf[M*i];
            /**
            #ifdef LOG_DATA
                 post_decim.write_data((float)I_buf[M*i], (float)q_buf[M*i]);
            #endif
            **/
        }

        DEBUG(5, Decimator, "Out "<<len_out<<" samples")
        dataIn->bufferEmptied();
        dataOut->pushPacket(I_out, Q_out);
/**
#ifdef PRINT_ENERGY
        std::cout << "Energy in = " << 10 * log10(E_in/decimator->I_in.length() + 0.01) << "  Energy out = " << 10 * log10(E_out/len_out + 0.01) << std::endl;
#endif
**/     
    }

}

short Decimator_i::f2s(float r)
{
    if (r > SHRT_MAX)
        return SHRT_MAX;

    if (r < SHRT_MIN)
        return SHRT_MIN;

    short ret = (short) r;

    return ret;
}

void Decimator_i::delete_filter_variables()
{
DEBUG(5, Decimator, "Enter delete_filter_variables")
        if (fir_i_filter!=NULL){
               delete fir_i_filter;
               fir_i_filter=NULL;
        };
        
        if (fir_q_filter!=NULL){
               delete fir_q_filter;
               fir_q_filter=NULL;
        };

        if (iir_i_filter!=NULL){
               delete iir_i_filter;
               iir_i_filter=NULL;
        };
        
        if (iir_q_filter!=NULL){
               delete iir_q_filter;
               iir_q_filter=NULL;
        };       

DEBUG(5, Decimator, "Exit delete_filter_variables")
}