// RTL-SDR AFSK UART receiver; 9 Jul 2016
// Copyright (C) 2016 Daniel Beer <dlbeer@gmail.com>
//
// Permission to use, copy, modify, and/or distribute this software for
// any purpose with or without fee is hereby granted, provided that the
// above copyright notice and this permission notice appear in all
// copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
// PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
// PERFORMANCE OF THIS SOFTWARE.
//
// Compile this program with:
//
//     g++ -O1 -Wall -std=c++11 -o rtlafsk rtlafsk.cpp
//
// Use with the RTL-SDR receiver as follows:
//
//     rtl_sdr -g 30 -f <frequency> | ./rtlafsk <options>
//
// Be sure to set the RTL-SDR to a fixed gain (-g), because automatic
// gain control across the whole band will prevent carrier detection
// from working correctly. It's best to tune to a frequency so that the
// band of interest doesn't cross 0 Hz. This avoids problems with ADC
// bias.

#include <functional>
#include <complex>
#include <cerrno>
#include <vector>
#include <algorithm>
#include <cstring>
#include <iostream>
#include <system_error>
#include <exception>
#include <cstdarg>
#include <cstdio>

#include <unistd.h>

////////////////////////////////////////////////////////////////////////
// Tags: digital signal components
////////////////////////////////////////////////////////////////////////

typedef uint8_t tag;

static const tag TAG_CD = 0x01;
static const tag TAG_RXD = 0x02;

////////////////////////////////////////////////////////////////////////
// Kernels
////////////////////////////////////////////////////////////////////////

static std::vector<float> lowpass(float band, float order)
{
    const unsigned int side = round(0.5 * order / band);
    const unsigned int taps = side * 2 + 1;

    std::vector<float> filter(taps);
    float sum = 0.0f;

    for (unsigned int i = 0; i < taps; i++) {
	const float t = int(i) - int(side);
	const float w = cos(t * M_PI / taps);
	const float x = 2.0f * M_PI * band * t;
	float s = sin(x) / x;

	if (!std::isfinite(s))
	    s = 1.0f;

	filter[i] = s * w;
	sum += filter[i];
    }

    for (auto& x : filter)
	x /= sum;

    return filter;
}

////////////////////////////////////////////////////////////////////////
// Deglitching filter
////////////////////////////////////////////////////////////////////////

class deglitch_filter {
public:
    deglitch_filter(unsigned int min_time) :
	_min_time(min_time), _state(true), _count(0)
    {
    }

    void reset()
    {
	_state = true;
	_count = 0;
    }

    bool feed(bool raw)
    {
	if (raw == _state) {
	    _count = 0;
	    return _state;
	}

	if (++_count >= _min_time) {
	    _state = raw;
	    _count = 0;
	}

	return _state;
    }

private:
    const unsigned int		_min_time;

    bool			_state;
    unsigned int		_count;
};

////////////////////////////////////////////////////////////////////////
// UART state machine
////////////////////////////////////////////////////////////////////////

class uart {
public:
    typedef uint32_t word;

    uart(float bit_period, unsigned int bits_per_word) :
       _bit_period(bit_period),
       _bits_per_word(bits_per_word),
       _filter(round(bit_period * 0.5f)),
       _state(-1),
       _counter(0.0f),
       _data(0)
    {
    }

    void reset()
    {
	_filter.reset();
	_state = -1;
    }

    bool feed(bool line_state)
    {
	line_state = _filter.feed(line_state);

	if (_state < 0) {
	    if (!line_state) {
		_state = 0;
		_counter = _bit_period * 0.5f + 1.0f;
		_data = 0;
	    }

	    return false;
	}

	if (_state == int(_bits_per_word)) {
	    if (line_state)
		_state = -1;

	    return false;
	}

	_counter += 1.0f;

	if (_counter >= _bit_period) {
	    _counter -= _bit_period;
	    _data <<= 1;
	    _state++;

	    if (line_state)
		_data |= 1;

	    if (_state == int(_bits_per_word))
		return true;
	}

	return false;
    }

    word data() const
    {
	return _data;
    }

private:
    const float			_bit_period;
    const unsigned int		_bits_per_word;

    deglitch_filter		_filter;
    int				_state;
    float			_counter;
    word			_data;
};

////////////////////////////////////////////////////////////////////////
// RF mixer and decimator
////////////////////////////////////////////////////////////////////////

class rf_decimator {
public:
    typedef std::function<void (const std::complex<float> *data,
				size_t len)> callback;

