/*
 * Copyright 1995 Phil Karn, KA9Q
 * Copyright 2008,2018 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 */

/*
 * Viterbi decoder for K=7 rate=1/2 convolutional code
 * Some modifications from original Karn code by Matt Ettus
 */

#include <gnuradio/fec/viterbi.h>

/* The two generator polynomials for the NASA Standard K=7 code.
 * Since these polynomials are known to be optimal for this constraint
 * length there is not much point in changing them. But if you do, you
 * will have to regenerate the BUTTERFLY macro calls in viterbi()
 */
#define POLYA 0x6d
#define POLYB 0x4f

/* The basic Viterbi decoder operation, called a "butterfly"
 * operation because of the way it looks on a trellis diagram. Each
 * butterfly involves an Add-Compare-Select (ACS) operation on the two nodes
 * where the 0 and 1 paths from the current node merge at the next step of
 * the trellis.
 *
 * The code polynomials are assumed to have 1's on both ends. Given a
 * function encode_state() that returns the two symbols for a given
 * encoder state in the low two bits, such a code will have the following
 * identities for even 'n' < 64:
 *
 * 	encode_state(n) = encode_state(n+65)
 *	encode_state(n+1) = encode_state(n+64) = (3 ^ encode_state(n))
 *
 * Any convolutional code you would actually want to use will have
 * these properties, so these assumptions aren't too limiting.
 *
 * Doing this as a macro lets the compiler evaluate at compile time the
 * many expressions that depend on the loop index and encoder state and
 * emit them as immediate arguments.
 * This makes an enormous difference on register-starved machines such
 * as the Intel x86 family where evaluating these expressions at runtime
 * would spill over into memory.
 */
#define BUTTERFLY(i, sym)                                         \
    {                                                             \
        long m0, m1;                                              \
                                                                  \
        /* ACS for 0 branch */                                    \
        m0 = state[i].metric + mets[sym];          /* 2*i */      \
        m1 = state[i + 32].metric + mets[3 ^ sym]; /* 2*i + 64 */ \
        if (m0 > m1) {                                            \
            next[2 * i].metric = m0;                              \
            next[2 * i].path = state[i].path << 1;                \
        } else {                                                  \
            next[2 * i].metric = m1;                              \
            next[2 * i].path = (state[i + 32].path << 1) | 1;     \
        }                                                         \
        /* ACS for 1 branch */                                    \
        m0 = state[i].metric + mets[3 ^ sym];  /* 2*i + 1 */      \
        m1 = state[i + 32].metric + mets[sym]; /* 2*i + 65 */     \
        if (m0 > m1) {                                            \
            next[2 * i + 1].metric = m0;                          \
            next[2 * i + 1].path = state[i].path << 1;            \
        } else {                                                  \
            next[2 * i + 1].metric = m1;                          \
            next[2 * i + 1].path = (state[i + 32].path << 1) | 1; \
        }                                                         \
    }

extern unsigned char Partab[]; /* Parity lookup table */

