/* -*- c++ -*- */
/*
 * Copyright 2004 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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 3, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

/*
 * gr-baz by Balint Seeber (http://spench.net/contact)
 * Information, documentation & samples: http://wiki.spench.net/wiki/gr-baz
 */

/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <baz_hopper.h>
#include <gnuradio/io_signature.h>
#include <pmt/pmt.h>

#include <stdio.h>

/*
 * Create a new instance of baz_hopper and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
baz_hopper_sptr
baz_make_hopper(
		size_t item_size,
		int sample_rate,
		int chunk_length,
		int drop_length,
		std::vector<std::vector<double> > freqs,
		::gr::basic_block_sptr source,
		bool verbose /*= false*/
)
{
	return baz_hopper_sptr(new baz_hopper(
		item_size,
		sample_rate,
		chunk_length,
		drop_length,
		freqs,
		source,
		verbose
	));
}

/*
 * The private constructor
 */
baz_hopper::baz_hopper(
	size_t item_size,
		int sample_rate,
		int chunk_length,
		int drop_length,
		std::vector<std::vector<double> > freqs,
		::gr::basic_block_sptr source,
		bool verbose /*= false*/
)
	: gr::block("baz_hopper",
		gr::io_signature::make(1, 1, item_size),
		gr::io_signature::make(freqs.size(), freqs.size(), item_size))
	, d_item_size(item_size), d_sample_rate(sample_rate), d_chunk_length(chunk_length), d_drop_length(drop_length)
	, d_freqs(freqs), d_verbose(verbose)
	, d_last_time_seconds(0), d_last_time_fractional_seconds(0)
	, d_time_offset(-1), d_seen_time(false)
	, d_chunk_counter(0), d_freq_idx(0), d_zero_counter(0), d_reset(false), d_skip(0)
{
	fprintf(stderr, "[%s<%li>] item size: %lu, sample rate: %d, chunk length: %d, drop length: %d, # freq sets: %lu\n", name().c_str(), unique_id(),
		item_size,
		sample_rate,
		chunk_length,
		drop_length,
		freqs.size()
	);

	//set_output_multiple(chunk_length);
	
	d_usrp_src = boost::dynamic_pointer_cast< ::gr::uhd::usrp_source>(source);
	if (not d_usrp_src)
		throw std::runtime_error("Could not dynamic cast to USRP source");

	d_dev = d_usrp_src->get_device();
	
	for (size_t i = 0; i < freqs.size(); ++i)
	{
		const std::vector<double>& f = freqs[i];
		
		for (size_t j = 0; j < f.size(); ++j)
		{
			d_freq_dest.push_back(std::pair<double,int>(f[j], i));
		}
	}
}

/*
 * Our virtual destructor.
 */
baz_hopper::~baz_hopper ()
{
}

static const pmt::pmt_t RX_TIME_KEY = pmt::string_to_symbol("rx_time");
/*
double baz_hopper::time(bool relative)
{
	gr::thread::scoped_lock guard(d_mutex);
	
	double d = ((double)d_last_time_seconds + d_last_time_fractional_seconds) + ((double)d_time_offset / (double)d_sample_rate);
	if (relative)
		d -= ((double)d_first_time_seconds + d_first_time_fractional_seconds);
	
	return d;
}
*/
void baz_hopper::forecast(int noutput_items, gr_vector_int &ninput_items_required)
{
	for (size_t i = 0; i < ninput_items_required.size(); ++i)
	{
		ninput_items_required[i] = noutput_items;
	}
}

