/*---------------------------------------------------------------------------*\

  FILE........: horus_l2.c
  AUTHOR......: David Rowe
  DATE CREATED: Dec 2015

  Horus telemetry layer 2 processing.  Takes an array of 8 bit payload
  data, generates parity bits for a (23,12) Golay code, interleaves
  data and parity bits, pre-pends a Unique Word for modem sync.
  Caller is responsible for providing storage for output packet.

  1/ Unit test on a PC:

     $ gcc horus_l2.c golay23.c H_128_384_23.c H_256_768_22.c mpdecode_core.c phi0.c -o horus_l2 -Wall -DHORUS_L2_UNITTEST
     $ ./horus_l2

     test 0: 22 bytes of payload data BER: 0.00 errors: 0
     test 0: 22 bytes of payload data BER: 0.01 errors: 0
     test 0: 22 bytes of payload data BER: 0.05 errors: 0
     test 0: 22 bytes of payload data BER: 0.10 errors: 7
     
     This indicates it's correcting all channel errors for 22 bytes of
     payload data, at bit error rate (BER) of 0, 0.01, 0.05.  It falls
     over at a BER of 0.10 which is expected.

  2/ To build with just the tx function, ie for linking with the payload
  firmware:

    $ gcc horus_l2.c golay23.c -c -Wall
    
  By default the RX side is #ifdef-ed out, leaving the minimal amount
  of code for tx.

  3/ Generate some tx_bits as input for testing with fsk_horus:
 
    $ gcc horus_l2.c golay23.c -o horus_l2 -Wall -DGEN_TX_BITS -DSCRAMBLER -DINTERLEAVER
    $ ./horus_l2
    $ more ../octave/horus_tx_bits_binary.txt

  4/ Streaming test bits to stdout, for 'live' testing with fsk_mod and horus_demod:

    $ gcc horus_l2.c golay23.c H_128_384_23.c H_256_768_22.c mpdecode_core.c phi0.c -o horus_l2 -Wall -DGEN_TX_BITSTREAM -DSCRAMBLER -DINTERLEAVER
    $ cp horus_l2 ../build/src/
    $ cd ../build/src/
    $ ./horus_l2 100 0 | ./fsk_mod 4 48000 100 750 250 - - | ./horus_demod -m binary - -

    Testing LDPC encode/decode
    $ ./horus_l2 100 1 | ../build/src/fsk_mod 2 48000 100 750 250 - - | ../build/src/cohpsk_ch - - -26 --Fs 48000 --ssbfilt 0 | ../build/src/horus_demod -m 256bit --tonespacing=250 - - | wc -l
   
  5/ Unit testing interleaver:

    $ gcc horus_l2.c golay23.c H_128_384_23.c H_256_768_22.c mpdecode_core.c phi0.c -o horus_l2 -Wall -DINTERLEAVER -DTEST_INTERLEAVER -DSCRAMBLER

  6/ Compile for use as decoder called by fsk_horus.m and fsk_horus_stream.m:

    $ gcc horus_l2.c golay23.c -o horus_l2 -Wall -DDEC_RX_BITS -DHORUS_L2_RX

\*---------------------------------------------------------------------------*/

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include "mpdecode_core.h"
#include "horus_l2.h"
#include "golay23.h"
//#include "H_128_384_23.h"
//#include "H_256_768_22.h"

#ifdef HORUS_L2_UNITTEST
#define HORUS_L2_RX
#endif

static const char uw[] = {'$','$'}; // UW for Horus Binary v1 and v2 Golay modes
//static const char uw_v2[] =  {0x96, 0x69, 0x69, 0x96}; // UW for Horus Binary v2 modes - DEPRECATED.

/* Function Prototypes ------------------------------------------------*/

#ifdef INTERLEAVER
static void interleave(unsigned char *inout, int nbytes, int dir);
#endif
#ifdef SCRAMBLER
static void scramble(unsigned char *inout, int nbytes);
#endif

/* Functions ----------------------------------------------------------*/

/*
   We are using a Golay (23,12) code which has a codeword 23 bits
   long.  The tx packet format is:

      | Unique Word | payload data bits | parity bits |

   This function works out how much storage the caller of
   horus_l2_encode_tx_packet() will need to store the tx packet
 */

int horus_l2_get_num_tx_data_bytes(int num_payload_data_bytes) {
    int num_payload_data_bits, num_golay_codewords;
    int num_tx_data_bits, num_tx_data_bytes;
    
    num_payload_data_bits = num_payload_data_bytes*8;
    num_golay_codewords = num_payload_data_bits/12;
    if (num_payload_data_bits % 12) /* round up to 12 bits, may mean some unused bits */
        num_golay_codewords++;

    num_tx_data_bits = sizeof(uw)*8 + num_payload_data_bits + num_golay_codewords*11;
    num_tx_data_bytes = num_tx_data_bits/8;
    if (num_tx_data_bits % 8) /* round up to nearest byte, may mean some unused bits */
        num_tx_data_bytes++;
    
    #ifdef DEBUG0
    fprintf(stderr, "\nnum_payload_data_bytes: %d\n", num_payload_data_bytes);
    fprintf(stderr, "num_golay_codewords...: %d\n", num_golay_codewords);
    fprintf(stderr, "num_tx_data_bits......: %d\n", num_tx_data_bits);
    fprintf(stderr, "num_tx_data_bytes.....: %d\n\n", num_tx_data_bytes);
    #endif

    return num_tx_data_bytes;
}

void horus_l2_init(void) {
    golay23_init();
}

/*
  Takes an array of payload data bytes, prepends a unique word and appends
  parity bits.

  The encoder will run on the payload on a small 8-bit uC.  As we are
  memory constrained so we do a lot of burrowing for bits out of
  packed arrays, and don't use a LUT for Golay encoding.  Hopefully it
  will run fast enough.  This was quite difficult to get going,
  suspect there is a better way to write this.  Oh well, have to start
  somewhere.
 */