namespace gr {
namespace fec {

/* Convolutionally encode data into binary symbols */
unsigned char encode(unsigned char* symbols,
                     const unsigned char* data,
                     unsigned int nbytes,
                     unsigned char encstate)
{
    int i;

    while (nbytes-- != 0) {
        for (i = 7; i >= 0; i--) {
            encstate = (encstate << 1) | ((*data >> i) & 1);
            *symbols++ = Partab[encstate & POLYA];
            *symbols++ = Partab[encstate & POLYB];
        }
        data++;
    }

    return encstate;
}

/* Viterbi decoder */
int viterbi(unsigned long* metric,  /* Final path metric (returned value) */
            unsigned char* data,    /* Decoded output data */
            unsigned char* symbols, /* Raw deinterleaved input symbols */
            unsigned int nbits,     /* Number of output bits */
            int mettab[2][256]      /* Metric table, [sent sym][rx symbol] */
)
{
    unsigned int bitcnt = 0;
    int mets[4];
    long bestmetric;
    int beststate, i;
    struct viterbi_state state0[64], state1[64], *state, *next;

    state = state0;
    next = state1;

    /* Initialize starting metrics to prefer 0 state */
    state[0].metric = 0;
    for (i = 1; i < 64; i++)
        state[i].metric = -999999;
    state[0].path = 0;

    for (bitcnt = 0; bitcnt < nbits; bitcnt++) {
        /* Read input symbol pair and compute all possible branch
         * metrics
         */
        mets[0] = mettab[0][symbols[0]] + mettab[0][symbols[1]];
        mets[1] = mettab[0][symbols[0]] + mettab[1][symbols[1]];
        mets[2] = mettab[1][symbols[0]] + mettab[0][symbols[1]];
        mets[3] = mettab[1][symbols[0]] + mettab[1][symbols[1]];
        symbols += 2;

        /* These macro calls were generated by genbut.c */
        BUTTERFLY(0, 0);
        BUTTERFLY(1, 1);
        BUTTERFLY(2, 3);
        BUTTERFLY(3, 2);
        BUTTERFLY(4, 3);
        BUTTERFLY(5, 2);
        BUTTERFLY(6, 0);
        BUTTERFLY(7, 1);
        BUTTERFLY(8, 0);
        BUTTERFLY(9, 1);
        BUTTERFLY(10, 3);
        BUTTERFLY(11, 2);
        BUTTERFLY(12, 3);
        BUTTERFLY(13, 2);
        BUTTERFLY(14, 0);
        BUTTERFLY(15, 1);
        BUTTERFLY(16, 2);
        BUTTERFLY(17, 3);
        BUTTERFLY(18, 1);
        BUTTERFLY(19, 0);
        BUTTERFLY(20, 1);
        BUTTERFLY(21, 0);
        BUTTERFLY(22, 2);
        BUTTERFLY(23, 3);
        BUTTERFLY(24, 2);
        BUTTERFLY(25, 3);
        BUTTERFLY(26, 1);
        BUTTERFLY(27, 0);
        BUTTERFLY(28, 1);
        BUTTERFLY(29, 0);
        BUTTERFLY(30, 2);
        BUTTERFLY(31, 3);

        /* Swap current and next states */
        if (bitcnt & 1) {
            state = state0;
            next = state1;
        } else {
            state = state1;
            next = state0;
        }
        // ETTUS
        // if(bitcnt > nbits-7){
        /* In tail, poison non-zero nodes */
        // for(i=1;i<64;i += 2)
        //	state[i].metric = -9999999;
        //}
        /* Produce output every 8 bits once path memory is full */
        if ((bitcnt % 8) == 5 && bitcnt > 32) {
            /* Find current best path */
            bestmetric = state[0].metric;
            beststate = 0;
            for (i = 1; i < 64; i++) {
                if (state[i].metric > bestmetric) {
                    bestmetric = state[i].metric;
                    beststate = i;
                }
            }
#ifdef notdef
            printf("metrics[%d] = %d state = %lx\n",
                   beststate,
                   state[beststate].metric,
                   state[beststate].path);
#endif
            *data++ = state[beststate].path >> 24;
        }
    }
    /* Output remaining bits from 0 state */
    // ETTUS  Find best state instead
    bestmetric = state[0].metric;
    beststate = 0;
    for (i = 1; i < 64; i++) {
        if (state[i].metric > bestmetric) {
            bestmetric = state[i].metric;
            beststate = i;
        }
    }
    if ((i = bitcnt % 8) != 6)
        state[beststate].path <<= 6 - i;

    *data++ = state[beststate].path >> 24;
    *data++ = state[beststate].path >> 16;
    *data++ = state[beststate].path >> 8;
    *data = state[beststate].path;
    // printf ("BS = %d\tBSM = %d\tM0 =
    // %d\n",beststate,state[beststate].metric,state[0].metric);
    *metric = state[beststate].metric;
    return 0;
}


void viterbi_chunks_init(struct viterbi_state* state)
{
    // Initialize starting metrics to prefer 0 state
    int i;
    state[0].metric = 0;
    state[0].path = 0;
    for (i = 1; i < 64; i++)
        state[i].metric = -999999;
}

void viterbi_butterfly8(unsigned char* symbols,
                        int mettab[2][256],
                        struct viterbi_state* state0,
                        struct viterbi_state* state1)
{
    unsigned int bitcnt;
    int mets[4];

    struct viterbi_state *state, *next;
    state = state0;
    next = state1;
    // Operate on 16 symbols (8 bits) at a time
    for (bitcnt = 0; bitcnt < 8; bitcnt++) {
        // Read input symbol pair and compute all possible branch metrics
        mets[0] = mettab[0][symbols[0]] + mettab[0][symbols[1]];
        mets[1] = mettab[0][symbols[0]] + mettab[1][symbols[1]];
        mets[2] = mettab[1][symbols[0]] + mettab[0][symbols[1]];
        mets[3] = mettab[1][symbols[0]] + mettab[1][symbols[1]];
        symbols += 2;

        // These macro calls were generated by genbut.c
        BUTTERFLY(0, 0);
        BUTTERFLY(1, 1);
        BUTTERFLY(2, 3);
        BUTTERFLY(3, 2);
        BUTTERFLY(4, 3);
        BUTTERFLY(5, 2);
        BUTTERFLY(6, 0);
        BUTTERFLY(7, 1);
        BUTTERFLY(8, 0);
        BUTTERFLY(9, 1);
        BUTTERFLY(10, 3);
        BUTTERFLY(11, 2);
        BUTTERFLY(12, 3);
        BUTTERFLY(13, 2);
        BUTTERFLY(14, 0);
        BUTTERFLY(15, 1);
        BUTTERFLY(16, 2);
        BUTTERFLY(17, 3);
        BUTTERFLY(18, 1);
        BUTTERFLY(19, 0);
        BUTTERFLY(20, 1);
        BUTTERFLY(21, 0);
        BUTTERFLY(22, 2);
        BUTTERFLY(23, 3);
        BUTTERFLY(24, 2);
        BUTTERFLY(25, 3);
        BUTTERFLY(26, 1);
        BUTTERFLY(27, 0);
        BUTTERFLY(28, 1);
        BUTTERFLY(29, 0);
        BUTTERFLY(30, 2);
        BUTTERFLY(31, 3);

        // Swap current and next states
        if (bitcnt & 1) {
            state = state0;
            next = state1;
        } else {
            state = state1;
            next = state0;
        }
    }
}

void viterbi_butterfly2(unsigned char* symbols,
                        int mettab[2][256],
                        struct viterbi_state* state0,
                        struct viterbi_state* state1)
{
    // unsigned int bitcnt;
    int mets[4];

