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

Copyright 2005,2006 Virginia Polytechnic Institute and State University

This file is part of the OSSIE USRP_UHD Device.

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


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

#include <byteswap.h>
#include <iostream>
#include <vector>
#include <complex>

#include "ossie/cf.h"
#include "ossie/PortTypes.h"

#include "ossie/ossieSupport.h"
#include "ossie/debug.h"

#include "USRP_UHD.h"

USRP_UHD_i::USRP_UHD_i(char *id, char *label, char *profile) :
        Device_impl(id, label, profile),
        set_rx_packet_count(-1),
        rx_packet_count(1024),
        rx_packet_size(256),
        rx_data_size(2),
        number_of_channels(1),
        complex(true),
        rx_overruns(0),
        rx_active(false),
        tx_active(false),
	full_duplex(true),
	did_rx(false)
{
// Create USRP_UHD Control ports for TX and RX
    rx_control_port = new USRP_UHD_RX_Control_i(this, "U2_RX_Control", "DomainName1");
    tx_control_port = new USRP_UHD_TX_Control_i(this, "U2_TX_Control", "DomainName1");

// Create the ports for TX data
    tx_data_port = new standardInterfaces_i::complexShort_p("U2_TX_Data", "DomainName1");

// Create the ports for RX Data
    rx_data_1_port = new standardInterfaces_i::complexShort_u("U2_RX_Data_1", "DomainName1");
    rx_data_2_port = new standardInterfaces_i::complexShort_u("U2_RX_Data_2", "DomainName1");
}

void USRP_UHD_i::start() throw (CF::Resource::StartError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Start USRP_UHD called")
}

void USRP_UHD_i::stop() throw (CF::Resource::StopError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Stop USRP_UHD called")
    (rx_active)?std::cout<<"Active": std::cout<<"NOT Active";
}

CORBA::Object_ptr USRP_UHD_i::getPort(const char* portName)
throw(CF::PortSupplier::UnknownPort, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD getPort called with : " << portName)

    CORBA::Object_var p;

    if (!CORBA::is_nil(p = rx_control_port->getPort(portName)))
        return p._retn();
    else if (!CORBA::is_nil(p = rx_data_1_port->getPort(portName)))
        return p._retn();
    else if (!CORBA::is_nil(p = rx_data_1_port->getPort(portName))) {
        return p._retn();
    } else if (!CORBA::is_nil(p = tx_control_port->getPort(portName))) {
        return p._retn();
    } else if (!CORBA::is_nil(p = tx_data_port->getPort(portName))) {
        return p._retn();
    }
    std::cerr << "Couldn't find port " << portName << "Throwing exception" << std::endl;
    throw CF::PortSupplier::UnknownPort();
}

void USRP_UHD_i::initialize() throw (CF::LifeCycle::InitializeError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD Initialize called")
    std::string args("");
    DEBUG(1, USRP_UHD, "Creating the usrp device with: "<< args)
    sdev = uhd::usrp::multi_usrp::make(args);
    dev = sdev->get_device();
    sdev->set_rx_antenna("RX2");
    sdev->set_rx_antenna("TX/RX");
    DEBUG(1, USRP_UHD, "Using Device: "<< sdev->get_pp_string())
}

void USRP_UHD_i::configure(const CF::Properties &configProperties)
throw (CORBA::SystemException, CF::PropertySet::InvalidConfiguration,
       CF::PropertySet::PartialConfiguration)
{

}

void USRP_UHD_i::query(CF::Properties &configProperties)
throw (CORBA::SystemException, CF::UnknownProperties)
{

}


void USRP_UHD_i::releaseObject()
throw (CF::LifeCycle::ReleaseError, CORBA::SystemException)
{

    DEBUG(3, USRP_UHD, "USRP_UHD releaseObject called")
}


void USRP_UHD_i::rx_data_process()
{

///\TODO assumes one channel operation.  Set full_deplex to false to prevent Rx buffering during Tx
///Full duplex mode is still broken. we still cannot transmit and receive simultaneously using WBX board

    //tells the device to stream samples indefinitely
    uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);

    //Use with STREAM_MODE_NUM_SAMPS_AND_DONE/_MORE	
    //stream_cmd.num_samps = 65536;
    //stream_cmd.time_spec = uhd::time_spec_t((time_t)0);
    
    //When true, the device will begin streaming ASAP
    stream_cmd.stream_now = true;

    sdev->issue_stream_cmd(stream_cmd);

    uhd::stream_args_t stream_args("sc16");
    stream_args.args["noclear"] = "1";
    uhd::rx_streamer::sptr rx_stream = sdev->get_rx_stream(stream_args);

    PortTypes::ShortSequence I;
    PortTypes::ShortSequence Q;
    