int baz_hopper::general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
	//fprintf(stderr, "%d/", noutput_items);

	if (d_skip > 0)
	{
		uint64_t to_consume = std::min(d_skip, (uint64_t)noutput_items);
		d_skip -= to_consume;
		consume_each(to_consume);
		return 0;
	}

	const int max_noutput_items = noutput_items;
	
	////////////////////////////////////////////////////////////////////////////
	
	if (d_zero_counter > 0)
	{
		int to_copy = std::min(noutput_items, d_zero_counter);
		uint64_t scheduled = d_scheduled[0];
		uint64_t dest = d_dest[scheduled];
		memset(output_items[dest], 0x00, d_item_size * to_copy);
		produce(dest, to_copy);
		d_zero_counter -= to_copy;
		if (d_zero_counter == 0)
		{
			d_scheduled.pop_front();
			d_dest.erase(scheduled);
			fprintf(stderr, "[%s<%li>] Finished zeroing %lld\n", name().c_str(), unique_id(), scheduled);
		}
		return WORK_CALLED_PRODUCE;
	}
	
	////////////////////////////////////////////////////////////////////////////
	
	//gr::thread::scoped_lock guard(d_mutex);
	
	//fprintf(stderr, "[%s] Work %d\n", name().c_str(), noutput_items);
	
	const int tag_channel = 0;
	const uint64_t nread = nitems_read(tag_channel);
	std::vector<gr::tag_t> tags;
	
	get_tags_in_range(tags, tag_channel, nread, (nread + noutput_items), RX_TIME_KEY);
	//std::sort(tags.begin(), tags.end(), tag_t::offset_compare);
	
	////////////////////////////////////////////////////////////////////////////
	
	if ((tags.size() == 0) && (d_seen_time == false))
	{
		fprintf(stderr, "[%s<%li>] not seen time; dropping incoming samples (after %llu items)\n", name().c_str(), unique_id(), nread);
		consume_each(noutput_items);
		return 0;
	}
	
	////////////////////////////////////////////////////////////////////////////
	
	//bool seen_time = d_seen_time;
	
	int offset = 0;
	//for (int i = 0; i < tags.size(); ++i)
	if (tags.size() > 0)
	{
		const int i = 0;
		
		const gr::tag_t& tag = tags[i];
		
		offset = tags[i].offset - nread;
		
		if (d_verbose) fprintf(stderr, "[%s<%li>] Tag #%d @ sample %llu (offset: %d): %s\n", name().c_str(), unique_id(), i, tags[i].offset, offset, pmt::write_string(tag.key).c_str());
		
		if (offset == 0)
		{
			d_time_offset = 0;
			
			d_last_time_seconds = pmt::to_uint64(pmt::tuple_ref(tag.value, 0));
			d_last_time_fractional_seconds = pmt::to_double(pmt::tuple_ref(tag.value, 1));
			
			d_seen_time = true;
		}
		
		//break;	// Just the first
	}
	
	////////////////////////////////////////////////////////////////////////////
	
	if ((offset == 0) && (tags.size() > 1))
	{
		const gr::tag_t& tag = tags[1];
		noutput_items = tag.offset - nread;
		if (d_verbose) fprintf(stderr, "[%s<%li>] Another tag @ %llu (noutput_items now: %d): %s\n", name().c_str(), unique_id(), tag.offset, noutput_items, pmt::write_string(tag.key).c_str());

		consume_each(noutput_items);
		return 0;
	}
	
	////////////////////////////////////////////////////////////////////////////
	
	if (offset > 0)
	{
		noutput_items = offset;
		
		if (d_verbose) fprintf(stderr, "[%s<%li>] Offset tag @ %d (noutput_items now: %d)\n", name().c_str(), unique_id(), offset, noutput_items);
		
		if (d_seen_time == false)
		{
			consume_each(noutput_items);
			return 0;
		}
	}
	
	////////////////////////////////////////////////////////////////////////////
	
	assert(d_seen_time);
	
	uint64_t time_begin =
		(d_last_time_seconds * (uint64_t)d_sample_rate) +
		(uint64_t)(d_last_time_fractional_seconds * (double)d_sample_rate) +
		d_time_offset;
	uint64_t time_end = time_begin + noutput_items;
	
	////////////////////////////////////

	bool cleared = false;
	
	bool hop_time_reset = false;
	//if ((seen_time == false) && (d_seen_time))
	if (((offset == 0) && (tags.size() > 0)) || (d_reset))	// New timestamp (start, or overrun)
	{
		d_reset = false;
		
		const double ahead_s = 0.01;	// MAGIC
		
		//uhd::time_spec_t prev_last_hop = d_last_hop;
		
		d_last_hop =
			uhd::time_spec_t(d_last_time_seconds, d_last_time_fractional_seconds) +
			uhd::time_spec_t::from_ticks(d_time_offset, d_sample_rate) +
			uhd::time_spec_t(ahead_s);
		hop_time_reset = true;
		
		if (d_chunk_counter == 0)	// If in chunk, then need to execute zeroing code first below
		{
			int currently_scheduled_count = d_scheduled.size();
			if (currently_scheduled_count > 0)
			{
				// We could zero fill up to next future existing scheduled chunk, but easier for now to dump them all and restart
				
				uint64_t last_scheduled = d_scheduled[currently_scheduled_count - 1];
				//uint64_t prev_last_hop_ticks = prev_last_hop.to_ticks(d_sample_rate);
				uint64_t last_hop_ticks = d_last_hop.to_ticks(d_sample_rate);
				if (last_scheduled > last_hop_ticks)
				{
					if (d_verbose) fprintf(stderr, "[%s<%li>] Re-scheduling base hop time (ahead: %lld)\n", name().c_str(), unique_id(), (last_scheduled - last_hop_ticks));
					d_last_hop = uhd::time_spec_t::from_ticks(last_scheduled, d_sample_rate) + uhd::time_spec_t(ahead_s);	// MAGIC (could smaller - total tune transaction process time)
				}
				
				d_scheduled.clear();
				d_dest.clear();
				
				//d_freq_idx = (d_freq_idx - currently_scheduled_count) % (d_freq_dest.size());
				d_freq_idx = (d_freq_idx - currently_scheduled_count);
				if (d_freq_idx < 0)
					d_freq_idx = (int)d_freq_dest.size() + d_freq_idx;
				
				if (d_verbose) fprintf(stderr, "[%s<%li>] Cleared %d times (idx: %d)\n", name().c_str(), unique_id(), currently_scheduled_count, d_freq_idx);

				//d_skip = (uint64_t)((double)d_sample_rate * ahead_s);

				cleared = true;
			}
		}
	}
	
	const size_t ahead_limit = 3;	// MAGIC (depends on FIFO depth)
	while (d_scheduled.size() < ahead_limit)
	{
		//const uint64_t ahead = (uint64_t)(ahead_d * (double)sample_rate);
		
		const std::pair<double,int> p = d_freq_dest[d_freq_idx];
		const double& freq = p.first;
		const int& dest = p.second;
		
		d_freq_idx = (d_freq_idx + 1) % (d_freq_dest.size());
		
		uhd::time_spec_t next_hop = d_last_hop;
		if (hop_time_reset == false)
			next_hop += uhd::time_spec_t::from_ticks((d_drop_length + d_chunk_length), d_sample_rate);
		
		{
		//boost::recursive_mutex::scoped_lock lock(d_usrp_src->get_lock());
		d_dev->set_command_time(next_hop);
		const double lo_offset = (d_sample_rate * 0.0) / 2.0;
		uhd::tune_request_t tune_request = uhd::tune_request_t(freq, lo_offset);
		d_dev->set_rx_freq(tune_request);
		d_dev->clear_command_time();
		}
		
		uint64_t scheduled = next_hop.to_ticks(d_sample_rate);
		
		d_scheduled.push_back(scheduled);
		d_dest[scheduled] = dest;
		hop_time_reset = false;
		d_last_hop = next_hop;
	}

	/*if (cleared)
	{
		fprintf(stderr, "[%s<%i>] Scheduled %d after clearing\n", name().c_str(), unique_id(), d_scheduled.size());
	}*/
	
	////////////////////////////////////
	
	uint64_t scheduled_offset = 0;
	if (d_chunk_counter == 0)
	{
		while (d_scheduled.size() > 0)	// FIXME: if (not looping)
		{
			uint64_t scheduled = d_scheduled[0];
			
			if (scheduled < time_begin)	// Might have overrun
			{
				//d_scheduled.pop_front();
				//d_dest.erase(scheduled);
				if (d_verbose) fprintf(stderr, "[%s<%li>] Too late for %lld (begin: %lld, diff: %lld)\n", name().c_str(), unique_id(), scheduled, time_begin, (time_begin - scheduled));
				//noutput_items = 0;
				d_reset = true;	// Will cause it to be removed (need this)
				break;
			}
			else if (scheduled >= time_end)	// In the future
			{
				break;
			}
			
			d_chunk_counter = d_drop_length + d_chunk_length;
			
			scheduled_offset = scheduled - time_begin;
			if (scheduled_offset > 0)
				noutput_items = scheduled_offset;	// Postpone
			//fprintf(stderr, "[%s<%i>] Scheduling %lld (begin: %lld, offset: %lld)\n", name().c_str(), unique_id(), scheduled, time_begin, scheduled_offset);
			break;
		}
	}
	
	////////////////////////////////////
	
	if ((d_chunk_counter > 0) && (scheduled_offset == 0))
	{
		uint64_t scheduled = d_scheduled[0];
		
		int done = (d_drop_length + d_chunk_length) - d_chunk_counter;
		
		uint64_t current_time = scheduled + done;
		
		if (current_time == time_begin)
		{
			uint64_t to_copy = std::min(noutput_items, d_chunk_counter);
			
			if ((d_drop_length > 0) && (d_chunk_counter > d_chunk_length))
			{
				to_copy = std::min(to_copy, (uint64_t)(d_chunk_counter - d_chunk_length));
				
				//fprintf(stderr, "[%s<%i>] Dropping for %lld samples: %lld (done: %d)\n", name().c_str(), unique_id(), scheduled, to_copy, done);
			}
			else
			{
				uint64_t dest = d_dest[scheduled];
				
				//fprintf(stderr, "[%s<%i>] Copying for %lld samples to %lld: %lld (done: %d)\n", name().c_str(), unique_id(), scheduled, dest, to_copy, done);
				
				memcpy(output_items[dest], input_items[0], d_item_size * to_copy);
				produce(dest, to_copy);
			}
			
			d_chunk_counter -= to_copy;
			
			if (d_chunk_counter == 0)
			{
				d_scheduled.pop_front();
				d_dest.erase(scheduled);
				//fprintf(stderr, "[%s<%i>] Removed %lld (begin: %lld)\n", name().c_str(), unique_id(), scheduled, time_begin);
			}
			
			noutput_items = to_copy;
		}
		else
		{
			if (d_chunk_counter < d_chunk_length)	// Re-try if we're still dropping
			{
				d_zero_counter = d_chunk_counter;
				//d_zero_counter = std::min(d_chunk_counter, d_chunk_length);	// Shows it on the waterfall
				noutput_items = 0;	// FIXME: Check this
			}
			
			if (d_verbose) fprintf(stderr, "[%s<%li>] Time jump for %lld: %lld (begin: %lld, done: %d, zeroing: %d)\n", name().c_str(), unique_id(), scheduled, current_time, time_begin, done, d_zero_counter);
			d_chunk_counter = 0;
			//noutput_items = 0;
		}
	}
	
	////////////////////////////////////////////////////////////////////////////

	/*if (cleared)
	{
		fprintf(stderr, "[%s<%i>] Scheduled %d after clearing\n", name().c_str(), unique_id(), d_scheduled.size());
	}*/

	//fprintf(stderr, "%d ", noutput_items);
	
	d_time_offset += noutput_items;
	consume_each(noutput_items);
	return WORK_CALLED_PRODUCE;
}
