root/ossiedev/branches/drdepoy/freqmod.cpp @ 9888

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

Copyright 2009 Virginia Polytechnic Institute and State University

This file is part of the OSSIE freqmod.

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

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


#include <string>
#include <iostream>
#include "freqmod.h"
#include "math.h"
#include "ossie/debug.h"

freqmod_i::freqmod_i(const char *uuid, omni_condition *condition) : 
    Resource_impl(uuid), component_running(condition) 
{
    dataIn_0 = new standardInterfaces_i::complexShort_p("real_in");
    dataOut_0 = new standardInterfaces_i::complexShort_u("complex_out");

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

}

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

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

CORBA::Object_ptr freqmod_i::getPort( const char* portName ) throw (
    CORBA::SystemException, CF::PortSupplier::UnknownPort)
{
    DEBUG(3, freqmod, "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();

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

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

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

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

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

//OSSIE generated configure method
//void freqmod_i::configure(const CF::Properties& props)
//throw (CORBA::SystemException,
//    CF::PropertySet::InvalidConfiguration,
//    CF::PropertySet::PartialConfiguration)
//{
//    DEBUG(3, freqmod, "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:03ab4ef4-c56d-11de-a210-000c29451dde") == 0)
//        {
//              CORBA::Long simple_temp;
//              props[i].value >>= simple_temp;
//              freq_dev = simple_temp;
//
//        }
//        if (strcmp(props[i].id, "DCE:23f3457c-c56d-11de-a210-000c29451dde") == 0)
//        {
//              CORBA::Long simple_temp;
//              props[i].value >>= simple_temp;
//              samp_freq = simple_temp;
//
//        }
//    }
//}
void freqmod_i::query (CF::Properties & configProperties)
throw (CORBA::SystemException, CF::UnknownProperties)
{
    if (configProperties.length () == 0)
    {
        configProperties.length (propertySet.length ());
        for (unsigned int i = 0; i < propertySet.length (); i++)
        {
            configProperties[i].id = CORBA::string_dup (propertySet[i].id);
            configProperties[i].value = propertySet[i].value;
        }

        return ;
    }
    else {
        for (unsigned int i = 0; i < configProperties.length(); i++) {
            for (unsigned int j=0; j < propertySet.length(); j++) {
                if ( strcmp(configProperties[i].id, propertySet[i].id) == 0 ){
                    configProperties[i].value = propertySet[i].value;
                } 
            }
        }
    }
}

//Configure method written to work with WAVEDASH
void freqmod_i::configure(const CF::Properties& props)
throw (CORBA::SystemException,
    CF::PropertySet::InvalidConfiguration,
    CF::PropertySet::PartialConfiguration)
{
    static int init = 0;

    DEBUG(3, freqmod, "configure() invoked")

    if (init == 0){
        if ( props.length() <= 0 ){
            std::cout << "freqmod: configure called with invalid props.length() - " << props.length() << std::endl;
            return;
        }

        propertySet.length(props.length());
        for (unsigned int i=0; i < props.length(); i++) {
            propertySet[i].id = CORBA::string_dup(props[i].id);
            propertySet[i].value = props[i].value;
        }
        init++;
    }

    for (unsigned int i = 0; i <props.length(); i++) {

        if (strcmp(props[i].id, "DCE:03ab4ef4-c56d-11de-a210-000c29451dde") == 0) {
            // Frequency Deviation
            CORBA::Long simple_temp;
            props[i].value >>= simple_temp;
            // Test: setting propertySet[] to the given value. 
            freq_dev = simple_temp;
            for (unsigned int j=0; j < propertySet.length(); j++ ) {
                if ( strcmp(propertySet[j].id, props[i].id) == 0 ) {
                    propertySet[i].value = props[i].value;
                    break;
                }
            }
            DEBUG(1, ChannelDemo, "Setting Frequency Deviation to " << freq_dev);
        } else if (strcmp(props[i].id, "DCE:23f3457c-c56d-11de-a210-000c29451dde") == 0) {
            // Sampling Frequency
            CORBA::Long simple_temp;
            props[i].value >>= simple_temp;
            // Test: Setting propertySet[i] to the given value
            propertySet[i].value = props[i].value;
            samp_freq = simple_temp;
            DEBUG(1, ChannelDemo, "Setting Sampling Frequency to " << samp_freq << " degrees");
           
            for (unsigned int j=0; j < propertySet.length(); j++ ) {
                if ( strcmp(propertySet[j].id, props[i].id) == 0 ) {
                    propertySet[i].value = props[i].value;
                    break;
                }
            }

        } else if (strcmp(props[i].id, "port_list") == 0) {
            // Frequency Deviation
            CORBA::Short simple_temp;
            props[i].value >>= simple_temp;
            // Test: setting propertySet[] to the given value. 
            for (unsigned int j=0; j < propertySet.length(); j++ ) {
                if ( strcmp(propertySet[j].id, props[i].id) == 0 ) {
                    propertySet[i].value = props[i].value;
                    break;
                }
            }
            DEBUG(1, ChannelDemo, "Setting Frequency Deviation to " << freq_dev);
        } else {
            // unknown property
            std::cerr << "ERROR: freqmod::configure() unknown property \""
               << props[i].id << "\"" << std::endl;
            throw CF::PropertySet::InvalidConfiguration();
        }
    }
}


void freqmod_i::ProcessData()
{
    DEBUG(3, freqmod, "ProcessData() invoked")

    PortTypes::ShortSequence I_out_0, Q_out_0;

    //declare variables
    float pi = 3.1415927; 
    double theta=0;                     //current output angle
    double theta_prev=0;        //previous output angle
    
    
    PortTypes::ShortSequence *I_in_0(NULL), *Q_in_0(NULL);
    CORBA::UShort I_in_0_length, Q_in_0_length;

    while(1)
    {
        dataIn_0->getData(I_in_0, Q_in_0);

        I_in_0_length = I_in_0->length();
        Q_in_0_length = Q_in_0->length();

        I_out_0.length(I_in_0->length()); //must define length of output
        Q_out_0.length(Q_in_0->length()); //must define length of output

        /*insert code here to do work*/
        //These variables may be refreshed at the beginning of each itiration
        double sens;
        int j=0;
        
        //Define the frequency sensitivity in terms of the desired frequency deviation and sampling rate
        sens = (double)freq_dev/samp_freq;
       
        //generate a discrete approximation of an FM signal recursively
        for (unsigned int k=0; k < I_in_0->length(); k++)
        {
                //compute the ouput angle
                theta = theta_prev+(double)(*I_in_0)[k]/32768;
                theta_prev = theta;
                //compute the output
                I_out_0[k] = (short int)(.25*32768)*(double)cos((double)(2*pi*sens*theta));
                Q_out_0[k] = (short int)(.25*32768)*(double)sin((double)(2*pi*sens*theta));   
        }
 
        dataIn_0->bufferEmptied();
        dataOut_0->pushPacket(I_out_0, Q_out_0);
    }
}