    rf_decimator(const callback& cb, float center, unsigned int ratio,
		 float lp_order = 3.0f) :
	_callback(cb),
	_step(exp(std::complex<float>(0.0f, -2.0f * M_PI * center))),
	_ratio(ratio),
	_kernel(lowpass(0.5 / ratio, lp_order)),
	_out(1024),
	_in(_out.size() * ratio + _kernel.size() - 1),
	_ptr(_kernel.size() - 1),
	_osc(1.0f, 0.0f)
    {
	std::fill(_in.begin(), _in.begin() + _ptr, 0.0f);
    }

    void feed(const std::complex<float> *data, size_t len);
    void flush();

private:
    const callback			_callback;
    const std::complex<float>		_step;
    const unsigned int			_ratio;
    const std::vector<float>		_kernel;

    std::vector<std::complex<float>>	_out;
    std::vector<std::complex<float>>	_in;
    size_t				_ptr;
    std::complex<float>			_osc;

    void process();
};

void rf_decimator::feed(const std::complex<float> *data, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	std::copy(data, data + r, _in.data() + _ptr);
	data += r;
	len -= r;
	_ptr += r;

	if (_ptr >= _in.size())
	    process();
    }
}

void rf_decimator::flush()
{
    std::fill(_in.begin() + _ptr, _in.end(), 0.0f);
    process();
}

void rf_decimator::process()
{
    // Mix the new data
    _osc /= abs(_osc);
    for (size_t i = _kernel.size() - 1; i < _in.size(); i++) {
	_in[i] *= _osc;
	_osc *= _step;
    }

    // Compute convolution
    for (size_t i = 0; i < _out.size(); i++) {
	std::complex<float> sum(0.0f, 0.0f);
	const size_t k = i * _ratio;

	for (size_t j = 0; j < _kernel.size(); j++)
	    sum += _in[k + j] * _kernel[j];

	_out[i] = sum;
    }

    // Shift and reset pointer
    _ptr = _kernel.size() - 1;
    std::copy(_in.end() - _ptr, _in.end(), _in.begin());

    // Feed forward
    _callback(_out.data(), _out.size());
}

////////////////////////////////////////////////////////////////////////
// Carrier detection and tracking
////////////////////////////////////////////////////////////////////////

class cd_tracker {
public:
    typedef std::function<void (const std::complex<float> *data,
				const tag *tags, size_t len)> callback;

    cd_tracker(const callback& cb, size_t period,
	    float freq_stable, float mix_offset = 0.0f) :
	_callback(cb),
	_mix_offset(mix_offset),
	_freq_stable(freq_stable),
	_in(period),
	_tags(period),
	_ptr(0),
	_last_amp(1.0f),
	_last_amp_good(false),
	_last_freq(0.0f),
	_cd_debounce(0),
	_osc(1.0f, 0.0f)
    {
    }

    void feed(const std::complex<float> *data, size_t len);
    void flush();

private:
    static const unsigned int DB_COUNT = 3;

    const callback				_callback;
    const float					_mix_offset;
    const float					_freq_stable;

    std::vector<std::complex<float>>		_in;
    std::vector<tag>				_tags;
    size_t					_ptr;

    float					_last_amp;
    bool					_last_amp_good;

    float					_last_freq;
    unsigned int				_cd_debounce;
    std::complex<float>				_osc;

    void process();
};

void cd_tracker::feed(const std::complex<float> *data, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	std::copy(data, data + r, _in.data() + _ptr);
	data += r;
	len -= r;
	_ptr += r;

	if (_ptr >= _in.size())
	    process();
    }
}

void cd_tracker::flush()
{
    std::fill(_in.begin() + _ptr, _in.end(), 0.0f);
    process();
}

void cd_tracker::process()
{
    float amp = 0.0f;
    float freq = 0.0f;

    // Calculate average amplitude and frequency
    for (size_t i = 1; i < _in.size(); i++) {
	const float dp = arg(_in[i] * conj(_in[i - 1]));

	amp += abs(_in[i]);
	if (std::isfinite(dp))
	    freq += dp;
    }

    // Calculate average
    amp /= _in.size();
    freq /= (_in.size() - 1) * 2.0 * M_PI;

    // Frequency stability signal
    const bool freq_good = fabs(freq - _last_freq) < _freq_stable;

    // Amplitude good signal
    bool amp_good = _last_amp_good;

    if (amp < _last_amp * 0.5f)
	amp_good = false;
    if (amp > _last_amp * 2.0f)
	amp_good = true;

    // Raw carrier detect
    const bool cd = amp_good && freq_good;

    // Debounced carrier detect
    if (!cd)
	_cd_debounce = 0;
    else if (_cd_debounce < DB_COUNT)
	_cd_debounce++;

    // Mix, with linear interpolation
    const float fi = (_mix_offset - _last_freq) * M_PI * 2.0f;
    const float fs = (freq - _last_freq) *
	M_PI * 2.0f / float(_in.size());

    _osc /= abs(_osc);

    for (size_t i = 0; i < _in.size(); i++) {
	_in[i] *= _osc;
	_osc *= exp(std::complex<float>(0.0f, fi + fs * i));
    }

    // AGC
    if (amp_good) {
	const float scale = 1.0f / amp;

	for (size_t i = 0; i < _in.size(); i++)
	    _in[i] *= scale;
    }

    // Produce carrier-detect tags
    std::fill(_tags.begin(), _tags.end(),
	      (_cd_debounce < DB_COUNT) ? 0 : TAG_CD);

    // Reset for next frame
    _last_freq = freq;
    _last_amp = amp;
    _last_amp_good = amp_good;
    _ptr = 0;

    _callback(_in.data(), _tags.data(), _in.size());
}

