/* -*- 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/expj.h>
#include <gnuradio/math.h>
#include "correct_frequency_impl.h"

namespace gr {
	namespace hamnet70 {

		correct_frequency::sptr
		correct_frequency::make(const std::vector<gr_complex> &symbols)
		{
			return gnuradio::get_initial_sptr
				(new correct_frequency_impl(symbols));
		}

		/*
		 * The private constructor
		 */
		correct_frequency_impl::correct_frequency_impl(const std::vector<gr_complex> &symbols)
			: gr::sync_block("correct_frequency",
					gr::io_signature::make(1, 1, sizeof(gr_complex)),
					gr::io_signature::make(1, 1, sizeof(gr_complex))),
				d_symbols(symbols),
				d_correctionIncrement(1, 0),
				d_correctionVector(1, 0),
				d_filteredPhaseInc(0.0f),
				d_preIdx(symbols.size()+1) // somewhere out of symbols' index range
		{}

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

		int
		correct_frequency_impl::work(int noutput_items,
				gr_vector_const_void_star &input_items,
				gr_vector_void_star &output_items)
		{
			const gr_complex *in = (const gr_complex *) input_items[0];
			gr_complex *out = (gr_complex *) output_items[0];

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

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

					tagidx++;
				}

				if(d_preIdx < d_symbols.size()) {
					gr_complex tmp = conj(d_symbols[d_preIdx]) * (*in);
					float phase = gr::fast_atan2f(tmp);

					if(d_preIdx != 0) {
						float phaseIncrement = phase - d_lastPhase;

						if(phaseIncrement > M_PI) {
							add_item_tag(0, nitems_read(0) + i, pmt::intern("phase_overflow"), pmt::from_double(phaseIncrement));
							phaseIncrement -= 2*M_PI;
						} else if(phaseIncrement < -M_PI) {
							add_item_tag(0, nitems_read(0) + i, pmt::intern("phase_underflow"), pmt::from_double(phaseIncrement));
							phaseIncrement += 2*M_PI;
						}

						/*
						d_filteredPhaseInc = c_phaseInc_alpha * phaseIncrement +
						                     (1 - c_phaseInc_alpha) * d_filteredPhaseInc;
						*/

						d_filteredPhaseInc += phaseIncrement;

						if(d_preIdx > d_symbols.size()/2) {
							d_correctionIncrement = gr_expj(-d_filteredPhaseInc/d_preIdx);
						}
					} else {
						d_unwrapOffset = 0;
						d_filteredPhaseInc = 0;
					}

					d_lastPhase = phase;

					if(d_preIdx == d_symbols.size()/2) {
						d_correctionVector = gr_complex(1, 0);
					}

					d_preIdx++;
				}

				d_correctionVector *= d_correctionIncrement;

				*out = *in * d_correctionVector;


				in++;
				out++;
			}

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

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