/* -*- 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 "garage_impl.h"
	
#define WITH_DEBUG_MESSAGES		0
#define WITH_DEBUG_FILE			0
	
namespace gr
{
namespace jay
{

garage::sptr garage::make(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold)
{
	return gnuradio::get_initial_sptr(new garage_impl(sampleRate, samplesPerSym, kLoopFilter, kThreshold));
}

/*
* The private constructor
*/
garage_impl::garage_impl(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold)
: gr::block("garage", 	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_baudRate((float)m_sampleRate/m_samplesPerSym)
, m_kLoopFilter(kLoopFilter)
, m_kThreshold(kThreshold)
, m_vmin(0)
, m_vmax(0)
, m_v_thr(0)
, m_lag_accu(0)
, m_bit(0)
, m_i(0)
, m_gapDistance(8*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.
*/
garage_impl::~garage_impl()
{
}

bool garage_impl::start()
{
	fprintf(stderr, "Opening garage\n");
#if WITH_DEBUG_FILE	
	m_pFile_y = fopen("/home/jens/garage_y.dat", "w");
	m_pFile_bits = fopen("/home/jens/garage_bits.dat", "w");
#endif
	return true;
}

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

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

int garage_impl::general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
	float rho = 1e-4;
	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;
	float alpha_thr = 0.5f;
	float hyst_thr = 0.5f;
	float k_leadLag = m_kLoopFilter;
	float loopThreshold = m_kThreshold;
	float k_lead = 1.0e-1f;
	float k_lag = 1.0e-4f;
	float k_leak = 0;
	float err = 0;
	
	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 = (1.f-i_f)*in[i_i] + i_f*in[i_i+1];
		
		// Calculate threshold
		m_v_thr = (fabs(m_vmax) - fabs(m_vmin))/2;
		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;
		}
		m_vmin = m_vmin + rho*(m_v_thr - m_vmin);
		m_vmax = m_vmax + rho*(m_v_thr - m_vmax);

		bool isSignal = (m_v_thr >= loopThreshold);

		// Detect bit change
		int bitChg = 0;
		if (m_bit == 1)
		{
			if (y < (hyst_thr*m_v_thr))
			{
				m_bit = 0;
				bitChg = 1;
			}
		}
		else 
		{
			if (y > (hyst_thr*m_v_thr))
			{
				m_bit = 1;
				bitChg = 1;
			}
		}

		bitProcessGap(bitChg && isSignal);

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

		float vcorr = k_leadLag*k_lead*err + m_lag_accu;

		// Lag part of loop filter
		m_lag_accu += k_leadLag*k_lag*err;
		m_lag_accu *= (1.f - k_leak);

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

		// Sample bit at baud rate = fs/numSamplesPerSym
		if (m_count == 0)
		{
			out[outCount++] = (float)m_bit;
			m_count = countReload;
			bitRecord(m_bit);
			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\n", (float)m_sampleCounter/m_sampleRate, y, m_v_thr, vcorr);
		}
		m_sampleCounter++;
		
		m_i += (1+vcorr);
		i_i = (int)m_i;
		
		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 garage_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 garage_impl::bitRecord(int bit)
{
	if (m_bitCounter < sizeof(m_bits)/sizeof(*m_bits))
	{
		m_bits[m_bitCounter++] = bit;
	}
}

void garage_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 */