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

Copyright 2005, 2006, 2007 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 <iostream>
#include <omniORB4/CORBA.h>
#include "ossie/debug.h"
#include "USRP_UHD.h"

USRP_UHD_TX_Control_i::USRP_UHD_TX_Control_i(USRP_UHD_i *_usrp_uhd, const char* _name, const char* _domain) : standardInterfaces_i::TX_Control_p(_name, _domain), usrp_uhd(_usrp_uhd)
{
    DEBUG(3, USRP_UHD, "TX Control port constructor called")
}

void USRP_UHD_TX_Control_i::set_number_of_channels(CORBA::ULong nchan)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);
    uhd::property_tree::sptr tree = usrp_uhd->dev->get_tree();
	uhd::fs_path db_path, mb_path = "/mboards";

	int assigned_channel = 0;
	std::string subdev_spec;
	for(int i=0;(i<usrp_uhd->mb_count)&&(assigned_channel<nchan);i++){
		mb_path = "/mboards";
		mb_path = mb_path / tree->list("/mboards")[i];

		for(int j=0;(j<usrp_uhd->db_count)&&(assigned_channel<nchan);j++){
			db_path = mb_path / "dboards" / tree->list( mb_path / "dboards")[j];
			uhd::usrp::dboard_eeprom_t prom = tree->access<uhd::usrp::dboard_eeprom_t>(db_path / "tx_eeprom").get();
			if (prom.id != uhd::usrp::dboard_id_t::none()){
				subdev_spec = subdev_spec + tree->list( mb_path / "dboards")[j] + ":"
						+ tree->list(db_path / "tx_frontends").at(0) + " ";
				assigned_channel++;

			}


		}
		DEBUG(3, USRP_UHD, "subdev of tx channel@MB"<<i<<" = "<<subdev_spec)
		usrp_uhd->sdev->set_tx_subdev_spec(uhd::usrp::subdev_spec_t(subdev_spec),i);

	}
	usrp_uhd->tx_channels = usrp_uhd->sdev->get_tx_num_channels();
	DEBUG(3, USRP_UHD, "# of Tx Channels Requested="<<nchan<<", Real="<<usrp_uhd->tx_channels)
}

void USRP_UHD_TX_Control_i::get_number_of_channels(CORBA::ULong &num)
{
///\TODO: Define if we need this interface. Number of channels is defined at transmit or receive
	omni_mutex_lock l(usrp_uhd->tx_control_access);
	num = usrp_uhd->sdev->get_tx_num_channels();
	DEBUG(3, USRP_UHD, "Tx get number of channels = "<<num)
}

void USRP_UHD_TX_Control_i::get_gain_range(CORBA::ULong channel, CORBA::Float &gmin, CORBA::Float &gmax, CORBA::Float &gstep)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);
	if (usrp_uhd->sdev) {
		uhd::gain_range_t range = usrp_uhd->sdev->get_tx_gain_range(channel);
		// All values in range are in dB
		gmin = range.start();
		gmax = range.stop();
		gstep = range.step();
		DEBUG(3, USRP_UHD, "Tx gain range = "<<range.to_pp_string())
	}
	else{

	}
}

void USRP_UHD_TX_Control_i::set_gain(CORBA::ULong channel, CORBA::Float gain)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);

	if (usrp_uhd->sdev) {
		usrp_uhd->sdev->set_tx_gain(gain,channel);
		DEBUG(3, USRP_UHD, "Tx set gain = "<<gain)
	}
	else {

	}
}

void USRP_UHD_TX_Control_i::get_gain(CORBA::ULong channel, CORBA::Float &gain)
{
    omni_mutex_lock l(usrp_uhd->tx_control_access);
    if (usrp_uhd->sdev) {
		gain = usrp_uhd->sdev->get_tx_gain(channel);
		DEBUG(3, USRP_UHD, "Tx get gain = "<<gain);
    }
    else{

    }

}

