/* -*- c++ -*- */
/* 
 * Copyright 2019 Thomas Kolb.
 * 
 * This 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.
 * 
 * This software 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 this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include "freq_est_lr_impl.h"

namespace gr {
	namespace hamnet70 {

		freq_est_lr::sptr
		freq_est_lr::make(const std::vector<gr_complex> &symbols, size_t kappa)
		{
			return gnuradio::get_initial_sptr
				(new freq_est_lr_impl(symbols, kappa));
		}

		/*
		 * The private constructor
		 */
		freq_est_lr_impl::freq_est_lr_impl(const std::vector<gr_complex> &symbols, size_t kappa)
			: gr::block("freq_est_lr",
					gr::io_signature::make(1, 1, sizeof(gr_complex)),
					gr::io_signature::make(1, 1, sizeof(float))),
				d_refSymbols(symbols),
				d_kappa(kappa),
				d_freq_est(0.0f)
		{
			d_recvSymbols.reserve(symbols.size() + 1);

			message_port_register_out(pmt::mp("freq_offset"));
		}

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

		void
		freq_est_lr_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
		{
			ninput_items_required[0] = noutput_items;
		}

		int
		freq_est_lr_impl::general_work (int noutput_items,
				gr_vector_int &ninput_items,
				gr_vector_const_void_star &input_items,
				gr_vector_void_star &output_items)
		{
			const gr_complex *in = (const gr_complex *) input_items[0];
			float *out = (float *) output_items[0];

			std::vector<tag_t> tags;
			get_tags_in_window(tags, 0, 0, ninput_items[0], pmt::intern("corr_est")); // FIXME: make name variable
			size_t tagidx = 0;

			for(size_t i = 0; i < noutput_items; i++) {
				if((tagidx < tags.size()) && (tags[tagidx].offset == nitems_read(0) + i)) {
					d_recvSymbols.clear();

					tagidx++;
				}

				if(d_recvSymbols.size() < d_refSymbols.size()) {
					// buffer symbols of the preamble
					d_recvSymbols.push_back(in[i]);
				} else if(d_recvSymbols.size() == d_refSymbols.size()) {
					// received enough symbols for the preamble => do frequency estimation
					d_recvSymbols.push_back(0); //???

					// remove reference symbols from the received preamble
					std::vector<gr_complex> z;
					z.reserve(d_recvSymbols.size());
					for(size_t k = 0; k < d_recvSymbols.size(); k++) {
						z.push_back( d_recvSymbols[k] * conj(d_refSymbols[k]) );
					}

					// Calculate averaged phase increments for <N> sub-sequences
					size_t N = d_recvSymbols.size()/2;
					std::vector<gr_complex> R_kappa(N);

					for(size_t kappa = 0; kappa < N; kappa++) {
						for(size_t k = 0; k < d_recvSymbols.size() - kappa; k++) {
							R_kappa[kappa] += z[k + kappa] * conj(z[k]);
						}

						R_kappa[kappa] /= z.size() - kappa;
					}

					// Calculate phase estimate (in radians/sample)
					gr_complex sum_R_kappa(0, 0);

					for(size_t kappa = 0; kappa < N; kappa++) {
						sum_R_kappa += R_kappa[kappa];
					}

					float arg = gr::fast_atan2f(sum_R_kappa);

					d_freq_est = arg / (M_PI * (1 + N));

					message_port_pub(pmt::intern("freq_offset"), pmt::from_double(-d_freq_est));
				}

				out[i] = d_freq_est;
			}

			// Do <+signal processing+>
			// Tell runtime system how many input items we consumed on
			// each input stream.
			consume_each (noutput_items);

			// Tell runtime system how many output items we produced.
			return noutput_items;
		}

	} /* namespace hamnet70 */
} /* namespace gr */