#!/usr/bin/env python

from gnuradio import gr
from gnuradio import audio
from gnuradio import trellis, digital, blocks
from gnuradio import eng_notation
import math
import sys
import random
import fsm_utils

try:
    from gnuradio import analog
except ImportError:
    sys.stderr.write("Error: Program requires gr-analog.\n")
    sys.exit(1)


def run_test(f, Kb, bitspersymbol, K, dimensionality, constellation, N0, seed):
    tb = gr.top_block()

    # TX
    src = blocks.lfsr_32k_source_s()
    src_head = blocks.head(gr.sizeof_short, Kb / 16)  # packet size in shorts
    # unpack shorts to symbols compatible with the FSM input cardinality
    s2fsmi = blocks.packed_to_unpacked_ss(bitspersymbol, gr.GR_MSB_FIRST)
    enc = trellis.encoder_ss(f, 0)  # initial state = 0
    mod = digital.chunks_to_symbols_sf(constellation, dimensionality)

    # CHANNEL
    add = blocks.add_ff()
    noise = analog.noise_source_f(analog.GR_GAUSSIAN, math.sqrt(N0 / 2), seed)

    # RX
    # data preprocessing to generate metrics for Viterbi
    metrics = trellis.metrics_f(
        f.O(), dimensionality, constellation, digital.TRELLIS_EUCLIDEAN)
    # Put -1 if the Initial/Final states are not set.
    va = trellis.viterbi_s(f, K, 0, -1)
    fsmi2s = blocks.unpacked_to_packed_ss(
        bitspersymbol, gr.GR_MSB_FIRST)  # pack FSM input symbols to shorts
    dst = blocks.check_lfsr_32k_s()

    tb.connect(src, src_head, s2fsmi, enc, mod)
    tb.connect(mod, (add, 0))
    tb.connect(noise, (add, 1))
    tb.connect(add, metrics)
    tb.connect(metrics, va, fsmi2s, dst)

    tb.run()

    # A bit of cheating: run the program once and print the
    # final encoder state.
    # Then put it as the last argument in the viterbi block
    # print "final state = " , enc.ST()

    ntotal = dst.ntotal()
    nright = dst.nright()
    runlength = dst.runlength()
    return (ntotal, ntotal - nright)


def main(args):
    nargs = len(args)
    if nargs == 3:
        fname = args[0]
        esn0_db = float(args[1])  # Es/No in dB
        # number of times the experiment is run to collect enough errors
        rep = int(args[2])
    else:
        sys.stderr.write(
            'usage: test_tcm.py fsm_fname Es/No_db  repetitions\n')
        sys.exit(1)

    # system parameters
    f = trellis.fsm(fname)  # get the FSM specification from a file
    # packet size in bits (make it multiple of 16 so it can be packed in a short)
    Kb = 1024 * 16
    # bits per FSM input symbol
    bitspersymbol = int(round(math.log(f.I()) / math.log(2)))
    K = Kb / bitspersymbol  # packet size in trellis steps
    modulation = fsm_utils.psk4  # see fsm_utlis.py for available predefined modulations
    dimensionality = modulation[0]
    constellation = modulation[1]
    if len(constellation) / dimensionality != f.O():
        sys.stderr.write(
            'Incompatible FSM output cardinality and modulation size.\n')
        sys.exit(1)
    # calculate average symbol energy
    Es = 0
    for i in range(len(constellation)):
        Es = Es + constellation[i]**2
    Es = Es / (len(constellation) // dimensionality)
    N0 = Es / pow(10.0, esn0_db / 10.0)  # noise variance

    tot_s = 0
    terr_s = 0
    for i in range(rep):
        # run experiment with different seed to get different noise realizations
        (s, e) = run_test(f, Kb, bitspersymbol, K,
                          dimensionality, constellation, N0, -int(666 + i))
        tot_s = tot_s + s
        terr_s = terr_s + e
        if (i % 100 == 0):
            print(i, s, e, tot_s, terr_s, '%e' % ((1.0 * terr_s) / tot_s))
    # estimate of the (short) error rate
    print(tot_s, terr_s, '%e' % ((1.0 * terr_s) / tot_s))


if __name__ == '__main__':
    main(sys.argv[1:])