////////////////////////////////////////////////////////////////////////
// Lowpass filter (with tags)
////////////////////////////////////////////////////////////////////////

template<class T>
class lp_filter {
public:
    typedef std::function<void (const T *data,
				const tag *tags, size_t len)> callback;

    lp_filter(const callback& cb,
	     const float cutoff, const float order = 3.0f) :
	_callback(cb),
	_kernel(lowpass(cutoff, order)),
	_out(1024),
	_in(_out.size() + _kernel.size() - 1),
	_tags(_in.size()),
	_ptr(_kernel.size() - 1)
    {
    }

    void feed(const T *data, const tag *tags, size_t len);
    void flush();

private:
    const callback			_callback;
    const std::vector<float>		_kernel;

    std::vector<T>			_out;
    std::vector<T>			_in;
    std::vector<tag>			_tags;
    size_t				_ptr;

    void process();
};

template<class T>
void lp_filter<T>::feed(const T *data, const tag *tags, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	std::copy(data, data + r, _in.data() + _ptr);
	std::copy(tags, tags + r, _tags.data() + _ptr);

	data += r;
	tags += r;
	len -= r;

	_ptr += r;

	if (_ptr >= _in.size())
	    process();
    }
}

template<class T>
void lp_filter<T>::flush()
{
    std::fill(_in.begin() + _ptr, _in.end(), 0.0f);
    std::fill(_tags.begin() + _ptr, _tags.end(), 0);
    process();
}

template<class T>
void lp_filter<T>::process()
{
    // Convolution
    for (size_t i = 0; i < _out.size(); i++) {
	T sum = 0;

	for (size_t j = 0; j < _kernel.size(); j++)
	    sum += _kernel[j] * _in[i + j];

	_out[i] = sum;
    }

    // Feed through
    _ptr = _kernel.size() - 1;
    _callback(_out.data(), _tags.data() + _kernel.size() / 2, _out.size());

    // Shift data
    std::copy(_in.end() - _ptr, _in.end(), _in.begin());
    std::copy(_tags.end() - _ptr, _tags.end(), _tags.begin());
}

////////////////////////////////////////////////////////////////////////
// Frequency detector
////////////////////////////////////////////////////////////////////////

class freq_detector {
public:
    typedef std::function<void (const float *data,
				const tag *tags, size_t len)> callback;

    freq_detector(const callback& cb, size_t side = 1) :
	_callback(cb),
	_out(1024),
	_in(_out.size() + 2),
	_tags(_out.size() + 2),
	_ptr(2)
    {
    }

    void feed(const std::complex<float> *data,
	      const tag *tags, size_t len);
    void flush();

private:
    const callback			_callback;

    std::vector<float>			_out;
    std::vector<std::complex<float>>	_in;
    std::vector<tag>			_tags;
    size_t				_ptr;

    void process();
};

void freq_detector::feed(const std::complex<float> *data,
		    const tag *tags, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	std::copy(data, data + r, _in.data() + _ptr);
	std::copy(tags, tags + r, _tags.data() + _ptr);

	data += r;
	tags += r;
	len -= r;

	_ptr += r;

	if (_ptr >= _in.size())
	    process();
    }
}

void freq_detector::flush()
{
    std::fill(_in.begin() + _ptr, _in.end(), 0.0f);
    std::fill(_tags.begin() + _ptr, _tags.end(), 0);
    process();
}