///\TODO Make sure rx_packet_size is >= than USRP_UHD_MIN_RX_SAMPLES!!!!!
    rx_buff.resize(rx_packet_size);
    I.length(rx_packet_size);
    Q.length(rx_packet_size);

    DEBUG(3, USRP_UHD, "RX packet count " << rx_packet_count)

    while (rx_active && ((rx_packet_count == -1) || (rx_packet_count > 0))) {
		uhd::rx_metadata_t md;
	//if we are running half duplex mode we will need to tell start streaming at the beginning of the loop
		if (full_duplex == false && did_rx == false)  {
			uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
			sdev->issue_stream_cmd(stream_cmd);
			did_rx = true;
		}

        
//TODO get rid of num_rx_samps variable
        size_t num_rx_samps = rx_stream->recv( &rx_buff.front(), rx_packet_size, md);
	
        //Handle UHD error codes
        switch(md.error_code){
            case uhd::rx_metadata_t::ERROR_CODE_NONE:
                break;
            case uhd::rx_metadata_t::ERROR_CODE_TIMEOUT:
                DEBUG(3, USRP_UHD, "Got timeout before all samples received, possible packet loss")
                break;
            default:
                DEBUG(3, USRP_UHD, "Got error code 0x"<<md.error_code)
        }

		for (unsigned int i = 0; i < num_rx_samps; i++) {
			I[i] = (CORBA::Short) rx_buff[i].real();
			Q[i] = (CORBA::Short) rx_buff[i].imag();
		}
		rx_data_1_port->pushPacket(I, Q);

		if (rx_packet_count != -1)
				--rx_packet_count;
	}

    DEBUG(3, USRP_UHD, "Exiting rx_data_process thread")
    //tells the device to stop streaming samples
    stream_cmd = uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS;
    sdev->issue_stream_cmd(stream_cmd);
    rx_active = false;
    rx_thread->exit();
}



// USRP_UHD Expects data in little endian format
static inline short convertToLE(short s)
{
#ifdef __powerpc__
    s = bswap_16(s);
#endif

    return s;
}

void USRP_UHD_i::tx_data_process()
{
    PortTypes::ShortSequence *I_in, *Q_in;

    const unsigned int tx_buf_len(USRP_UHD_MAX_TX_SAMPLES);

    uhd::stream_args_t stream_args("sc16");
    stream_args.args["noclear"] = "1";
    uhd::tx_streamer::sptr tx_stream = sdev->get_tx_stream(stream_args);


    uhd::tx_metadata_t md3;

    //Set start and end of burst flags to help with packet fragmentation if needed
    //Host will send ACK packets when an EOB flag is received
    //SOB true -> first packet in sequence gets SOB flag
    //EOB true -> last packet in sequence gets EOB flag
    md3.start_of_burst = false;  
    md3.end_of_burst   = false;     

    //We can send packets in the future if needed
    //md.has_time_spec  = 0;     //no time
    //md.time_spec = uhd::time_spec_t((time_t)0 );

    std::vector< std::complex<short int> > tx_buff(tx_buf_len);

    DEBUG(3, USRP_UHD, "Starting tx_data_process thread.")
    
    tx_active = true; // Cleared to stop TX in USRP_UHD_TX_Control->stop()

    while (tx_active) {
		//if running half duplex mode we want to stop Rx streaming while transmitting
		if (full_duplex == false && did_rx == true)  {
				did_rx = false;
			uhd::rx_metadata_t md;
			size_t num_rx_samps = dev->recv( &rx_buff.front(), rx_packet_size, md,
												uhd::io_type_t::COMPLEX_INT16,
												uhd::device::RECV_MODE_FULL_BUFF);
			sdev->issue_stream_cmd(uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS);

		}


        tx_data_port->getData(I_in, Q_in);
        size_t samps_to_send = I_in->length();
        
        if(samps_to_send > tx_buf_len){
            std::cerr << "USRP_UHD Tx Buffer Overflow. Throwing Exception"<<std::endl;
            throw std::runtime_error("USRP_UHD buffer overflow");
        }

        for (unsigned int i = 0; i < samps_to_send; ++i) {
            tx_buff[i] = std::complex<short int> (convertToLE((*I_in)[i]),convertToLE((*Q_in)[i]));
        }

        //send the entire packet (driver fragments internally)
        size_t num_tx_samps = tx_stream->send( &tx_buff.front(), samps_to_send, md3);
        tx_data_port->bufferEmptied();
		
    }
    
    DEBUG(3, USRP_UHD, "Exiting tx_data_process thread.")
    tx_thread->exit();
}