void USRP_UHD_TX_Control_i::get_frequency_range(CORBA::ULong channel, CORBA::Float &fmin, CORBA::Float &fmax, CORBA::Float &fstep)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);

	if (usrp_uhd->sdev) {
		uhd::freq_range_t range = usrp_uhd->sdev->get_tx_freq_range(channel);
		// All values in range are in dB
		fmin = range.start();
		fmax = range.stop();
		fstep = range.step();
		DEBUG(3, USRP_UHD, "Tx Frequency range = "<<range.to_pp_string())

	}
	else{

	}
            
}

void USRP_UHD_TX_Control_i::set_frequency(CORBA::ULong channel, CORBA::Float f)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);

    if (usrp_uhd->sdev) {
        uhd::tune_result_t result = usrp_uhd->sdev->set_tx_freq(f,channel);

        DEBUG(3, USRP_UHD, "USRP_UHD TX tune_result:" <<std::endl<<
              " \tTarget_RF_Freq=" << result.target_rf_freq <<std::endl<<
              " \tActual_RF_Freq=" << result.actual_rf_freq <<std::endl<<
              " \tTarget_DSP_freq="   << result.target_dsp_freq <<std::endl<<
              " \tActual_DSP_freq="   << result.actual_dsp_freq);
    } else {

    }

}

void USRP_UHD_TX_Control_i::get_frequency(CORBA::ULong channel, CORBA::Float &f)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);
	if(usrp_uhd->sdev){
		f = usrp_uhd->sdev->get_tx_freq(channel);
	}
	else{

	}
}

void USRP_UHD_TX_Control_i::start(CORBA::ULong channel)
{
    omni_mutex_lock l(usrp_uhd->tx_control_access);

    if (!usrp_uhd->tx_active) {
        usrp_uhd->tx_thread = new omni_thread(USRP_UHD_i::do_tx_data_process, ((void *)usrp_uhd));
        usrp_uhd->tx_thread->start();
    }
}

void USRP_UHD_TX_Control_i::stop(CORBA::ULong channel)
{
    omni_mutex_lock l(usrp_uhd->tx_control_access);

    usrp_uhd->tx_active = false;

}

void USRP_UHD_TX_Control_i::set_values(const CF::Properties &values)
{

    DEBUG(3, USRP_UHD, "TX set_values: " << values.length() << " values, value[0].id " << values[0].id)

    for (unsigned int i =0; i < values.length(); ++i) {
        if (strcmp(values[i].id, "SET_MUX") == 0 ) {
            CORBA::ULong mux;
            values[i].value >>= mux;
            DEBUG(1, USRP_UHD, "Request to set transmit mux to  " << mux<<" Not Supported")
            //omni_mutex_lock l(usrp_uhd->tx_control_access);
            //usrp_uhd->usrp_uhd_ptr->set_mux(mux);
        } else if (strcmp(values[i].id, "SET_AUTO_TR_1") == 0) {
            CORBA::ULong atx;
            values[i].value >>= atx;
            DEBUG(3, USRP_UHD, "Set Auto TX/RX for DB1 to " << atx)
            omni_mutex_lock l(usrp_uhd->tx_control_access);
			usrp_uhd->sdev->get_tx_dboard_iface(0)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_TX,atx,0x0);
			usrp_uhd->sdev->get_tx_dboard_iface(0)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_RX,atx,0x0);
			usrp_uhd->sdev->get_rx_dboard_iface(0)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_TX,atx,0x0);
			usrp_uhd->sdev->get_rx_dboard_iface(0)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_RX,atx,0x0);

        } else if (strcmp(values[i].id, "SET_AUTO_TR_2") == 0) {
			CORBA::ULong atx;
			values[i].value >>= atx;
			DEBUG(3, USRP_UHD, "Set Auto TX/RX for DB2 to " << atx)
			omni_mutex_lock l(usrp_uhd->tx_control_access);
			usrp_uhd->sdev->get_tx_dboard_iface(1)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_TX,atx,0x0);
			usrp_uhd->sdev->get_tx_dboard_iface(1)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_RX,atx,0x0);
			usrp_uhd->sdev->get_rx_dboard_iface(1)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_TX,atx,0x0);
			usrp_uhd->sdev->get_rx_dboard_iface(1)->set_pin_ctrl(uhd::usrp::dboard_iface::UNIT_RX,atx,0x0);

        }
    }

}