void freq_detector::process()
{
    // Discrimination
    for (size_t i = 0; i < _out.size(); i++) {
	const float f = arg(conj(_in[i]) * _in[i + 2]);

	_out[i] = std::isfinite(f) ? (f / (M_PI * 4.0f)) : 0.0f;
    }

    // Feed through
    _ptr = 2;
    _callback(_out.data(), _tags.data() + 1, _out.size());

    // Shift data
    std::copy(_in.end() - _ptr, _in.end(), _in.begin());
    std::copy(_tags.end() - _ptr, _tags.end(), _tags.begin());
}

////////////////////////////////////////////////////////////////////////
// RTL-SDR data parser
////////////////////////////////////////////////////////////////////////

class sample_parser {
public:
    typedef std::function<void (const std::complex<float> *data,
				size_t len)> callback;

    sample_parser(const callback& cb) : _callback(cb), _ptr(0)
    {
	_out.resize(65536);
	_in.resize(_out.size() * 2);
    }

    void feed(const uint8_t *data, size_t len);
    void flush();

private:
    const callback			_callback;

    std::vector<uint8_t>		_in;
    std::vector<std::complex<float>>	_out;
    size_t				_ptr;
};

void sample_parser::feed(const uint8_t *data, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	memcpy(_in.data() + _ptr, data, r);
	data += r;
	len -= r;
	_ptr += r;

	if (_ptr >= _in.size())
	    flush();
    }
}

void sample_parser::flush()
{
    const size_t out_size = _ptr >> 1;

    for (size_t i = 0; i < out_size; i++)
	_out[i] = std::complex<float>(float(_in[i * 2] - 127) / 128.0,
				      float(_in[i * 2 + 1] - 127) / 128.0);

    _ptr = 0;
    _callback(_out.data(), out_size);
}

////////////////////////////////////////////////////////////////////////
// Read data from file
////////////////////////////////////////////////////////////////////////

void read_data(int fd, const std::function<void
		(const uint8_t *data, size_t len)>& callback)
{
    std::vector<uint8_t> data(65536);

    for (;;) {
	const int r = read(fd, data.data(), data.size());

	if (r < 0) {
	    if ((errno == EINTR) || (errno == EAGAIN))
		continue;

	    throw std::system_error(errno, std::system_category());
	}

	if (!r)
	    break;

	callback(data.data(), r);
    }
}

////////////////////////////////////////////////////////////////////////
// UART receiver
////////////////////////////////////////////////////////////////////////

class uart_receiver {
public:
    typedef int32_t event;

    static const event CARRIER_DETECT = -1;
    static const event NO_CARRIER = -2;

    typedef std::function<void (event w)> callback;

    uart_receiver(const callback& cb, float period, unsigned int bpw) :
	_callback(cb),
	_tags(1024),
	_ptr(0),
	_last(0),
	_uart(period, bpw)
    {
    }

    void feed(const tag *tags, size_t len);
    void flush();

private:
    callback			_callback;

    std::vector<tag>		_tags;
    size_t			_ptr;

    tag				_last;
    uart			_uart;
};

void uart_receiver::feed(const tag *tags, size_t len)
{
    while (len) {
	const size_t r = std::min(_tags.size() - _ptr, len);

	std::copy(tags, tags + r, _tags.data() + _ptr);
	tags += r;
	len -= r;
	_ptr += r;

	if (_ptr >= _tags.size())
	    flush();
    }
}

void uart_receiver::flush()
{
    for (size_t i = 0; i < _ptr; i++) {
	const tag t = _tags[i];

	// Carrier events
	if (!(t & TAG_CD)) {
	    if (_last & TAG_CD)
		_callback(NO_CARRIER);
	} else {
	    if (!(_last & TAG_CD)) {
		_callback(CARRIER_DETECT);
		_uart.reset();
	    }

	    if (_uart.feed(t & TAG_RXD))
		_callback(_uart.data());
	}

	_last = t;
    }

    _ptr = 0;
}

////////////////////////////////////////////////////////////////////////
// Level binarizer
////////////////////////////////////////////////////////////////////////

class binarizer {
public:
    typedef std::function<void (const tag *tags, size_t len)> callback;

    binarizer(const callback& cb, size_t period,
	      float separation, float range) :
	_callback(cb),
	_bin_size(separation * 0.5f),
	_offset(range),
	_in(period * 2),
	_tags(_in.size()),
	_ptr(_in.size() / 2),
	_hist_last(round(range * 2.0 / _bin_size)),
	_hist_cur(_hist_last.size()),
	_hist_next(_hist_last.size()),
	_work(_hist_last.size()),
	_state(false)
    {
    }

    void feed(const float *data, const tag *tags, size_t len);
    void flush();

private:
    const callback		_callback;
    const float			_bin_size;
    const float			_offset;

    std::vector<float>		_in;
    std::vector<tag>		_tags;
    size_t			_ptr;

