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

Copyright 2006 Virginia Polytechnic Institute and State University

This file is part of the OSSIE SymbolSync.

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

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


#include <iostream>

#include "SymbolSync.h"

//#define WRITEFILE

SymbolSync_i::SymbolSync_i(const char *uuid, omni_condition *con) : Resource_impl(uuid)
{
    previous_length = 0;

    dataIn = new standardInterfaces_i::complexShort_p("inData");
    dataOut = new standardInterfaces_i::complexShort_u("outData");

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

    // Interpolation factor
    M = 1;

    // Filter
    len_h = 1;
    h = new float[len_h];

    h[0] = 1.0;

    i_filter = new SigProc::fir_filter(h, len_h);
    q_filter = new SigProc::fir_filter(h, len_h);

}                                  
                                  

SymbolSync_i::~SymbolSync_i(void)
{
    delete dataIn;
    delete dataOut;

    delete in_data;
    delete out_data;
}

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

    CORBA::Object_var p;

    p = dataIn->getPort(portName);

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

    p = dataOut->getPort(portName);

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

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

void SymbolSync_i::start() throw (CORBA::SystemException, CF::Resource::StartError)
{
    std::cout << "SymbolSync start() called" << std::endl;
}

void SymbolSync_i::stop() throw (CORBA::SystemException, CF::Resource::StopError) 
{
    std::cout << "SymbolSync stop() called" << std::endl;
}

void SymbolSync_i::releaseObject() throw (CORBA::SystemException, CF::LifeCycle::ReleaseError)
{
    component_running->signal();
}

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

    out_data = new SigProc::dump_data("out_interp.dat", 0, 100000);

}

void SymbolSync_i::configure(const CF::Properties& props) throw (CORBA::SystemException, CF::PropertySet::InvalidConfiguration, CF::PropertySet::PartialConfiguration)
{
    std::cout << "SymbolSync 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;

        // InterpFactor property, sets the interpolation factor
        if (strcmp(props[i].id, "DCE:50827747-bd16-4e82-9b9f-7d6d7f9e83d0") == 0) {
            CORBA::UShort D;
            props[i].value >>= D;

	    omni_mutex_lock l(accessPrivateData);

            M = D;
            std::cout << "Symbol Spacing set to " << M << std::endl;
        }

        // filter property, Filter coefficients
        if (strcmp(props[i].id, "DCE:a516b1d9-7e9d-42ac-8519-50babf9f5be7") == 0) {
            CORBA::FloatSeq *coeff_ptr;
            props[i].value >>= coeff_ptr;

	    omni_mutex_lock l(accessPrivateData);

            len_h = coeff_ptr->length();

            delete []h;
            delete i_filter;
            delete q_filter;

            h = new float[len_h];
            std::cout << "SymbolSync filter length = " << len_h << std::endl;
            for (unsigned int i = 0; i < len_h; i++) {
                h[i] = (*coeff_ptr)[i];
                std::cout << "Coeff[" << i << "] = " << h[i] << std::endl;
            }
            i_filter = new SigProc::fir_filter(h, len_h);
            q_filter = new SigProc::fir_filter(h, len_h);

        }

    }



}

void SymbolSync_i::run_interpolation()
{
    std::cout << "SymbolSync run_interpolation() thread started" << std::endl;
    PortTypes::ShortSequence *I_in, *Q_in;
    PortTypes::ShortSequence I_out, Q_out;

    unsigned int Symbol_Spacing(M);
    unsigned int old_Symbol_Spacing(0);
    unsigned int old_len(0);
    unsigned int offset(0);
    long int energy(0), energymax(0);
    bool first_packet=true;	
    float Early_Late_Diff=0;
    float threshold=200;
    long int EarlyLateCount(0),bitsleft(0),current_location(0);
    short int Early(0),Late(0);


    while (1) {

	dataIn->getData(I_in, Q_in);
	{
	    omni_mutex_lock l(accessPrivateData);

	   Symbol_Spacing = M;
	}

        unsigned int len = I_in->length();
	unsigned int out_idx(0);

	if (len != old_len || Symbol_Spacing != Symbol_Spacing) {
	    std::cout << "data len or M changed." << std::endl;
	    I_out.length(int(len / Symbol_Spacing));
	    Q_out.length(int(len / Symbol_Spacing));
	    old_len = len;
	    old_Symbol_Spacing = Symbol_Spacing;
	}
	current_location=0;
        if (first_packet)
	{
	        std::cout << "SymbolSync Estimating tracking..." << std::endl;
		for (unsigned int i = 0; i < Symbol_Spacing; i++) 
		{
		   for (unsigned int j = 0; j<10; j++)
			{
	 	   		i_filter->do_work(true, (*I_in)[j*Symbol_Spacing+i], I_out[out_idx]);
	    	   //q_filter->do_work(true, (*Q_in)[i+offset], Q_out[out_idx]);
		   		out_idx++;
				energy+=abs(I_out[out_idx]);
			
				if (energy>energymax)
				{
					energymax=energy;
					energy=0;
					offset=i;
				}
			}
		out_idx=0;

        	}
			
		first_packet=false;
		current_location=offset;
	}

	
	std::cout << "SymbolSync Early Late tracking" << std::endl;
	current_location+=bitsleft;;
	Early_Late_Diff=0;
	while (current_location <len)
	{
	    if (current_location>0) i_filter->do_work(true, (*I_in)[current_location-1], Early);
	    i_filter->do_work(true, (*I_in)[current_location], I_out[out_idx]);
	    if ((current_location+1)<len) i_filter->do_work(true, (*I_in)[current_location+1], Late);
	    q_filter->do_work(true, (*Q_in)[current_location], Q_out[out_idx]);

	    //out_data->write_data(I_out[out_idx]);
		if (((current_location)>0)&&((current_location+1)<len))
		{
		EarlyLateCount++;
		
		
		Early_Late_Diff+=Late-Early;
			if (EarlyLateCount>15)
			{
				Early_Late_Diff=Early_Late_Diff/EarlyLateCount;
				//std::cout << "ELdifft="<<Early_Late_Diff <<std::endl;
				EarlyLateCount=0;
				if (Early_Late_Diff>threshold)
				{
				current_location+=Symbol_Spacing+1;
				}
				else if (Early_Late_Diff<-threshold)
				{
				current_location+=Symbol_Spacing-1;
				}
				else
				{
				current_location+=Symbol_Spacing;
				};

			}
			else
			{
			current_location+=Symbol_Spacing;
			};	
		

		}
		else
		current_location+=Symbol_Spacing;

	    out_idx++;
	    

	};
	bitsleft=current_location-len;
	std::cout << "bitsleft=" <<bitsleft<<"  offset="<<offset<<" current_location="<<current_location<<std::endl;
	
	dataIn->bufferEmptied();
	dataOut->pushPacket(I_out, Q_out);
    }
}