/* -*- c++ -*- */
/*
 * Copyright 2025 BG2BHC.
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "oqpsk_coherent_demod_cc_impl.h"
#include <gnuradio/io_signature.h>
#include <gnuradio/expj.h>
#include <math.h>
#include <stdio.h>
#include <time.h>

namespace gr {
namespace dslwp {

using input_type = gr_complex;
using output_type = gr_complex;
oqpsk_coherent_demod_cc::sptr
oqpsk_coherent_demod_cc::make(int samples_per_symbol,
                              const std::vector<gr_complex>& taps,
                              int opt_point,
                              int pll,
                              float pll_loop_bw,
                              float pll_damping,
                              float freq_max,
                              float freq_min,
                              int dttl,
                              float dttl_loop_bw,
                              float dttl_damping,
                              float max_rate_deviation,
                              int asm_ignore)
{
    return gnuradio::make_block_sptr<oqpsk_coherent_demod_cc_impl>(samples_per_symbol,
                                                                   taps,
                                                                   opt_point,
                                                                   pll,
                                                                   pll_loop_bw,
                                                                   pll_damping,
                                                                   freq_max,
                                                                   freq_min,
                                                                   dttl,
                                                                   dttl_loop_bw,
                                                                   dttl_damping,
                                                                   max_rate_deviation,
                                                                   asm_ignore);
}


/*
 * The private constructor
 */
oqpsk_coherent_demod_cc_impl::oqpsk_coherent_demod_cc_impl(
    int samples_per_symbol,
    const std::vector<gr_complex>& taps,
    int opt_point,
    int pll,
    float pll_loop_bw,
    float pll_damping,
    float freq_max,
    float freq_min,
    int dttl,
    float dttl_loop_bw,
    float dttl_damping,
    float max_rate_deviation,
    int asm_ignore)
    : gr::block("oqpsk_coherent_demod_cc",
                gr::io_signature::make(
                    1 /* min inputs */, 1 /* max inputs */, sizeof(input_type)),
                gr::io_signature::make(
                    1 /* min outputs */, 1 /*max outputs */, sizeof(output_type))),
	d_samples_per_symbol(samples_per_symbol), d_taps(taps), d_opt_point(opt_point), d_pll(pll), d_pll_loop_bw(pll_loop_bw), d_pll_damping(pll_damping), d_freq_max(freq_max), d_freq_min(freq_min), d_dttl(dttl), d_dttl_loop_bw(dttl_loop_bw), d_dttl_damping(dttl_damping), d_max_rate_deviation(max_rate_deviation), d_asm_ignore(asm_ignore), d_symbols_since_asm(0)
{
	d_mix_out = (gr_complex *)malloc(sizeof(gr_complex)*taps.size());		
	for(int i=0; i<taps.size(); i++)
	{
		d_mix_out[i] = 1.0f;
	}

	d_mf_out = (gr_complex *)malloc(sizeof(gr_complex)*(samples_per_symbol/2+1));
	for(int i=0; i<samples_per_symbol/2+1; i++)
	{
		d_mf_out[i] = 1.0f;
	}

	float denom = 1.0f + 2.0f*pll_damping*pll_loop_bw + pll_loop_bw*pll_loop_bw;
	d_alpha = (4*pll_damping*pll_loop_bw)/denom;
	d_beta = (4*pll_loop_bw*pll_loop_bw)/denom;

	set_tag_propagation_policy(TPP_DONT);

	d_sample_in_symbol = 0;
	d_freq = 0;
	d_phase = 0;
}

/*
 * Our virtual destructor.
 */
oqpsk_coherent_demod_cc_impl::~oqpsk_coherent_demod_cc_impl() {
	free(d_mix_out);
	d_mix_out = nullptr; 
	free(d_mf_out);
	d_mf_out = nullptr; 
}

void oqpsk_coherent_demod_cc_impl::forecast(int noutput_items,
                                            gr_vector_int& ninput_items_required)
{
      /* <+forecast+> e.g. ninput_items_required[0] = noutput_items */
		ninput_items_required[0] = d_samples_per_symbol * noutput_items;
}