void USRP_UHD_TX_Control_i::set_interpolation_rate(CORBA::ULong channel, CORBA::ULong I)
{
    omni_mutex_lock l(usrp_uhd->tx_control_access);
    ///\TODO: rate matching in UHD works a little different. Review
    if(usrp_uhd->sdev){
		double master_clock = usrp_uhd->sdev->get_master_clock_rate();
		double request_da_rate = master_clock/(double)I;
		usrp_uhd->sdev->set_tx_rate(request_da_rate,channel);
		double real_da_rate = usrp_uhd->sdev->get_tx_rate(channel);
		DEBUG(3, USRP_UHD, "Requested Tx sample rate ="<<request_da_rate<<", Real Tx sample rate ="<<real_da_rate)
    }
    else{

    }
}

void USRP_UHD_TX_Control_i::get_interpolation_range(CORBA::ULong channel, CORBA::ULong &imin, CORBA::ULong &imax, CORBA::ULong &istep)
{
	omni_mutex_lock l(usrp_uhd->tx_control_access);
	if(usrp_uhd->sdev){
		double master_clock = usrp_uhd->sdev->get_master_clock_rate();
		uhd::meta_range_t tx_range = usrp_uhd->sdev->get_tx_rates(channel);
		imax = master_clock/tx_range.start();
		imin = master_clock/tx_range.stop();
		istep = master_clock*tx_range.step()/(tx_range.stop()*tx_range.start());
		if(istep < 1)
			istep = 1;
		double step = master_clock*tx_range.step()/(tx_range.stop()*tx_range.start());
		DEBUG(3, USRP_UHD, "Interpolation min="<<imin<<", max="<<imax<<", step="<<istep)
	}
	else{

	}
}

USRP_UHD_RX_Control_i::USRP_UHD_RX_Control_i(USRP_UHD_i *_usrp_uhd, const char* _name, const char* _domain) : standardInterfaces_i::RX_Control_p(_name, _domain), usrp_uhd(_usrp_uhd)
{
    DEBUG(3, USRP_UHD, "RX Control port constructor called")
}

void USRP_UHD_RX_Control_i::set_number_of_channels(CORBA::ULong nchan)
{
	omni_mutex_lock l(usrp_uhd->rx_control_access);
    uhd::property_tree::sptr tree = usrp_uhd->dev->get_tree();
	uhd::fs_path db_path, mb_path = "/mboards";

	int assigned_channel = 0;
	std::string subdev_spec;
	for(int i=0;(i<usrp_uhd->mb_count)&&(assigned_channel<nchan);i++){
		mb_path = "/mboards";
		mb_path = mb_path / tree->list("/mboards")[i];

		for(int j=0;(j<usrp_uhd->db_count)&&(assigned_channel<nchan);j++){
			db_path = mb_path / "dboards" / tree->list( mb_path / "dboards")[j];
			uhd::usrp::dboard_eeprom_t prom = tree->access<uhd::usrp::dboard_eeprom_t>(db_path / "rx_eeprom").get();
			if (prom.id != uhd::usrp::dboard_id_t::none()){
				subdev_spec = subdev_spec + tree->list( mb_path / "dboards")[j] + ":"
						+ tree->list(db_path / "rx_frontends").at(0) + " ";
				assigned_channel++;

			}


		}
		DEBUG(3, USRP_UHD, "subdev of rx channel@MB"<<i<<" = "<<subdev_spec)
		usrp_uhd->sdev->set_rx_subdev_spec(uhd::usrp::subdev_spec_t(subdev_spec),i);

	}
	usrp_uhd->rx_channels = usrp_uhd->sdev->get_rx_num_channels();
	DEBUG(3, USRP_UHD, "# of Rx Channels Requested="<<nchan<<", Real="<<usrp_uhd->rx_channels)
}