    struct bin {
	float		sum;
	unsigned int	count;

	bin() : sum(0.0f), count(0) { }
    };

    std::vector<bin>		_hist_last;
    std::vector<bin>		_hist_cur;
    std::vector<bin>		_hist_next;
    std::vector<bin>		_work;

    // Schmitt state
    bool			_state;

    void process();
    int next_peak();
};

void binarizer::feed(const float *data, const tag *tags, size_t len)
{
    while (len) {
	const size_t r = std::min(_in.size() - _ptr, len);

	std::copy(data, data + r, _in.data() + _ptr);
	std::copy(tags, tags + r, _tags.data() + _ptr);

	data += r;
	tags += r;
	len -= r;

	_ptr += r;

	if (_ptr >= _in.size())
	    process();
    }
}

void binarizer::flush()
{
    std::fill(_in.begin() + _ptr, _in.end(), 0.0f);
    std::fill(_tags.begin() + _ptr, _tags.end(), 0);
    process();
}

void binarizer::process()
{
    // Initialize current bins
    std::fill(_hist_next.begin(), _hist_next.end(), bin());

    // Count values in the second half of the window
    for (size_t i = _in.size() / 2; i < _in.size(); i++) {
	if (!(_tags[i] & TAG_CD))
	    continue;

	const int b = floor((_in[i] + _offset) / _bin_size);

	if ((b < 0) || (size_t(b) >= _hist_next.size()))
	    continue;

	_hist_next[b].count++;
	_hist_next[b].sum += _in[i];
    }

    // Convolve bins
    bin last;

    last.sum = 0.0f;
    last.count = 0;

    for (size_t i = 0; i < _hist_next.size(); i++) {
	const bin cur = _hist_next[i];

	_hist_next[i].sum += last.sum;
	_hist_next[i].count += last.count;

	last = cur;
    }

    // Combine results over last, current and next period
    for (size_t i = 0; i < _work.size(); i++) {
	_work[i].sum = _hist_last[i].sum +
		_hist_cur[i].sum + _hist_next[i].sum;
	_work[i].count = _hist_last[i].count +
		_hist_cur[i].count + _hist_next[i].count;
    }

    // Squash small bins and calculate averages
    for (auto& x : _work) {
	if (x.count <= _in.size() / 20) {
	    x.count = 0;
	    x.sum = 0.0f;
	} else {
	    x.sum /= float(x.count);
	}
    }

    // Find two largest bins and choose levels
    const int pa = next_peak();
    const int pb = next_peak();

    float low = std::numeric_limits<float>::infinity();
    float high = std::numeric_limits<float>::infinity();
    float radius = 0.0f;

    if ((pa >= 0) && (pb >= 0)) {
	low = _work[pa].sum;
	high = _work[pb].sum;

	if (low > high)
	    std::swap(low, high);

	radius = (high - low) / 3.0f;
    }

    // Schmitt trigger over first half
    for (size_t i = 0; i * 2 < _in.size(); i++) {
	if (!(_tags[i] & TAG_CD))
	    _state = true;
	else if (fabs(low - _in[i]) < radius)
	    _state = false;
	else if (fabs(high - _in[i]) < radius)
	    _state = true;

	if (_state)
	    _tags[i] |= TAG_RXD;
    }

    // Prepare for next period and dispatch results
    _ptr = _in.size() / 2;
    _callback(_tags.data(), _ptr);

    _hist_last.swap(_hist_cur);
    _hist_cur.swap(_hist_next);
    std::copy(_in.begin() + _ptr, _in.end(), _in.begin());
    std::copy(_tags.begin() + _ptr, _tags.end(), _tags.begin());
}

int binarizer::next_peak()
{
    int biggest = -1;

    for (size_t i = 0; i < _work.size(); i++)
	if (_work[i].count &&
	    ((biggest < 0) || (_work[i].count > _work[biggest].count)))
	    biggest = i;

    if (biggest >= 0)
	_work[biggest].count = 0;
    if (biggest > 0)
	_work[biggest - 1].count = 0;
    if (size_t(biggest + 1) < _work.size())
	_work[biggest + 1].count = 0;

    return biggest;
}

////////////////////////////////////////////////////////////////////////
// Word-format
////////////////////////////////////////////////////////////////////////

enum parity_type {
    NONE = 'N',
    EVEN = 'E',
    ODD = 'O'
};

struct word_format {
    unsigned int	data;
    parity_type		parity;
    unsigned int	stop;

    word_format(unsigned int d, parity_type p, unsigned int s) :
	data(d), parity(p), stop(s) { }
};