int horus_l2_encode_tx_packet(unsigned char *output_tx_data,
                              unsigned char *input_payload_data,
                              int            num_payload_data_bytes)
{
    int            num_tx_data_bytes, num_payload_data_bits;
    unsigned char *pout = output_tx_data;
    int            ninbit, ningolay, nparitybits;
    int32_t        ingolay, paritybyte, inbit, golayparity;
    int            ninbyte, shift, golayparitybit, i;

    num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(num_payload_data_bytes);
    memcpy(pout, uw, sizeof(uw)); pout += sizeof(uw);
    memcpy(pout, input_payload_data, num_payload_data_bytes); pout += num_payload_data_bytes;

    /* Read input bits one at a time.  Fill input Golay codeword.  Find output Golay codeword.
       Write this to parity bits.  Write parity bytes when we have 8 parity bits.  Bits are
       written MSB first. */

    num_payload_data_bits = num_payload_data_bytes*8;
    ninbit = 0;
    ingolay = 0;
    ningolay = 0;
    paritybyte = 0;
    nparitybits = 0;

    while (ninbit < num_payload_data_bits) {

        /* extract input data bit */

        ninbyte = ninbit/8;
        shift = 7 - (ninbit % 8);
        inbit = (input_payload_data[ninbyte] >> shift) & 0x1;
        #ifdef DEBUG1
        fprintf(stderr, "inbit %d ninbyte: %d inbyte: 0x%02x inbit: %d\n", 
                ninbit, ninbyte, input_payload_data[ninbyte], inbit);
        #endif
        ninbit++;

        /* build up input golay codeword */

        ingolay = ingolay | inbit;
        ningolay++;

        /* when we get 12 bits do a Golay encode */

        if (ningolay % 12) {
            ingolay <<= 1;
        }
        else {
            #ifdef DEBUG0
            fprintf(stderr, "  ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay);
            #endif
            golayparity = golay23_syndrome(ingolay<<11);
            ingolay = 0;

            #ifdef DEBUG0
            fprintf(stderr, "  golayparity: 0x%04x\n", golayparity);
            #endif

            /* write parity bits to output data */

            for (i=0; i<11; i++) {
                golayparitybit = (golayparity >> (10-i)) & 0x1;
                paritybyte = paritybyte | golayparitybit;
                #ifdef DEBUG0
                fprintf(stderr, "    i: %d golayparitybit: %d paritybyte: 0x%02x\n", 
                        i, golayparitybit, paritybyte);
                #endif
                nparitybits++;
                if (nparitybits % 8) {
                   paritybyte <<= 1;
                }
                else {
                    /* OK we have a full byte ready */
                    *pout = paritybyte;
                    #ifdef DEBUG0
                    fprintf(stderr,"      Write paritybyte: 0x%02x\n", paritybyte);
                    #endif
                    pout++;
                    paritybyte = 0;
                }
            }
        }
    } /* while(.... */


    /* Complete final Golay encode, we may have partially finished ingolay, paritybyte */

    #ifdef DEBUG0
    fprintf(stderr, "finishing up .....\n");
    #endif

    if (ningolay % 12) {
        ingolay >>= 1;
        golayparity = golay23_syndrome(ingolay<<12);
        #ifdef DEBUG0
        fprintf(stderr, "  ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay);
        fprintf(stderr, "  golayparity: 0x%04x\n", golayparity);
        #endif

        /* write parity bits to output data */

        for (i=0; i<11; i++) {
            golayparitybit = (golayparity >> (10 - i)) & 0x1;
            paritybyte = paritybyte | golayparitybit;
            #ifdef DEBUG1
            fprintf(stderr, "    i: %d golayparitybit: %d paritybyte: 0x%02x\n", 
                    i, golayparitybit, paritybyte);
            #endif
            nparitybits++;
            if (nparitybits % 8) {
                paritybyte <<= 1;
            }
            else {
                /* OK we have a full byte ready */
                *pout++ = (unsigned char)paritybyte;
                #ifdef DEBUG0
                fprintf(stderr,"      Write paritybyte: 0x%02x\n", paritybyte);
                #endif
                paritybyte = 0;
            }
        }
    }
 
    /* and final, partially complete, parity byte */

    if (nparitybits % 8) {
        paritybyte <<= 7 - (nparitybits % 8);  // use MS bits first
        *pout++ = (unsigned char)paritybyte;
        #ifdef DEBUG0
        fprintf(stderr,"      Write last paritybyte: 0x%02x nparitybits: %d \n", paritybyte, nparitybits);
        #endif
    }

    #ifdef DEBUG0
    fprintf(stderr, "\npout - output_tx_data: %ld num_tx_data_bytes: %d\n",
            pout - output_tx_data, num_tx_data_bytes);
    #endif
    assert(pout == (output_tx_data + num_tx_data_bytes));

    /* optional interleaver - we dont interleave UW */

    #ifdef INTERLEAVER
    interleave(&output_tx_data[sizeof(uw)], num_tx_data_bytes-2, 0);
    #endif

    /* optional scrambler to prevent long strings of the same symbol
       which upsets the modem - we dont scramble UW */

    #ifdef SCRAMBLER
    scramble(&output_tx_data[sizeof(uw)], num_tx_data_bytes-2);
    #endif

    return num_tx_data_bytes;
}


#ifdef HORUS_L2_RX
void horus_l2_decode_rx_packet(unsigned char *output_payload_data,
                               unsigned char *input_rx_data,
                               int            num_payload_data_bytes)
{
    int            num_payload_data_bits;
    unsigned char *pout = output_payload_data;
    unsigned char *pin  = input_rx_data;
    int            ninbit, ingolay, ningolay, paritybyte, nparitybits;
    int            ninbyte, shift, inbit, golayparitybit, i, outbit, outbyte, noutbits, outdata;
    #if defined(SCRAMBLER) || defined(INTERLEAVER)
    int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(num_payload_data_bytes);
    #endif
    
    /* optional scrambler and interleaver - we dont interleave UW */

    #ifdef SCRAMBLER
    scramble(&input_rx_data[sizeof(uw)], num_tx_data_bytes-2);
    #endif

    #ifdef INTERLEAVER
    interleave(&input_rx_data[sizeof(uw)], num_tx_data_bytes-2, 1);
    #endif

    pin = input_rx_data + sizeof(uw) + num_payload_data_bytes;

    /* Read input data bits one at a time.  When we have 12 read 11 parity bits. Golay decode.
       Write decoded (output data) bits every time we have 8 of them. */

    num_payload_data_bits = num_payload_data_bytes*8;
    ninbit = 0;
    ingolay = 0;
    ningolay = 0;
    nparitybits = 0;
    paritybyte = *pin++;
    #ifdef DEBUG0
    fprintf(stderr,"  Read paritybyte: 0x%02x\n", paritybyte);
    #endif
    pout = output_payload_data;
    noutbits = 0;
    outbyte = 0;

    while (ninbit < num_payload_data_bits) {

        /* extract input data bit */

        ninbyte = ninbit/8 + sizeof(uw);
        shift = 7 - (ninbit % 8);
        inbit = (input_rx_data[ninbyte] >> shift) & 0x1;
        #ifdef DEBUG1
        fprintf(stderr, "inbit %d ninbyte: %d inbyte: 0x%02x inbit: %d\n", 
                ninbit, ninbyte, input_rx_data[ninbyte], inbit);
        #endif
        ninbit++;

        /* build up golay codeword */

        ingolay = ingolay | inbit;
        ningolay++;
        ingolay <<= 1;

        /* when we get 12 data bits start reading parity bits */

        if ((ningolay % 12) == 0) {
            #ifdef DEBUG0
            fprintf(stderr, "  ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay>>1);
            #endif
            for (i=0; i<11; i++) {
                shift = 7 - (nparitybits % 8);
                golayparitybit = (paritybyte >> shift) & 0x1;
                ingolay |= golayparitybit;
                if (i != 10)
                    ingolay <<=1;
                nparitybits++;
                if ((nparitybits % 8) == 0) {
                    /* OK grab a new byte */
                    paritybyte = *pin++;
                    #ifdef DEBUG0
                    fprintf(stderr,"  Read paritybyte: 0x%02x\n", paritybyte);
                    #endif
                }
            }

            #ifdef DEBUG0
            fprintf(stderr, "  golay code word: 0x%04x\n", ingolay);
            fprintf(stderr, "  golay decode...: 0x%04x\n", golay23_decode(ingolay));
            #endif
           
            /* write decoded/error corrected bits to output payload data */

            outdata = golay23_decode(ingolay) >> 11;
            #ifdef DEBUG0
            fprintf(stderr, "  outdata...: 0x%04x\n", outdata);
            #endif

            for(i=0; i<12; i++) {   
                shift = 11 - i;
                outbit = (outdata >> shift) & 0x1;
                outbyte |= outbit;
                noutbits++;
                if (noutbits % 8) {
                    outbyte <<= 1;
                }
                else {
                    #ifdef DEBUG0
                    fprintf(stderr, "  output payload byte: 0x%02x\n", outbyte);
                    #endif
                    *pout++ = outbyte;
                    outbyte = 0;
                }
            }

            ingolay = 0;
        }
    } /* while(.... */


    #ifdef DEBUG0
    fprintf(stderr, "finishing up .....\n");
    #endif

    /* Complete final Golay decode  */

    int golayparity = 0;
    if (ningolay % 12) {
        for (i=0; i<11; i++) {
            shift = 7 - (nparitybits % 8);
            golayparitybit = (paritybyte >> shift) & 0x1;
            golayparity |= golayparitybit;
            if (i != 10)
                golayparity <<=1;
            nparitybits++;
            if ((nparitybits % 8) == 0) {
                /* OK grab a new byte */
                paritybyte = *pin++;
                #ifdef DEBUG0
                fprintf(stderr,"  Read paritybyte: 0x%02x\n", paritybyte);
                #endif
            }
        }

        ingolay >>= 1;
        int codeword = (ingolay<<12) + golayparity;
        #ifdef DEBUG0
        fprintf(stderr, "  ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay);
        fprintf(stderr, "  golay code word: 0x%04x\n", codeword);
        fprintf(stderr, "  golay decode...: 0x%04x\n", golay23_decode(codeword));
        #endif

        outdata = golay23_decode(codeword) >> 11;
        #ifdef DEBUG0
        fprintf(stderr, "  outdata...: 0x%04x\n", outdata);
        fprintf(stderr, "  num_payload_data_bits: %d noutbits: %d\n", num_payload_data_bits, noutbits);
        #endif

        /* write final byte */

        int ntogo = num_payload_data_bits - noutbits;
        for(i=0; i<ntogo; i++) {   
            shift = ntogo - i;
            outbit = (outdata >> shift) & 0x1;
            outbyte |= outbit;
            noutbits++;
            if (noutbits % 8) {
                outbyte <<= 1;
            }
            else {
                #ifdef DEBUG0
                fprintf(stderr, "  output payload byte: 0x%02x\n", outbyte);
                #endif
                *pout++ = outbyte;
                outbyte = 0;
            }
        }
    }

    #ifdef DEBUG0
    fprintf(stderr, "\npin - output_payload_data: %ld num_payload_data_bytes: %d\n",
            pout - output_payload_data, num_payload_data_bytes);
    #endif

    assert(pout == (output_payload_data + num_payload_data_bytes));

}
#endif

#ifdef INTERLEAVER

// from https://github.com/drowe67/codec2/blob/96e8a19c2487fd83bd981ce570f257aef42618f9/src/gp_interleaver.c#L39-L40
int is_prime(int x) {
  for (int i = 2; i < x; i++) {
    if ((x % i) == 0) return 0;
  }
  return 1;
}

int next_prime(int x) {
  x++;
  while (is_prime(x) == 0) x++;
  return x;
}

int choose_interleaver_b(int Nbits) {
  int b = floor(Nbits / 1.62);
  b = next_prime(b);
  return b;
}

void interleave(unsigned char *inout, int nbytes, int dir)
{   
    /* note: to work on small uCs (e.g. AVR) needed to declare specific words sizes */
    uint16_t nbits = (uint16_t)nbytes*8;
    uint32_t i, j, n, ibit, ibyte, ishift, jbyte, jshift;
    uint32_t b;
    unsigned char out[nbytes];

    memset(out, 0, nbytes);

    switch(nbytes)
    {
        case 43: // horus v1
            b = 337;
            break;
        case 63: // horus v2
            b = 389;
            break;
        default: // everything else (including horus v3)
            b = choose_interleaver_b(nbits);
    }

    // fprintf(stderr,"n: %d b: %d bits: %d\n",nbytes, b, nbits);


    for(n=0; n<nbits; n++) {

        /*
          "On the Analysis and Design of Good Algebraic Interleavers", Xie et al,eq (5)
        */

        i = n;
        j = (b*i) % nbits; /* note these all need to be 32-bit ints to make multiply work without overflow */
        
        if (dir) {
            uint16_t tmp = j;
            j = i;
            i = tmp;
        }
        
        #ifdef DEBUG0
        printf("i: %d j: %d\n",i, j);
        #endif

        /* read bit i and write to bit j postion */

        ibyte = i/8;
        ishift = i%8;
        ibit = (inout[ibyte] >> ishift) & 0x1;

        jbyte = j/8;
        jshift = j%8;

        /* write jbit to ibit position */

        out[jbyte] |= ibit << jshift; // replace with i-th bit
        //out[ibyte] |= ibit << ishift; // replace with i-th bit
    }
 
    memcpy(inout, out, nbytes);

    #ifdef DEBUG0
    printf("\nInterleaver Out:\n");
    for (i=0; i<nbytes; i++)
        printf("%02d 0x%02x\n", i, inout[i]);
    #endif
}
#endif


#ifdef TEST_INTERLEAVER
int main(void) {
    int nbytes = 43;
    //int nbytes = 63; // v2 large packet
    unsigned char inout[nbytes];
    unsigned char inter[nbytes];
    unsigned char incopy[nbytes];
    int i;

    /* copy of input for later comp   */

    for(i=0; i<nbytes; i++)
        inout[i] = incopy[i] = rand() & 0xff;    
    
    interleave(inout, nbytes, 0);    /* interleave                     */
    memcpy(inter, inout, nbytes);    /* snap shot of interleaved bytes */
    interleave(inout, nbytes, 1);    /* de-interleave                  */

    /* all ones in last col means it worked! */

    for(i=0; i<nbytes; i++) {
        printf("%d 0x%02x 0x%02x 0x%02x %d\n", 
               i, incopy[i], inter[i], inout[i],  incopy[i] == inout[i]);
        assert(incopy[i] == inout[i]);
    }
    printf("Interleaver tested OK!\n");

    return 0;
}
#endif


#ifdef SCRAMBLER

/* 16 bit DVB additive scrambler as per Wikpedia example */

void scramble(unsigned char *inout, int nbytes)
{
    int nbits = nbytes*8;
    int i, ibit, ibits, ibyte, ishift, mask;
    uint16_t scrambler = 0x4a80;  /* init additive scrambler at start of every frame */
    uint16_t scrambler_out;

    /* in place modification of each bit */

    for(i=0; i<nbits; i++) {

        scrambler_out = ((scrambler & 0x2) >> 1) ^ (scrambler & 0x1);

        /* modify i-th bit by xor-ing with scrambler output sequence */

        ibyte = i/8;
        ishift = i%8;
        ibit = (inout[ibyte] >> ishift) & 0x1;
        ibits = ibit ^ scrambler_out;                  // xor ibit with scrambler output

        mask = 1 << ishift;
        inout[ibyte] &= ~mask;                  // clear i-th bit
        inout[ibyte] |= ibits << ishift;         // set to scrambled value

        /* update scrambler */

        scrambler >>= 1;
        scrambler |= scrambler_out << 14;

        #ifdef DEBUG0
        printf("i: %02d ibyte: %d ishift: %d ibit: %d ibits: %d scrambler_out: %d\n", 
               i, ibyte, ishift, ibit, ibits, scrambler_out);
        #endif

    }

    #ifdef DEBUG0
    printf("\nScrambler Out:\n");
    for (i=0; i<nbytes; i++)
        printf("%02d 0x%02x\n", i, inout[i]);
    #endif
}
#endif

#ifdef HORUS_L2_UNITTEST

/*
  Test function to construct a packet of payload data, encode, add
  some bit errors, decode, count errors.
*/

int test_sending_bytes(int nbytes, float ber, int error_pattern) {
    unsigned char input_payload[nbytes];
    int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(sizeof(input_payload));
    unsigned char tx[num_tx_data_bytes];
    unsigned char output_payload[sizeof(input_payload)];
    int b, nbiterrors = 0;
    int i;

    for(i=0; i<nbytes; i++)
        input_payload[i] = i;

    horus_l2_encode_tx_packet(tx, input_payload, sizeof(input_payload));

    #ifdef DEBUG0
    fprintf(stderr, "\nTx Data:\n");
    for(i=0; i<num_tx_data_bytes; i++)
        fprintf(stderr, "  %02d 0x%02x\n", i, tx[i]);
    #endif

    /* insert random bit errors */

    if (error_pattern == 0) {
        float r;
        for(i=0; i<num_tx_data_bytes; i++) {
            for (b=0; b<8; b++) {
                r = (float)rand()/RAND_MAX;
                if (r < ber) {
                    unsigned char mask = (1<<b);
                    #ifdef DEBUG1
                    fprintf("mask: 0x%x tx[%d] = 0x%x ", mask, i, tx[i]);
                    #endif
                    tx[i] ^= mask;
                    #ifdef DEBUG1
                    fprintf("0x%x\n", tx[i]);
                    #endif
                    nbiterrors++;
                }
            }
        }
    }

    /* insert and error burst */

    if (error_pattern == 1) {
        tx[2] ^= 0xff;
        tx[3] ^= 0xff;
    }

    /* insert 1 error every 12 bits, this gives up to 3 errors per 23
       bit codeword, which is the limit of the code */

    if (error_pattern == 2) {
        int bn = 0;
        for(i=0; i<num_tx_data_bytes; i++) {
            for (b=0; b<8; b++) {
                bn++;
                if ((bn % 12) == 0) {
                    unsigned char mask = (1<<(7-b));
                    #ifdef DEBUG1
                    fprintf("mask: 0x%x tx[%d] = 0x%x ", mask, i, tx[i]);
                    #endif
                    tx[i] ^= mask;
                    #ifdef DEBUG1
                    fprintf("0x%x\n", tx[i]);
                    #endif
                    nbiterrors++;
                }
            }
        }
    }

    #ifdef DEBUG0
    fprintf(stderr, "\nTx Data after errors:\n");
    for(i=0; i<num_tx_data_bytes; i++)
        fprintf(stderr, "  %02d 0x%02x\n", i, tx[i]);
    #endif

    #ifdef DEBUG0
    fprintf(stderr, "nbiterrors: %d BER: %3.2f\n", nbiterrors, (float)nbiterrors/(num_tx_data_bytes*8));
    #endif

    golay23_init();
    horus_l2_decode_rx_packet(output_payload, tx, sizeof(input_payload));

    #ifdef DEBUG0
    fprintf(stderr, "\nOutput Payload:\n");
    for(i=0; i<sizeof(input_payload); i++)
        fprintf(stderr, "  %02d 0x%02x\n", i, output_payload[i]);
    #endif

    /* count bit errors */

    int nerr = 0;
    for(i=0; i<nbytes; i++) {
        int error_pattern = input_payload[i] ^ output_payload[i];
        for(b=0; b<8; b++)
            nerr += (error_pattern>>b) & 0x1;
    }
    
    return nerr;
}

/* unit test designed to run on a PC */

int main(void) {
    int num_tx_data_bytes_v1 = horus_l2_get_num_tx_data_bytes(22);
    printf("Horus v1 length packets (22 bytes uncoded, %d bytes coded)\n", num_tx_data_bytes_v1);
    printf("test 0: BER: 0.00 ...........: %d\n", test_sending_bytes(22, 0.00, 0));
    printf("test 1: BER: 0.01 ...........: %d\n", test_sending_bytes(22, 0.01, 0));
    printf("test 2: BER: 0.05 ...........: %d\n", test_sending_bytes(22, 0.05, 0));

    /* we expect this always to fail, as chance of > 3 errors/codeword is high */

    printf("test 3: BER: 0.10 ...........: %d\n", test_sending_bytes(22, 0.10, 0));

    /* -DINTERLEAVER will make this puppy pass */

    printf("test 4: 8 bit burst error....: %d\n", test_sending_bytes(22, 0.00, 1));

    /* Insert 2 errors in every codeword, the maximum correction
       capability of a Golay (23,12) code. note this one will fail
       with -DINTERLEAVER, as we can't guarantee <= 3 errors per
       codeword after interleaving */

    printf("test 5: 1 error every 12 bits: %d\n", test_sending_bytes(22, 0.00, 2));

    int num_tx_data_bytes_v2_16 = horus_l2_get_num_tx_data_bytes(16);
    printf("Horus v2 length packets (16 bytes uncoded, %d bytes coded)\n", num_tx_data_bytes_v2_16);

    printf("test 0: BER: 0.00 ...........: %d\n", test_sending_bytes(16, 0.00, 0));
    printf("test 1: BER: 0.01 ...........: %d\n", test_sending_bytes(16, 0.01, 0));
    printf("test 2: BER: 0.05 ...........: %d\n", test_sending_bytes(16, 0.05, 0));

    /* we expect this always to fail, as chance of > 3 errors/codeword is high */

    printf("test 3: BER: 0.10 ...........: %d\n", test_sending_bytes(16, 0.10, 0));

    /* -DINTERLEAVER will make this puppy pass */

    printf("test 4: 8 bit burst error....: %d\n", test_sending_bytes(16, 0.00, 1));

    /* Insert 2 errors in every codeword, the maximum correction
       capability of a Golay (23,12) code. note this one will fail
       with -DINTERLEAVER, as we can't guarantee <= 3 errors per
       codeword after interleaving */

    printf("test 5: 1 error every 12 bits: %d\n", test_sending_bytes(16, 0.00, 2));

    int num_tx_data_bytes_v2_32 = horus_l2_get_num_tx_data_bytes(32);
    printf("Horus v2 length packets (32 bytes uncoded, %d bytes coded)\n", num_tx_data_bytes_v2_32);
    printf("test 0: BER: 0.00 ...........: %d\n", test_sending_bytes(32, 0.00, 0));
    printf("test 1: BER: 0.01 ...........: %d\n", test_sending_bytes(32, 0.01, 0));
    printf("test 2: BER: 0.05 ...........: %d\n", test_sending_bytes(32, 0.05, 0));

    /* we expect this always to fail, as chance of > 3 errors/codeword is high */

    printf("test 3: BER: 0.10 ...........: %d\n", test_sending_bytes(32, 0.10, 0));

    /* -DINTERLEAVER will make this puppy pass */

    printf("test 4: 8 bit burst error....: %d\n", test_sending_bytes(32, 0.00, 1));

    /* Insert 2 errors in every codeword, the maximum correction
       capability of a Golay (23,12) code. note this one will fail
       with -DINTERLEAVER, as we can't guarantee <= 3 errors per
       codeword after interleaving */

    printf("test 5: 1 error every 12 bits: %d\n", test_sending_bytes(32, 0.00, 2));
    return 0;
}
#endif

/* Horus binary packet */

struct TBinaryPacket
{
    uint8_t     PayloadID;
    uint16_t	Counter;
    uint8_t	Hours;
    uint8_t	Minutes;
    uint8_t	Seconds;
    float	Latitude;
    float	Longitude;
    uint16_t  	Altitude;
    uint8_t     Speed;       // Speed in km/hr
    uint8_t     Sats;
    int8_t      Temp;        // Twos Complement Temp value.
    uint8_t     BattVoltage; // 0 = 0.5v, 255 = 2.0V, linear steps in-between.
    uint16_t    Checksum;    // CRC16-CCITT Checksum.
}  __attribute__ ((packed));


struct V2SmallBinaryPacket
{
uint8_t   PayloadID;	// Legacy list
uint8_t   Counter;	// 8 bit counter
uint16_t  BiSeconds;	// Time of day / 2
uint8_t   Latitude[3];	// (int)(float * 1.0e7) / (1<<8)
uint8_t   Longitude[3];	// ( better than 10m precision )
uint16_t  Altitude;	// 0 - 65 km
uint8_t   Voltage;	// scaled 5.0v in 255 range
uint8_t   User;		// Temp / Sats
	// Temperature	6 bits MSB => (+30 to -32)
	// Satellites	2 bits LSB => 0,4,8,12 is good enough
uint16_t  Checksum;	// CRC16-CCITT Checksum.
}  __attribute__ ((packed));


struct V2LargeBinaryPacket
{
    uint16_t     PayloadID;
    uint16_t	Counter;
    uint8_t	Hours;
    uint8_t	Minutes;
    uint8_t	Seconds;
    float	Latitude;
    float	Longitude;
    uint16_t  	Altitude;
    uint8_t     Speed;       // Speed in km/hr
    uint8_t     Sats;
    int8_t      Temp;        // Twos Complement Temp value.
    uint8_t     BattVoltage; // 0 = 0.5v, 255 = 2.0V, linear steps in-between.
    uint8_t     User[9];
    uint16_t    Checksum;    // CRC16-CCITT Checksum.
}  __attribute__ ((packed));

#ifdef GEN_TX_BITS
/* generate a file of tx_bits to modulate using fsk_horus.m for modem simulations */

int main(void) {
    int nbytes = sizeof(struct TBinaryPacket);
    struct TBinaryPacket input_payload;
    int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(nbytes);
    unsigned char tx[num_tx_data_bytes];
    int i;

    /* all zeros is nastiest sequence for demod before scrambling */

    memset(&input_payload, 0, nbytes);
    input_payload.Checksum = horus_l2_gen_crc16((unsigned char*)&input_payload, nbytes-2);

    horus_l2_encode_tx_packet(tx, (unsigned char*)&input_payload, nbytes);
    
    FILE *f = fopen("../octave/horus_tx_bits_binary.txt","wt");
    assert(f != NULL);
    int b, tx_bit;
    for(i=0; i<num_tx_data_bytes; i++) {
        for(b=0; b<8; b++) {
            tx_bit = (tx[i] >> (7-b)) & 0x1; /* msb first */
            fprintf(f,"%d ", tx_bit);
        }
    }
    fclose(f);
    for(i=0; i<num_tx_data_bytes; i++) {
        fprintf(stdout,"%02X", tx[i]);
    }
    fprintf(stdout, "\n");

    return 0;
}
#endif


#ifdef DEC_RX_BITS

/* Decode a binary file rx_bytes, e.g. from fsk_horus.m */

int main(void) {
    int nbytes = 22;
    unsigned char output_payload[nbytes];
    int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(nbytes);

    /* real world data horus payload generated when running tx above */
    unsigned char rx[45] = {
        0x24,0x24,0x01,0x0b,0x00,0x00,0x05,0x3b,0xf2,0xa7,0x0b,0xc2,0x1b,
        0xaa,0x0a,0x43,0x7e,0x00,0x05,0x00,0x25,0xc0,0xce,0xbb,0x36,0x69,
        0x50,0x00,0x41,0xb0,0xa6,0x5e,0x91,0xa2,0xa3,0xf8,0x1d,0x00,0x00,
        0x0c,0x76,0xc6,0x05,0xb0,0xb8};
    int i, ret;

    assert(num_tx_data_bytes == 45);

    #define READ_FILE /* overwrite tx[] above, that's OK */
    #ifdef READ_FILE
    FILE *f = fopen("../octave/horus_rx_bits_binary.bin","rb");
    assert(f != NULL);
    ret = fread(rx, sizeof(char), num_tx_data_bytes, f);
    assert(ret == num_tx_data_bytes);
    fclose(f);
    #endif

    golay23_init();
    horus_l2_decode_rx_packet(output_payload, rx, nbytes);

    #ifdef HEX_DUMP
    fprintf(stderr, "\nOutput Payload:\n");
    for(i=0; i<nbytes; i++)
        fprintf(stderr, "  %02d 0x%02x 0x%02x\n", i, output_payload[i], rx[i+2]);
    #endif

    struct TBinaryPacket h;
    assert(sizeof(h) == nbytes);
    memcpy(&h, output_payload, nbytes);

    uint16_t crc_rx = horus_l2_gen_crc16(output_payload, nbytes-2);
    char crc_str[80];
    
    if (crc_rx == h.Checksum) {
        sprintf(crc_str, "CRC OK");
    } else {
        sprintf(crc_str, "CRC BAD");
    }

    fprintf(stderr, "%d,%d,%02d:%02d:%02d,%f,%f,%d,%d,%d,%d,%d,%04x %s\n",
        h.PayloadID, h.Counter, h.Hours, h.Minutes, h.Seconds,
        h.Latitude, h.Longitude, h.Altitude, h.Speed, h.Sats, h.Temp, 
            h.BattVoltage, h.Checksum, crc_str);
    
    /* Hex ASCII file output */

    #define WRITE_HEX_FILE /* overwrite tx[] above, that's OK */
    #ifdef WRITE_HEX_FILE
    FILE *fh = fopen("../octave/horus_rx_bits_hex.txt","wt");
    assert(fh != NULL);
    for(i=0; i<nbytes; i++) {
        fprintf(fh, "%02X", (unsigned int)output_payload[i]);
    }
    fclose(fh);
    #endif

    return 0;
}
#endif

#ifdef GEN_TX_BITSTREAM
/* Generate a stream of encoded Horus packets in a format suitable to feed into fsk_mod */

int main(int argc,char *argv[]) {

    int i, framecnt, mode;
    int num_tx_data_bytes = 0;

    if(argc != 3){
        fprintf(stderr,"usage: %s numFrames mode\n",argv[0]);
        exit(1);
    }

    framecnt = atoi(argv[1]);
    mode = atoi(argv[2]);


    if (mode == 0){
        // Horus Mode v1
        int nbytes = sizeof(struct TBinaryPacket);
        struct TBinaryPacket input_payload;
        int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(nbytes);
        unsigned char tx[num_tx_data_bytes];

        memset(&input_payload, 0, nbytes);
        input_payload.Checksum = horus_l2_gen_crc16((unsigned char*)&input_payload, nbytes-2);

        horus_l2_encode_tx_packet(tx, (unsigned char*)&input_payload, nbytes);

        int b;
        uint8_t tx_bit;
        while(framecnt >= 0){
            for(i=0; i<num_tx_data_bytes; i++) {
                for(b=0; b<8; b++) {
                    tx_bit = (tx[i] >> (7-b)) & 0x1; /* msb first */
                    fwrite(&tx_bit,sizeof(uint8_t),1,stdout);
                    fflush(stdout);
                }
            }
            framecnt -= 1;
        }
    }
    // } else if (mode == 1) {
    //     // 32-byte horus mode
    //     int nbytes = sizeof(struct V2LargeBinaryPacket);
    //     struct V2LargeBinaryPacket input_payload;

    //     int num_tx_data_bytes = sizeof(uw_v2) + H_256_768_22_DATA_BYTES + H_256_768_22_PARITY_BYTES;
    //     unsigned char tx[num_tx_data_bytes];
    //     memset(&input_payload, 0, nbytes);

    //     input_payload.PayloadID = 1;
    //     input_payload.Counter = 2;
    //     input_payload.Checksum = horus_l2_gen_crc16((unsigned char*)&input_payload, nbytes-2);

    //     int ldpc_tx_bytes = ldpc_encode_packet(tx, (unsigned char*)&input_payload, 1);

    //     int b;
    //     uint8_t tx_bit;
    //     while(framecnt >= 0){
    //         for(i=0; i<num_tx_data_bytes; i++) {
    //             for(b=0; b<8; b++) {
    //                 tx_bit = (tx[i] >> (7-b)) & 0x1; /* msb first */
    //                 fwrite(&tx_bit,sizeof(uint8_t),1,stdout);
    //                 fflush(stdout);
    //             }
    //         }
    //         framecnt -= 1;
    //     }

    // } else if (mode == 2) {
    //     // 16-byte horus mode
    //     int nbytes = sizeof(struct V2SmallBinaryPacket);
    //     struct V2SmallBinaryPacket input_payload;

    //     int num_tx_data_bytes = sizeof(uw_v2) + H_128_384_23_DATA_BYTES + H_128_384_23_PARITY_BYTES;
    //     unsigned char tx[num_tx_data_bytes];
    //     memset(&input_payload, 0, nbytes);
    //     input_payload.PayloadID = 1;
    //     input_payload.Counter = 2;

    //     input_payload.Checksum = horus_l2_gen_crc16((unsigned char*)&input_payload, nbytes-2);

    //     int ldpc_tx_bytes = ldpc_encode_packet(tx, (unsigned char*)&input_payload, 2);

    //     int b;
    //     uint8_t tx_bit;
    //     while(framecnt >= 0){
    //         for(i=0; i<num_tx_data_bytes; i++) {
    //             for(b=0; b<8; b++) {
    //                 tx_bit = (tx[i] >> (7-b)) & 0x1; /* msb first */
    //                 fwrite(&tx_bit,sizeof(uint8_t),1,stdout);
    //                 fflush(stdout);
    //             }
    //         }
    //         framecnt -= 1;
    //     }
    // }

    return 0;
}
#endif

// from http://stackoverflow.com/questions/10564491/function-to-calculate-a-crc16-checksum

unsigned short horus_l2_gen_crc16(unsigned char* data_p, unsigned char length) {
    unsigned char x;
    unsigned short crc = 0xFFFF;

    while (length--){
        x = crc >> 8 ^ *data_p++;
        x ^= x>>4;
        crc = (crc << 8) ^ ((unsigned short)(x << 12)) ^ ((unsigned short)(x <<5)) ^ ((unsigned short)x);
    }
    return crc;
}


// //  Take payload data bytes, prepend a unique word and append parity bits
// int ldpc_encode_packet(unsigned char *out_data, unsigned char *in_data, int mode) {
//     unsigned int   i, last = 0;
//     unsigned char *pout;

//     unsigned int data_bytes, parity_bytes, number_parity_bits, max_row_weight;

//     if (mode == 1){
//         // 32-byte Mode.
//         data_bytes = H_256_768_22_DATA_BYTES;
//         parity_bytes = H_256_768_22_PARITY_BYTES;
//         number_parity_bits = H_256_768_22_NUMBERPARITYBITS;
//         max_row_weight = H_256_768_22_MAX_ROW_WEIGHT;
//     } else {
//         // 16-byte Mode.
//         data_bytes = H_128_384_23_DATA_BYTES;
//         parity_bytes = H_128_384_23_PARITY_BYTES;
//         number_parity_bits = H_128_384_23_NUMBERPARITYBITS;
//         max_row_weight = H_128_384_23_MAX_ROW_WEIGHT;
//     }

//     pout = out_data;
//     memcpy(pout, uw_v2, sizeof(uw_v2));
//     pout += sizeof(uw_v2);
//     memcpy(pout, in_data, data_bytes);
//     pout += data_bytes;
//     memset(pout, 0, parity_bytes);

//     // process parity bit offsets
//     for (i = 0; i < number_parity_bits; i++) {
//         unsigned int shift, j;
//         uint8_t tmp;

// 	for(j = 0; j < max_row_weight; j++) {
//         // This is a bit silly, move this out of this loop.
//         if (mode  == 1){
//             tmp  = H_256_768_22_H_rows[i + number_parity_bits * j];
//         } else if (mode == 2) {
// 		    tmp  = H_128_384_23_H_rows[i + number_parity_bits * j];
//         }
// 		if (!tmp)
// 			continue;
// 		tmp--;
// 		shift = 7 - (tmp & 7); // MSB
// 		last ^= in_data[tmp >> 3] >> shift;
// 	}
// 	shift = 7 - (i & 7); // MSB
// 	pout[i >> 3] |= (last & 1) << shift;
//     }

//     pout = out_data + sizeof(uw_v2);
//     interleave(pout, data_bytes + parity_bytes, 0);
//     scramble(pout, data_bytes + parity_bytes);

//     return data_bytes + parity_bytes + sizeof(uw_v2);
// }

// /* Scramble and interleave are 8bit lsb, but bitstream is sent msb */
// #define LSB2MSB(X) (X + 7 - 2 * (X & 7) )

// /* Invert bits - ldpc expects negative floats for high hits */
// void soft_unscramble(float *in, float* out, int nbits) {
// 	int i, ibit;
// 	uint16_t scrambler = 0x4a80;  /* init additive scrambler at start of every frame */
// 	uint16_t scrambler_out;

// 	for ( i = 0; i < nbits; i++ ) {
// 		scrambler_out = ( (scrambler >> 1) ^ scrambler) & 0x1;

// 		/* modify i-th bit by xor-ing with scrambler output sequence */
// 		ibit = LSB2MSB(i);
// 		if ( scrambler_out ) {
// 			out[ibit] = in[ibit];
// 		} else {
// 			out[ibit] = -in[ibit];
// 		}

// 		scrambler >>= 1;
// 		scrambler |= scrambler_out << 14;
// 	}
// }

// // soft bit deinterleave
// void soft_deinterleave(float *in, float* out, int mode) {
// 	int n, i, j, bits_per_packet, coprime;

//     if (mode == 1) {
//         // 256_768
//         bits_per_packet = H_256_768_22_BITS_PER_PACKET;
//         coprime = H_256_768_22_COPRIME;
//     } else {
//         bits_per_packet = H_128_384_23_BITS_PER_PACKET;
//         coprime = H_128_384_23_COPRIME;
//     }


// 	for ( n = 0; n < bits_per_packet; n++ ) {
// 		i = LSB2MSB(n);
// 		j = LSB2MSB( (coprime * n) % bits_per_packet);
// 		out[i] = in[j];
// 	}
// }

// // packed bit deinterleave - same as Golay version , but different Coprime
// void bitwise_deinterleave(uint8_t *inout, int nbytes)
// {
//     uint16_t nbits = (uint16_t)nbytes*8;
//     uint32_t i, j, ibit, ibyte, ishift, jbyte, jshift;
//     uint8_t out[nbytes];

//     memset(out, 0, nbytes);
//     for(j = 0; j < nbits; j++) {
//         i = (COPRIME * j) % nbits;

//         /* read bit i */
//         ibyte = i>>3;
//         ishift = i&7;
//         ibit = (inout[ibyte] >> ishift) & 0x1;

// 	/* write bit i to bit j position */
//         jbyte = j>>3;
//         jshift = j&7;
//         out[jbyte] |= ibit << jshift;
//     }
 
//     memcpy(inout, out, nbytes);
// }

// /* Compare detected bits to corrected bits */
// void ldpc_errors( const uint8_t *outbytes, uint8_t *rx_bytes ) {
// 	int	length = DATA_BYTES + PARITY_BYTES;
// 	uint8_t temp[length];
// 	int	i, percentage, count = 0;
// 	memcpy(temp, rx_bytes, length);

// 	scramble(temp, length); // use scrambler from Golay code
// 	bitwise_deinterleave(temp, length);

// 	// count bits changed during error correction
// 	for(i = 0; i < BITS_PER_PACKET; i++) {
// 		int x, y, offset, shift;

// 		shift = i & 7;
// 		offset = i >> 3;
// 		x = temp[offset] >> shift;
// 		y = outbytes[offset] >> shift;
// 		count += (x ^ y) & 1;
// 	}

// 	// scale errors against a maximum of 20% BER
// 	percentage = (count * 5 * 100) / BITS_PER_PACKET;
// 	if (percentage > 100)
// 		percentage = 100;
// 	set_error_count( percentage );
// }

// /* LDPC decode */
// void horus_ldpc_decode(uint8_t *payload, float *sd, int mode) {
// 	float sum, mean, sumsq, estEsN0, x;
//     int bits_per_packet, payload_bytes;

//     if(mode == 1){
//         bits_per_packet = H_256_768_22_BITS_PER_PACKET;
//         payload_bytes = H_256_768_22_DATA_BYTES;
//     } else {
//         bits_per_packet = H_128_384_23_BITS_PER_PACKET;
//         payload_bytes = H_128_384_23_DATA_BYTES;
//     }

//     double sd_double[bits_per_packet];
//     float llr[bits_per_packet];
//     float temp[bits_per_packet];
//     uint8_t outbits[bits_per_packet];

//     int b, i, parityCC;
// 	struct LDPC ldpc;

//     // cast incoming SDs to doubles for sd_to_llr
//     // For some reason I need to flip the sign ?!?!
//     for ( i = 0; i < bits_per_packet; i++ )
//         sd_double[i] = (double)sd[i]*-1.0;


//     sd_to_llr(llr, sd_double, bits_per_packet);

// 	/* reverse whitening and re-order bits */
// 	soft_unscramble(llr, temp, bits_per_packet);
// 	soft_deinterleave(temp, llr, mode);

// 	/* correct errors */
//     if (mode == 1){
//         // 32-byte mode H_256_768_22
//         ldpc.max_iter = H_256_768_22_MAX_ITER;
//         ldpc.dec_type = 0;
//         ldpc.q_scale_factor = 1;
//         ldpc.r_scale_factor = 1;
//         ldpc.CodeLength = H_256_768_22_CODELENGTH;
//         ldpc.NumberParityBits = H_256_768_22_NUMBERPARITYBITS;
//         ldpc.NumberRowsHcols = H_256_768_22_NUMBERROWSHCOLS;
//         ldpc.max_row_weight = H_256_768_22_MAX_ROW_WEIGHT;
//         ldpc.max_col_weight = H_256_768_22_MAX_COL_WEIGHT;
//         ldpc.H_rows = (uint16_t *)H_256_768_22_H_rows;
//         ldpc.H_cols = (uint16_t *)H_256_768_22_H_cols;
//     } else {
//         // 16-byte mode
//         ldpc.max_iter = H_128_384_23_MAX_ITER;
//         ldpc.dec_type = 0;
//         ldpc.q_scale_factor = 1;
//         ldpc.r_scale_factor = 1;
//         ldpc.CodeLength = H_128_384_23_CODELENGTH;
//         ldpc.NumberParityBits = H_128_384_23_NUMBERPARITYBITS;
//         ldpc.NumberRowsHcols = H_128_384_23_NUMBERROWSHCOLS;
//         ldpc.max_row_weight = H_128_384_23_MAX_ROW_WEIGHT;
//         ldpc.max_col_weight = H_128_384_23_MAX_COL_WEIGHT;
//         ldpc.H_rows = (uint16_t *)H_128_384_23_H_rows;
//         ldpc.H_cols = (uint16_t *)H_128_384_23_H_cols;
//     }

// 	i = run_ldpc_decoder(&ldpc, outbits, llr, &parityCC);
//     fprintf(stderr,"iterations: %d\n", i);

// 	/* convert MSB bits to a packet of bytes */    
// 	for (b = 0; b < payload_bytes; b++) {
// 		uint8_t rxbyte = 0;
// 		for(i=0; i<8; i++)
// 			rxbyte |= outbits[b*8+i] << (7 - i);
// 		payload[b] = rxbyte;
// 	}
// }