void USRP_UHD_RX_Control_i::get_number_of_channels(CORBA::ULong &num)
{
	omni_mutex_lock l(usrp_uhd->rx_control_access);
	num = usrp_uhd->sdev->get_rx_num_channels();
	DEBUG(3, USRP_UHD, "Rx get number of channels = "<<num)
}

void USRP_UHD_RX_Control_i::get_gain_range(CORBA::ULong channel, CORBA::Float &gmin, CORBA::Float &gmax, CORBA::Float &gstep)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);

    if (channel == 0 && usrp_uhd->sdev) {
        uhd::gain_range_t range = usrp_uhd->sdev->get_rx_gain_range(channel);
        // All values in range are in dB
        gmin = range.start();
        gmax = range.stop();
        gstep = range.step();
    }
}

void USRP_UHD_RX_Control_i::set_gain(CORBA::ULong channel, CORBA::Float gain)
{
	omni_mutex_lock l(usrp_uhd->rx_control_access);

	if (usrp_uhd->sdev) {
		usrp_uhd->sdev->set_rx_gain(gain,channel);
		DEBUG(3, USRP_UHD, "Rx set gain = "<<gain)
	}
	else {

	}
}

void USRP_UHD_RX_Control_i::get_gain(CORBA::ULong channel, CORBA::Float &gain)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);
    if (usrp_uhd->sdev) {
		gain = usrp_uhd->sdev->get_rx_gain(channel);
		DEBUG(3, USRP_UHD, "Rx get gain = "<<gain);
    }
    else{

    }
}

void USRP_UHD_RX_Control_i::get_frequency_range(CORBA::ULong channel, CORBA::Float &fmin, CORBA::Float &fmax, CORBA::Float &fstep)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);

	if (usrp_uhd->sdev) {
		uhd::freq_range_t range = usrp_uhd->sdev->get_rx_freq_range(channel);
		// All values in range are in dB
		fmin = range.start();
		fmax = range.stop();
		fstep = range.step();
		DEBUG(3, USRP_UHD, "Rx Frequency range = "<<range.to_pp_string())

	}
	else{

	}
}

void USRP_UHD_RX_Control_i::set_frequency(CORBA::ULong channel, CORBA::Float f)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);

    if (usrp_uhd->sdev) {
        uhd::tune_result_t result = usrp_uhd->sdev->set_rx_freq(f,channel);

        DEBUG(3, USRP_UHD, "USRP_UHD RX tune_result:" <<std::endl<<
              " \tTarget_RF_Freq=" << result.target_rf_freq <<std::endl<<
              " \tActual_RF_Freq=" << result.actual_rf_freq <<std::endl<<
              " \tTarget_DSP_freq="   << result.target_dsp_freq <<std::endl<<
              " \tActual_DSP_freq="   << result.actual_dsp_freq);
    } else {

    }
}

void USRP_UHD_RX_Control_i::get_frequency(CORBA::ULong channel, CORBA::Float &f)
{
	omni_mutex_lock l(usrp_uhd->rx_control_access);
	if(usrp_uhd->sdev){
		f = usrp_uhd->sdev->get_rx_freq(channel);
	}
	else{

	}
}

void USRP_UHD_RX_Control_i::start(CORBA::ULong channel)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);

    usrp_uhd->rx_packet_count = usrp_uhd->set_rx_packet_count;
    usrp_uhd->rx_active = true;

// Set up RX thread
    usrp_uhd->rx_thread = new omni_thread(USRP_UHD_i::do_rx_data_process, ((void *)usrp_uhd));
    usrp_uhd->rx_thread->start();
}

void USRP_UHD_RX_Control_i::stop(CORBA::ULong channel)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);
    DEBUG(0, USRP_UHD, "USRP_RX_Control stop Called")
    usrp_uhd->rx_active = false;
}

