/* -*- c++ -*- */
/* 
 * Copyright 2018 Jay Arrowfield.
 * 
 * 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 "bit_slicer_impl.h"
	
#define WITH_DEBUG_MESSAGES		0
#define WITH_DEBUG_FILE			0
	
namespace gr
{
namespace jay
{

bit_slicer::sptr bit_slicer::make(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold, int numGapBits, int doPrintPackets)
{
	return gnuradio::get_initial_sptr(new bit_slicer_impl(sampleRate, samplesPerSym, kLoopFilter, kThreshold, numGapBits, doPrintPackets));
}

/*
* The private constructor
*/
bit_slicer_impl::bit_slicer_impl(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold, int numGapBits, int doPrintPackets)
: gr::block("bit_slicer", 	gr::io_signature::make(1, 1, sizeof(float)),
						gr::io_signature::make(1, 1, sizeof(float)))
, m_sampleRate(sampleRate)
, m_samplesPerSym(samplesPerSym)
, m_count(samplesPerSym)
, m_isSignal(false)
, m_baudRate((float)m_sampleRate/m_samplesPerSym)
, m_kLoopFilter(kLoopFilter)
, m_kThreshold(kThreshold)
, m_doPrintPackets(doPrintPackets)
, m_vmin(0)
, m_vmax(0)
, m_v_sig(0)
, m_dc_corr(0)
, m_agc_gain(1.0f)
, m_lag_accu(0)
, m_id_accu(0)
, m_id_bit(0)
, m_bitZeroCross(0)
, m_i(0)
, m_gapDistance(numGapBits*m_samplesPerSym)
, m_gapCounter(0)
, m_gapCounterEnable(0)
, m_packetCounter(0)
, m_bitCounter(0)
, m_sampleCounter(0)
, m_pFile_y(nullptr)
, m_pFile_bits(nullptr)
{
}

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

bool bit_slicer_impl::start()
{
	fprintf(stderr, "Opening bit_slicer\n");
#if WITH_DEBUG_FILE	
	m_pFile_y = fopen("/home/jens/bit_slicer_y.dat", "w");
	m_pFile_bits = fopen("/home/jens/bit_slicer_bits.dat", "w");
#endif
	return true;
}

bool bit_slicer_impl::stop()
{
	fprintf(stderr, "Closing Bit Slicer\n");
	if (m_pFile_y)
	{
		fclose(m_pFile_y);
	}
	if (m_pFile_bits)
	{
		fclose(m_pFile_bits);
	}
	return true;
}

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