static const unsigned int raw_word_size(const word_format& w)
{
    return w.data + w.stop + 1 + ((w.parity == NONE) ? 0 : 1);
}

static uart::word reverse_word(uart::word w, unsigned int bits)
{
    uart::word reversed = 0;

    for (unsigned int i = 0; i < bits; i++) {
	reversed = (reversed << 1) | (w & 1);
	w >>= 1;
    }

    return reversed;
}

void print_word(std::ostream& out, const word_format& fmt, uart::word w)
{
    const unsigned int total_size = raw_word_size(fmt);

    // Print data in hex
    {
	uart::word n = (w >> (total_size - 1 - fmt.data)) &
			((uart::word(1) << fmt.data) - 1);

	for (unsigned int i = 0; i < fmt.data; i += 4) {
	    out << "0123456789abcdef"[reverse_word(n & 0xf, 4)];
	    n >>= 4;
	}
    }

    out << ' ';

    // Print raw data in binary
    {
	uart::word n = reverse_word(w, total_size);

	for (unsigned int i = 0; i < total_size; i++) {
	    out << ((n & 1) ? '1' : '-');
	    n >>= 1;
	}
    }

    out << ' ';

    // Check parity, if applicable
    if (fmt.parity != NONE) {
	uart::word n = w >> fmt.stop;
	unsigned int p = 0;

	for (unsigned int i = 0; i < fmt.data + 1; i++) {
	    p ^= n;
	    n >>= 1;
	}

	if (fmt.parity == ODD)
	    p ^= 1;

	if (p & 1)
	    out << " *PARITY-ERROR*";
    }

    if ((~w) & ((uart::word(1) << fmt.stop) - 1))
	out << " *FRAMING-ERROR*";

    if (w >> (total_size - 1))
	out << " *TIMING-ERROR*";
}

////////////////////////////////////////////////////////////////////////
// Command-line parser
////////////////////////////////////////////////////////////////////////

class argument_error : public std::exception {
public:
    argument_error(const char *fmt, ...)
	__attribute__((format (printf, 2, 3)))
    {
	va_list ap;

	va_start(ap, fmt);
	vsnprintf(_message.data(), _message.size(), fmt, ap);
	va_end(ap);
    }

    virtual ~argument_error() throw () { }

    virtual const char *what() const throw ()
    {
	return "Argument error";
    }

    const char *message() const
    {
	return _message.data();
    }

private:
    std::array<char, 128>	_message;
};

struct arguments {
    typedef unsigned int flag;

    static const flag HELP = 0x01;
    static const flag VERSION = 0x02;
    static const flag DEBUG = 0x04;

    flag		flags;

    // Input characteristics
    float		sample_rate;

    // Carrier characteristics
    float		carrier_frequency;
    float		carrier_offset;
    float		carrier_stability;
    float		carrier_tracking_rate;

    // FSK options
    float		fsk_separation;
    float		fsk_max_freq;
    float		fsk_min_freq;

    // UART characteristics
    float		uart_baud;
    word_format		uart_format;

    arguments() :
	flags(0),
	sample_rate(2048000.0f),
	carrier_frequency(-82750.0f),
	carrier_offset(5000.0f),
	carrier_stability(50.0f),
	carrier_tracking_rate(10.0f),
	fsk_separation(150.0f),
	fsk_max_freq(3000.0f),
	fsk_min_freq(1500.0f),
	uart_baud(300.0f),
	uart_format(8, ODD, 1)
    {
    }
};

static void version()
{
    std::cout << "RTL-SDR AFSK UART receiver; 9 Jul 2016\n"
		 "Copyright (C) 2016 Daniel Beer <dlbeer@gmail.com>\n";
}

static void usage(const char *progname)
{
    std::cout << "Usage: " << progname << " [options]\n"
"\n"
"Samples are read from stdin and are assumed to be RTL-SDR format. That\n"
"is, two unsigned bytes (offset 128) per sample. I byte comes first, and\n"
"then Q byte.\n"
"\n"
"General options:\n"
"    --help                    Show this text.\n"
"    --version                 Show version text.\n"
"    --debug                   Dump specified and computed options.\n"
"    --sample-rate F           Set input sample rate.\n"
"\n"
"Carrier tracking options:\n"
"    --carrier-frequency F     Set nominal carrier frequency offset.\n"
"    --carrier-offset F        Maximum allowable carrier error.\n"
"    --carrier-stability F     Maximum allowable short-term carrier\n"
"                              wobble.\n"
"    --carrier-tracking-rate F\n"
"                              Rate at which carrier offset is\n"
"                              estimated.\n"
"\n"
"FSK options:\n"
"    --fsk-separation F        Minimum tone separation.\n"
"    --fsk-min-freq F          Minimum tone frequency.\n"
"    --fsk-max-freq F          Maximum tone frequency.\n"
"\n"
"UART options:\n"
"    --uart-baud F             Set baud rate.\n"
"    --uart-format D,P,S       Set word format (data bits, parity, stop).\n";
}