void USRP_UHD_RX_Control_i::set_values(const CF::Properties &values)
{
    DEBUG(3, USRP_UHD, "RX set_values: " << values.length() << " values, value[0].id " << values[0].id)

    for (unsigned int i =0; i < values.length(); ++i) {
        if (strcmp(values[i].id, "SET_NUM_RX_PACKETS") == 0) {
            CORBA::ULong num_packets;
            values[i].value >>= num_packets;
            DEBUG(3, USRP_UHD, "Number of packets to RX = " << num_packets)
            usrp_uhd->set_rx_packet_count = num_packets;
        } else if (strcmp(values[i].id, "SET_MUX") == 0 ) {
            CORBA::ULong mux;
            values[i].value >>= mux;
            DEBUG(1, USRP_UHD, "Request to setting receive mux to  " << mux<<" not supported")
            //omni_mutex_lock l(usrp_uhd->rx_control_access);
            //usrp_uhd->usrp_uhd_ptr->set_mux(mux);
        } else if (strcmp(values[i].id, "SET_RX_ANT_1") == 0 ) {
            CORBA::ULong ant;
            values[i].value >>= ant;
            omni_mutex_lock l(usrp_uhd->rx_control_access);
            std::vector<std::string>rx_antennas0 = usrp_uhd->sdev->get_rx_antennas(0);
            DEBUG(3, USRP_UHD, "Setting receive antenna to  " << rx_antennas0[ant])

            usrp_uhd->sdev->set_rx_antenna(rx_antennas0[ant]);

        } else if (strcmp(values[i].id, "SET_RX_ANT_2") == 0 ) {
            CORBA::ULong ant;
            values[i].value >>= ant;
            omni_mutex_lock l(usrp_uhd->rx_control_access);
            std::vector<std::string>rx_antennas1 = usrp_uhd->sdev->get_rx_antennas(1);
            DEBUG(3, USRP_UHD, "Setting receive antenna to  " << rx_antennas1[ant])
            usrp_uhd->sdev->set_rx_antenna(rx_antennas1[ant]);

        }
    }
}

void USRP_UHD_RX_Control_i::set_decimation_rate(CORBA::ULong channel, CORBA::ULong D)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);
    ///\TODO: rate matching in UHD works a little different. Review
    double master_clock = usrp_uhd->sdev->get_master_clock_rate();
    double request_da_rate = master_clock/(double)D;
    usrp_uhd->sdev->set_rx_rate(request_da_rate,channel);
    double real_da_rate = usrp_uhd->sdev->get_rx_rate(channel);
    DEBUG(3, USRP_UHD, "Requested Rx sample rate ="<<request_da_rate<<", Real Rx sample rate ="<<real_da_rate)
}

void USRP_UHD_RX_Control_i::get_decimation_range(CORBA::ULong channel, CORBA::ULong &dmin, CORBA::ULong &dmax, CORBA::ULong &dstep)
{
	omni_mutex_lock l(usrp_uhd->rx_control_access);
	if(usrp_uhd->sdev){
		double master_clock = usrp_uhd->sdev->get_master_clock_rate();
		uhd::meta_range_t rx_range = usrp_uhd->sdev->get_rx_rates(channel);
		dmax = master_clock/rx_range.start();
		dmin = master_clock/rx_range.stop();
		dstep = master_clock*rx_range.step()/(rx_range.stop()*rx_range.start());
		if(dstep < 1)
			dstep = 1;

		DEBUG(3, USRP_UHD, "Decimation min="<<dmin<<", max="<<dmax<<", step="<<dstep)
	}
	else{

	}
}

void USRP_UHD_RX_Control_i::set_data_packet_size(CORBA::ULong channel, CORBA::ULong N)
{
    omni_mutex_lock l(usrp_uhd->rx_control_access);
    usrp_uhd->rx_packet_size = N;
}