int oqpsk_coherent_demod_cc_impl::general_work(int noutput_items,
                                               gr_vector_int& ninput_items,
                                               gr_vector_const_void_star& input_items,
                                               gr_vector_void_star& output_items)
{
    auto in = static_cast<const input_type*>(input_items[0]);
    auto out = static_cast<output_type*>(output_items[0]);
	int i_output = 0;
	bool use_asm;
    // Do <+signal processing+>
      for(int i=0; i<ninput_items[0]; i++)
      {
		std::vector<tag_t> tags;
		get_tags_in_window(tags, 0, i, i+1);

		use_asm = d_symbols_since_asm >= d_asm_ignore;
		for(int j=0; j<tags.size(); j++)
		{
			if(tags[j].key == pmt::mp("corr_start"))
			{
				if (use_asm)
				{
					int sample_in_symbol_old = d_sample_in_symbol;
					d_sample_in_symbol = 0;
					d_symbols_since_asm = 0;

					/* For opt_point=12, [B-1, B0], [B1, B2]... */
					//add_item_tag(0, nitems_written(0)+i_output+1, tags[j].key, tags[j].value );
					
					/* For opt_point=4, [B0, B1], [B2, B3]... */
					add_item_tag(0, nitems_written(0)+i_output+2, tags[j].key, tags[j].value );
					
					static time_t time_curr;
					static struct tm *tblock_curr;
					
					time_curr = time(NULL);
					tblock_curr = gmtime(&time_curr);
					
					fprintf(stdout, "\n**** ASM found at: %02d:%02d:%02d\nSet sample_in_symbol: %d -> 0\n", tblock_curr->tm_hour, tblock_curr->tm_min, tblock_curr->tm_sec, sample_in_symbol_old);
				}
				else
				{
					static time_t time_curr;
					static struct tm *tblock_curr;
					
					time_curr = time(NULL);
					tblock_curr = gmtime(&time_curr);
					
					fprintf(stdout, "\n**** Ignored ASM at: %02d:%02d:%02d\n", tblock_curr->tm_hour, tblock_curr->tm_min, tblock_curr->tm_sec);
				}
			}
			if(tags[j].key == pmt::mp("payload_start"))
			{
				/* For opt_point=12, [B-1, B0], [B1, B2]... */
				//add_item_tag(0, nitems_written(0)+i_output+1, tags[j].key, tags[j].value );

				/* For opt_point=4, [B0, B1], [B2, B3]... */
				add_item_tag(0, nitems_written(0)+i_output+2, tags[j].key, tags[j].value );
			}
			else if(use_asm && (tags[j].key == pmt::mp("freq_est")))
			{
				if(pmt::is_real(tags[j].value))
				{
					float value = pmt::to_double(tags[j].value);
					float freq_old = d_freq;
					d_freq = value;
					fprintf(stdout, "Set freq: %f -> %f\n", freq_old, d_freq);
				}
			}
			else if(use_asm && (tags[j].key == pmt::mp("phase_est")))
			{
				if(pmt::is_real(tags[j].value))
				{
					float value = pmt::to_double(tags[j].value);
					float phase_old = d_phase;
					d_phase = value;
					fprintf(stdout, "Set phase: %f -> %f\n", phase_old, d_phase);
				}
			}
			else if(use_asm && (tags[j].key == pmt::mp("snr_est")))
			{
				if(pmt::is_real(tags[j].value))
				{
					float value = pmt::to_double(tags[j].value);
					float eb_n0_est = value*d_samples_per_symbol/2.0f;

					/* For opt_point=12, [B-1, B0], [B1, B2]... */
					//add_item_tag(0, nitems_written(0)+i_output+1, pmt::mp("eb_n0"), pmt::from_double(value*d_samples_per_symbol/2.0f) );
					
					/* For opt_point=4, [B0, B1], [B2, B3]... */
					add_item_tag(0, nitems_written(0)+i_output+2, pmt::mp("eb_n0_est"), pmt::from_double(eb_n0_est) );
					fprintf(stdout, "Estimated Eb/N0 = %f\n", eb_n0_est);
				}
			}
		}
		
		gr_complex nco;
		nco = gr_expj(-d_phase);

		for(int j=0; j<(d_taps.size()-1); j++)
		{
			d_mix_out[j] = d_mix_out[j+1];
		}
		d_mix_out[d_taps.size()-1] = nco*in[i];

		for(int j=0; j<d_samples_per_symbol/2; j++)
		{
			d_mf_out[j] = d_mf_out[j+1];
		}		

		d_mf_out[d_samples_per_symbol/2] = 0.0f;
		for(int j=0; j<d_taps.size(); j++)
		{
			d_mf_out[d_samples_per_symbol/2] = d_mf_out[d_samples_per_symbol/2] + d_mix_out[j] * d_taps[j];
		}

		float pd_out;
		if( d_sample_in_symbol==d_opt_point )
		{
			/* For opt_point=12, [B-1, B0], [B1, B2]... */
			//out[i_output].real() = d_mf_out[0].real();
			//out[i_output].imag() = d_mf_out[d_samples_per_symbol/2].imag();

			/* For opt_point=4, [B0, B1], [B2, B3]... */
		        out[i_output] = gr_complex(d_mf_out[0].imag(), d_mf_out[d_samples_per_symbol/2].real());
			i_output++;
			d_symbols_since_asm++;

			/* For opt_point=12, [B-1, B0], [B1, B2]... */
			//pd_out = d_mf_out[0].real()*d_mf_out[0].imag() - d_mf_out[d_samples_per_symbol/2].real()*d_mf_out[d_samples_per_symbol/2].imag();

			/* For opt_point=4, [B0, B1], [B2, B3]... */
			pd_out = d_mf_out[d_samples_per_symbol/2].real()*d_mf_out[d_samples_per_symbol/2].imag() - d_mf_out[0].real()*d_mf_out[0].imag();
		}

		d_sample_in_symbol++;
		if (d_sample_in_symbol == d_samples_per_symbol)
		{
			d_sample_in_symbol = 0;
		}
		
		if(d_pll)
		{
			d_freq = d_freq + d_beta * pd_out / d_samples_per_symbol / d_samples_per_symbol;

			if(d_freq > d_freq_max)
			{
				d_freq = d_freq_max;
			}
			else if(d_freq < d_freq_min)
			{
				d_freq = d_freq_min;
			}

			d_phase = d_phase + d_freq + d_alpha * pd_out / d_samples_per_symbol;
		}
		else
		{
			d_phase = d_phase + d_freq;
		}

		if(d_phase > M_PI)
		{
			d_phase -= 2.0f*M_PI;
		}
		else if(d_phase < -M_PI)
		{
			d_phase += 2.0f*M_PI;
		}

      }

    // Tell runtime system how many input items we consumed on
    // each input stream.
    consume_each(ninput_items[0]);

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

} /* namespace dslwp */
} /* namespace gr */