static const char *expect_arg(int argc, char **argv, int index)
{
    if (index + 1 >= argc)
	throw argument_error("Option requires an argument: %s", argv[index]);

    return argv[index + 1];
}

static parity_type parse_parity(const char *p)
{
    if ((*p == 'N') || (*p == 'n'))
	return NONE;
    if ((*p == 'O') || (*p == 'o'))
	return ODD;
    if ((*p == 'E') || (*p == 'e'))
	return EVEN;

    throw argument_error("Invalid parity: %s", p);
}

static word_format parse_format(const char *f)
{
    std::array<char, 64> copy;
    size_t len = std::min(copy.size() - 1, strlen(f));
    std::array<char *, 3> bits;
    char *sp;

    std::copy(f, f + len, copy.begin());
    copy[len] = 0;

    sp = copy.data();
    for (unsigned int i = 0; i < bits.size(); i++)
	if (!(bits[i] = strsep(&sp, ",")))
	    throw argument_error("Invalid word format: %s", f);

    const unsigned int data_bits = atoi(bits[0]);

    if ((data_bits < 1) || (data_bits > 16))
	throw argument_error("Bad data size: %d", data_bits);

    const unsigned int stop_bits = atoi(bits[2]);

    if ((stop_bits < 1) || (stop_bits > 16))
	throw argument_error("Bad stop bits: %d", stop_bits);

    return word_format(data_bits, parse_parity(bits[1]), stop_bits);
}

static arguments parse_options(int argc, char **argv)
{
    arguments out;

    for (int i = 1; i < argc; i++) {
	const char *a = argv[i];

	if (!strcmp(a, "--help"))
	    out.flags |= arguments::HELP;
	else if (!strcmp(a, "--version"))
	    out.flags |= arguments::VERSION;
	else if (!strcmp(a, "--debug"))
	    out.flags |= arguments::DEBUG;
	else if (!strcmp(a, "--sample-rate"))
	    out.sample_rate = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--carrier-frequency"))
	    out.carrier_frequency = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--carrier-offset"))
	    out.carrier_offset = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--carrier-stability"))
	    out.carrier_stability = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--carrier-tracking-rate"))
	    out.carrier_tracking_rate = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--fsk-separation"))
	    out.fsk_separation = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--fsk-min-freq"))
	    out.fsk_min_freq = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--fsk-max-freq"))
	    out.fsk_max_freq = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--uart-baud"))
	    out.uart_baud = atof(expect_arg(argc, argv, i++));
	else if (!strcmp(a, "--uart-format"))
	    out.uart_format = parse_format(expect_arg(argc, argv, i++));
	else
	    throw argument_error("Unknown option: %s", a);
    }

    return out;
}

////////////////////////////////////////////////////////////////////////
// Simple formatter
////////////////////////////////////////////////////////////////////////

template<size_t N>
class qfmt {
public:
    qfmt(const char *fmt, ...)
	__attribute__((format (printf, 2, 3)))
    {
	va_list ap;

	va_start(ap, fmt);
	vsnprintf(_text.data(), _text.size(), fmt, ap);
	va_end(ap);
    }

    const char *text() const
    {
	return _text.data();
    }

private:
    std::array<char, N>		_text;
};

template<size_t N>
static inline std::ostream& operator<<(std::ostream& out, const qfmt<N>& q)
{
    out << q.text();
    return out;
}

////////////////////////////////////////////////////////////////////////
// Main
////////////////////////////////////////////////////////////////////////

static void print_event(const word_format& f, uart_receiver::event w)
{
    if (w == uart_receiver::CARRIER_DETECT) {
	std::cout << "CONNECT\n";
    } else if (w == uart_receiver::NO_CARRIER) {
	std::cout << "NO CARRIER\n";
    } else {
	std::cout << "> ";
	print_word(std::cout, f, w);
	std::cout << '\n';
    }

    std::cout.flush();
}

