#!/usr/bin/env python

# Usage:
#
# Adjust the center frequency (-f) and gain (-g) according to your needs.


import sys
import math
from gnuradio import gr, gru, eng_notation, blocks, filter, digital, analog
from gnuradio.eng_option import eng_option
from optparse import OptionParser
import osmosdr
import time
import threading
import subprocess
from gnuradio.filter import firdes

# Target: 175, -50, -125, 575 kHz

class top_block(gr.top_block):
  def __init__(self):
    gr.top_block.__init__(self)

    self.src = osmosdr.source("rtl=0")

    self.src.set_center_freq(424150e3)
    self.src.set_sample_rate(1800e3)
    self.src.set_freq_corr(15, 0)

    self.src.set_gain_mode(False)
    self.src.set_gain(22)
    self.src.set_if_gain(22)

    self.tuners = []
    self.afc_probes = []

#    for f in [7, -2, -5, 23]:
    for f in [7, -5, -2, 23]:

        tuner = filter.freq_xlating_fir_filter_ccf(50,
                                                   firdes.root_raised_cosine(1, 1800*1000, 18200, 0.35, 79),
                                                   f*25000, 1800*1000)

        self.tuners.append(tuner)

        mpsk = digital.mpsk_receiver_cc(
                    4, math.pi/4, math.pi/100.0, -0.5, 0.5, 0.25, 0.001, 2, 0.001, 0.001)
        diff_phasor = digital.diff_phasor_cc()
        complex_to_arg = blocks.complex_to_arg(1)
        multiply_const = blocks.multiply_const_vff((2./math.pi, ))
        add_const = blocks.add_const_vff((1.5, ))
        float_to_uchar = blocks.float_to_uchar()
        map_bits = digital.map_bb(([3, 2, 0, 1, 3]))
        unpack_k_bits = blocks.unpack_k_bits_bb(2)

        a = abs(f)

        sink = blocks.file_sink(gr.sizeof_char, "/root/tetra-tmp/fifo/bits%i"%a, False)
        #sink.set_unbuffered(True)

        self.connect((self.src, 0), (tuner, 0),
            (mpsk, 0),
            (diff_phasor, 0),
            (complex_to_arg, 0),
            (multiply_const, 0),
            (add_const, 0),
            (float_to_uchar, 0),
            (map_bits, 0),
            (unpack_k_bits, 0),
            (sink, 0))

        if f == 7:
            afc_decimation = 32000
            afc_demod = analog.quadrature_demod_cf(1800000/50/(2*math.pi*afc_decimation))
            integrate = blocks.integrate_ff(afc_decimation)
            afc_probe = blocks.probe_signal_f()
            self.afc_probes.append(afc_probe)

            self.connect((tuner, 0), (afc_demod,0))
            self.connect((afc_demod, 0), (integrate,0))
            self.connect((integrate, 0), (afc_probe, 0))

    def _variable_function_probe_0_probe():
        while True:
            time.sleep(5)
            for ch in range(0,len(self.tuners)):
                err = self.afc_probes[0].level()
                if abs(err) < 100:
                    continue
                freq = self.tuners[ch].center_freq() + err * 0.2
                self.tuners[ch].set_center_freq(freq)
                sys.stderr.write("Chan %d freq err: %5.0f\tfreq: %f\n" % (ch, err, freq))
            sys.stderr.write("\n")
            time.sleep(50)
    _variable_function_probe_0_thread = threading.Thread(target=_variable_function_probe_0_probe)
    _variable_function_probe_0_thread.daemon = True
    _variable_function_probe_0_thread.start()



if __name__ == '__main__':
        tb = top_block()
#        tb.run(True)
        tb.start()
        tb.wait()