int bit_slicer_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 float *in = (const float *) input_items[0];
	float *out = (float *) output_items[0];
	int outCount = 0;
	int inCount = 0;
	
	// Do <+signal processing+>
	int countReload = m_samplesPerSym-1;
	const float alpha_thr = 0.01f;
	const float alpha_dc = 0.002f;
	const float hyst_thr = 0.1f;
	const float rho_thr = 0.9999f;
	const float agc_mu = 0.01f;
	const float agc_target = 0.5f;
	const float agc_gain_max = 10.0f;
	const float k_lead = 1.0e-1f;
	const float k_lag = 4.0e-4f;
	const float rho_leak = 0.99995f;

	float err = 0;
	float k_leadLag = m_kLoopFilter;
	float loopThreshold = m_kThreshold;
	
	float alpha_sig = 0.5f/m_samplesPerSym;
	
	int i_i = 0;
	while ((i_i+1) < ninput_items[0])
	{
		// Get next interpolated sample
		float i_f = m_i - (float)i_i;
		float y = m_agc_gain * ((1.f-i_f)*in[i_i] + i_f*in[i_i+1] - m_dc_corr);
		
		// Calculate threshold
		m_vmin *= rho_thr;
		m_vmax *= rho_thr;
		if (y < m_vmin)
		{
			m_vmin = (1-alpha_thr)*m_vmin + alpha_thr*y;
		}
		else if (y > m_vmax)
		{
			m_vmax = (1-alpha_thr)*m_vmax + alpha_thr*y;
		}

		// AGC
		float agc_err = m_vmax - agc_target;
		m_agc_gain = std::max(0.0f, std::min(agc_gain_max, m_agc_gain - agc_mu*agc_err));
		
		// Calculate DC offset correction
		m_dc_corr = (1.f-alpha_dc)*m_dc_corr + alpha_dc*(m_vmax + m_vmin);
		
		// Signal detection
		float yd = std::abs(y)-m_vmax;
		m_v_sig = (1.f-alpha_sig)*m_v_sig + alpha_sig*yd*yd;
		
		if (!m_isSignal)
		{
			if (m_v_sig <= loopThreshold)
			{
				m_isSignal = true;
			}
		}
		else
		{
			if (m_v_sig > 1.2*loopThreshold)
			{
				m_isSignal = false;
			}
		}

		// Integrate and dump
		float k_id = 1.f/((1+m_lag_accu)*m_samplesPerSym);
		m_id_accu += k_id*y;
		if (m_count == m_samplesPerSym/2)
		{
			float accu = m_id_accu;
			m_id_accu = 0;
			int bit_id = (int)(accu >= 0.0);
			if (m_isSignal)
			{
				if (m_doPrintPackets)
				{
					bitRecord(bit_id);
				}
				out[outCount++] = (float)bit_id;
			}
			else
			{
				out[outCount++] = (float)0xFFFFFFFF;
			}
		}

		// Zero crossing detector
		int bitChg = 0;
		if (m_isSignal)
		{
			if (m_bitZeroCross == 1)
			{
				if (y < -hyst_thr)
				{
					m_bitZeroCross = 0;
					bitChg = 1;
				}
			}
			else 
			{
				if (y > +hyst_thr)
				{
					m_bitZeroCross = 1;
					bitChg = 1;
				}
			}
		}

		if (m_doPrintPackets)
		{
			bitProcessGap(bitChg);
		}

		// Calc error
		err = 0;
		if (bitChg)
		{
			err = -(m_count - (int)(m_samplesPerSym/2));
		}

		float vcorr = k_leadLag*k_lead*err + m_lag_accu;

		m_lag_accu *= rho_leak;
		if (m_isSignal)
		{
			if  (m_lag_accu > -0.2f and m_lag_accu < 0.2f)
			{
				// Lag part of loop filter
				m_lag_accu += k_leadLag*k_lag*err;
			}
		}			
		else
		{
			m_lag_accu = 0;
		}

		m_baudRate = (float)m_sampleRate/((1+m_lag_accu)*m_samplesPerSym);

		// Sample bit at baud rate = fs/numSamplesPerSym
		if (m_count == 0)
		{
			m_count = countReload;
			if (m_pFile_bits)
			{
				fprintf(m_pFile_bits, "%f %f\n", (float)m_sampleCounter/m_sampleRate, y);
			}
		}
		else
		{
			m_count--;
		}

		if (m_pFile_y)
		{
			fprintf(m_pFile_y, "%f %f %f %f %f\n", (float)m_sampleCounter/m_sampleRate, y, m_v_sig, m_lag_accu, m_dc_corr);
		}
		m_sampleCounter++;
		
		m_i += (1+vcorr);
		i_i = (int)m_i;
		
		if (outCount >= noutput_items)
		{
			break;
		}

		inCount++;
		if (inCount >= ninput_items[0])
		{
			break;
		}
		
		if (i_i >= (float)ninput_items[0])
		{
			break;
		}
	}

#if WITH_DEBUG_MESSAGES	
	fprintf(stderr, "ninput_items[0] = %d\n", ninput_items[0]);
	fprintf(stderr, "m_i             = %f\n", m_i);
	fprintf(stderr, "inCount  = %d\n", inCount);
	fprintf(stderr, "outCount = %d\n", outCount);
#endif
	
	// Tell runtime system how many input items we consumed on
	// each input stream.
	m_i -= i_i;
	consume_each (i_i);

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

void bit_slicer_impl::bitsPrint()
{
	for (int k=0; k < m_bitCounter; k++)
	{
		printf("%d", m_bits[k]);
	}
	printf("\n");
	printf("Recorded %d bits\n", m_bitCounter);
}

void bit_slicer_impl::bitRecord(int bit)
{
	if (m_bitCounter < sizeof(m_bits)/sizeof(*m_bits))
	{
		m_bits[m_bitCounter++] = bit;
	}
}

void bit_slicer_impl::bitProcessGap(int bitChg)
{
	if (bitChg)
	{
		if (!m_gapCounterEnable)
		{
			m_bitCounter = 0;
		}
		m_gapCounterEnable = 1;
		m_gapCounter = 0;
	}
	else
	{	
		m_gapCounter += m_gapCounterEnable;
		if (m_gapCounter >= m_gapDistance)
		{
			m_gapCounter = 0;
			m_gapCounterEnable = 0;
			printf("------------------------\n");
			printf("Packet: %03d\n", m_packetCounter++);
			bitsPrint();
			fprintf(stderr, "baudrate = %d\n", (int)m_baudRate);
		}
	}
}

} /* namespace jay */
} /* namespace gr */