static void run(int argc, char **argv)
{
    const auto a = parse_options(argc, argv);

    if (a.flags & arguments::HELP) {
	usage(argv[0]);
	return;
    }

    if (a.flags & arguments::VERSION) {
	version();
	return;
    }

    if (a.flags & arguments::DEBUG) {
	std::cout << "Specified parameters:\n";
	std::cout << qfmt<128>
	    ("  sample_rate            = %15.03f\n", a.sample_rate);
	std::cout << qfmt<128>
	    ("  carrier_frequency      = %15.03f\n", a.carrier_frequency);
	std::cout << qfmt<128>
	    ("  carrier_offset         = %15.03f\n", a.carrier_offset);
	std::cout << qfmt<128>
	    ("  carrier_stability      = %15.03f\n", a.carrier_stability);
	std::cout << qfmt<128>
	    ("  carrier_tracking_rate  = %15.03f\n", a.carrier_tracking_rate);
	std::cout << qfmt<128>
	    ("  fsk_separation         = %15.03f\n", a.fsk_separation);
	std::cout << qfmt<128>
	    ("  fsk_max_freq           = %15.03f\n", a.fsk_max_freq);
	std::cout << qfmt<128>
	    ("  fsk_min_freq           = %15.03f\n", a.fsk_min_freq);
	std::cout << qfmt<128>
	    ("  uart_baud              = %15.03f\n", a.uart_baud);
	std::cout << qfmt<128>
	    ("  uart_format            = %d,%c,%d\n",
	     a.uart_format.data, a.uart_format.parity, a.uart_format.stop);
    }

    const float crate_req_tone =
	(a.fsk_max_freq + a.carrier_offset + a.carrier_stability) * 2.0f;
    const float crate_req_baud = a.uart_baud * 20.0f;
    const float crate_required =
	std::max(crate_req_tone, crate_req_baud) * 1.5f;
    const unsigned int decimation =
	floor(a.sample_rate / crate_required);
    const float crate = a.sample_rate / decimation;

    if (a.flags & arguments::DEBUG) {
	std::cout << "Computed parameters:\n";
	std::cout << qfmt<128>
	    ("  crate_req_tone         = %15.03f\n", crate_req_tone);
	std::cout << qfmt<128>
	    ("  crate_req_baud         = %15.03f\n", crate_req_baud);
	std::cout << qfmt<128>
	    ("  decimation             = %15d\n", decimation);
	std::cout << qfmt<128>
	    ("  crate                  = %15.03f\n", crate);
	std::cout.flush();
    }

    // Build the processing chain
    uart_receiver uart(
	[&a](uart_receiver::event w) {
	    print_event(a.uart_format, w);
	}, crate / a.uart_baud, raw_word_size(a.uart_format));

    binarizer bin(
	[&uart](const tag *t, size_t len) {
	    uart.feed(t, len);
	}, crate / a.uart_baud * raw_word_size(a.uart_format) * 2.0f,
	   a.fsk_separation / crate,
	   a.fsk_max_freq / crate);

    lp_filter<float> denoise(
	[&bin](const float *f, const tag *t, size_t len) {
	    bin.feed(f, t, len);
	}, a.uart_baud * 5.0f / crate);

    freq_detector fmd(
	[&denoise](const float *f, const tag *t, size_t len) {
	    denoise.feed(f, t, len);
	});

    lp_filter<std::complex<float>> amd(
	[&fmd](const std::complex<float> *f, const tag *t, size_t len) {
	    fmd.feed(f, t, len);
	}, (a.fsk_max_freq - a.fsk_min_freq) * 0.5f / crate);

    cd_tracker cdt(
	[&amd](const std::complex<float> *f, const tag *t, size_t len) {
	    amd.feed(f, t, len);
	}, round(crate / a.carrier_tracking_rate),
	   a.carrier_stability / crate,
	   (a.fsk_min_freq + a.fsk_max_freq) * -0.5f / crate);

    rf_decimator dec(
	[&cdt](const std::complex<float> *f, size_t len) {
	    cdt.feed(f, len);
	}, a.carrier_frequency / a.sample_rate,
	   decimation);

    sample_parser parse(
	[&dec](const std::complex<float> *f, size_t len) {
	    dec.feed(f, len);
	});

    // Feed data
    read_data(0,
	[&parse](const uint8_t *d, size_t len) {
	    parse.feed(d, len);
	});

    // Terminate processing
    parse.flush();
    dec.flush();
    cdt.flush();
    amd.flush();
    fmd.flush();
    denoise.flush();
    bin.flush();
    uart.flush();
}

int main(int argc, char **argv)
{
    try {
	run(argc, argv);
    }
    catch (const argument_error& e) {
	std::cerr << qfmt<128>("Argument error: %s\n", e.message());
	return -1;
    }
    catch (const std::system_error& e) {
	std::cerr << qfmt<128>("System error: %s (%d)\n",
		e.code().message().c_str(), e.code().value());
	return -2;
    }

    return 0;
}