    struct viterbi_state *state, *next;
    state = state0;
    next = state1;
    // Operate on 4 symbols (2 bits) at a time

    // Read input symbol pair and compute all possible branch metrics
    mets[0] = mettab[0][symbols[0]] + mettab[0][symbols[1]];
    mets[1] = mettab[0][symbols[0]] + mettab[1][symbols[1]];
    mets[2] = mettab[1][symbols[0]] + mettab[0][symbols[1]];
    mets[3] = mettab[1][symbols[0]] + mettab[1][symbols[1]];

    // These macro calls were generated by genbut.c
    BUTTERFLY(0, 0);
    BUTTERFLY(1, 1);
    BUTTERFLY(2, 3);
    BUTTERFLY(3, 2);
    BUTTERFLY(4, 3);
    BUTTERFLY(5, 2);
    BUTTERFLY(6, 0);
    BUTTERFLY(7, 1);
    BUTTERFLY(8, 0);
    BUTTERFLY(9, 1);
    BUTTERFLY(10, 3);
    BUTTERFLY(11, 2);
    BUTTERFLY(12, 3);
    BUTTERFLY(13, 2);
    BUTTERFLY(14, 0);
    BUTTERFLY(15, 1);
    BUTTERFLY(16, 2);
    BUTTERFLY(17, 3);
    BUTTERFLY(18, 1);
    BUTTERFLY(19, 0);
    BUTTERFLY(20, 1);
    BUTTERFLY(21, 0);
    BUTTERFLY(22, 2);
    BUTTERFLY(23, 3);
    BUTTERFLY(24, 2);
    BUTTERFLY(25, 3);
    BUTTERFLY(26, 1);
    BUTTERFLY(27, 0);
    BUTTERFLY(28, 1);
    BUTTERFLY(29, 0);
    BUTTERFLY(30, 2);
    BUTTERFLY(31, 3);

    state = state1;
    next = state0;

    // Read input symbol pair and compute all possible branch metrics
    mets[0] = mettab[0][symbols[2]] + mettab[0][symbols[3]];
    mets[1] = mettab[0][symbols[2]] + mettab[1][symbols[3]];
    mets[2] = mettab[1][symbols[2]] + mettab[0][symbols[3]];
    mets[3] = mettab[1][symbols[2]] + mettab[1][symbols[3]];

    // These macro calls were generated by genbut.c
    BUTTERFLY(0, 0);
    BUTTERFLY(1, 1);
    BUTTERFLY(2, 3);
    BUTTERFLY(3, 2);
    BUTTERFLY(4, 3);
    BUTTERFLY(5, 2);
    BUTTERFLY(6, 0);
    BUTTERFLY(7, 1);
    BUTTERFLY(8, 0);
    BUTTERFLY(9, 1);
    BUTTERFLY(10, 3);
    BUTTERFLY(11, 2);
    BUTTERFLY(12, 3);
    BUTTERFLY(13, 2);
    BUTTERFLY(14, 0);
    BUTTERFLY(15, 1);
    BUTTERFLY(16, 2);
    BUTTERFLY(17, 3);
    BUTTERFLY(18, 1);
    BUTTERFLY(19, 0);
    BUTTERFLY(20, 1);
    BUTTERFLY(21, 0);
    BUTTERFLY(22, 2);
    BUTTERFLY(23, 3);
    BUTTERFLY(24, 2);
    BUTTERFLY(25, 3);
    BUTTERFLY(26, 1);
    BUTTERFLY(27, 0);
    BUTTERFLY(28, 1);
    BUTTERFLY(29, 0);
    BUTTERFLY(30, 2);
    BUTTERFLY(31, 3);
}

unsigned char viterbi_get_output(struct viterbi_state* state, unsigned char* outbuf)
{
    // Produce output every 8 bits once path memory is full
    //  if((bitcnt % 8) == 5 && bitcnt > 32) {

    //  Find current best path
    unsigned int i, beststate;
    int bestmetric;

    bestmetric = state[0].metric;
    beststate = 0;
    for (i = 1; i < 64; i++)
        if (state[i].metric > bestmetric) {
            bestmetric = state[i].metric;
            beststate = i;
        }
    *outbuf = state[beststate].path >> 24;
    return bestmetric;
}


// printf ("BS = %d\tBSM = %d\tM0 =
// %d\n",beststate,state[beststate].metric,state[0].metric);
// In tail, poison non-zero nodes
// if(bits_out > packet_size-7)
//  for(i=1;i<64;i += 2)
//    state[i].metric = -9999999;

} // namespace fec
} // namespace gr
