diff --git a/Golay24.hpp b/Golay24.hpp
index 89f9a47..0731024 100644
--- a/Golay24.hpp
+++ b/Golay24.hpp
@@ -1,3 +1,7 @@
+
+#ifndef GOLAY24_HPP_7a68240afda9406facf81fcad3851111
+#define GOLAY24_HPP_7a68240afda9406facf81fcad3851111
+
 /**
  * Based on the work of Mr Hank Wallace.
  *
@@ -8,7 +12,7 @@
  */
 
 #include <stdio.h>
-
+#include <assert.h>
 
 #define POLY  0xAE3
 
@@ -129,7 +133,7 @@ private:
     }
 
 public:
-    unsigned int correct(unsigned int cw, int *errs)
+    unsigned int correct(unsigned int cw, int *errs, unsigned int *errors_detected)
     /* This function corrects Golay [23,12] codeword cw, returning the
        corrected codeword. This function will produce the corrected codeword
        for three or fewer errors. It will produce some other valid Golay
@@ -148,6 +152,8 @@ public:
 
       cwsaver=cw;         /* save */
       *errs=0;
+      *errors_detected = 0;
+
       w=3;                /* initial syndrome weight threshold */
       j=-1;               /* -1 = no trial bit flipping on first pass */
       mask=1;
@@ -167,6 +173,7 @@ public:
           s=syndrome(cw); /* look for errors */
           if (s) /* errors exist */
             {
+              (*errors_detected)++;
               for (i=0; i<23; i++) /* check syndrome of each cyclic shift */
                 {
                   if ((*errs=weight(s)) <= w) /* syndrome matches error pattern */
@@ -184,7 +191,9 @@ public:
               j++; /* toggle next trial bit */
             }
           else
-            return(cw); /* return corrected codeword */
+            {
+              return(cw); /* return corrected codeword */
+            }
         }
 
       return(cwsaver); /* return original if no corrections */
@@ -200,6 +209,11 @@ public:
         return codeword;
     }
 
+    unsigned int encode23(unsigned int data)
+    {
+        return golay(data); /* make a test codeword */
+    }
+
     int decode(int *errs, unsigned int *cw)
     /* This function decodes codeword *cw. Here error correction is attempted,
        with *errs set to the number of
@@ -207,11 +221,11 @@ public:
        exist. */
     {
       unsigned int parity_bit;
-
+      unsigned int detected_errors;
       parity_bit=*cw & 0x800000l; /* save parity bit */
       *cw&=~0x800000l;            /* remove parity bit for correction */
 
-      *cw=correct(*cw, errs);     /* correct up to three bits */
+      *cw=correct(*cw, errs, &detected_errors);     /* correct up to three bits */
       *cw|=parity_bit;            /* restore parity bit */
 
       /* check for 4 bit errors */
@@ -226,8 +240,6 @@ public:
        returning 0 if no errors exist, 1 if an overall parity error exists, and
        2 if a codeword error exists. */
     {
-      unsigned int parity_bit;
-
       *errs=0;
       if (parity(cw)) /* odd parity is an error */
         {
@@ -254,34 +266,59 @@ public:
 class DSDGolay24 : public Golay24
 {
 public:
-    unsigned int adapt_to_codeword(const char* hex, const char* parity)
+    unsigned int adapt_to_codeword(const char* word, unsigned int length, const char* parity)
     {
         unsigned int codeword = 0;
 
         // Data needs to be packed with the 12 bits of parity as the most significant
         // bits and 12 bits of data as the less significant. All these discovered by trial and error.
         for (unsigned int i=0; i<12; i++) {
+            assert(parity[11-i] == 0 || parity[11-i] == 1);
+            codeword <<= 1;
             codeword |= parity[11-i];
-            codeword <<= 1;
         }
-        for (unsigned int i=0; i<6; i++) {
-            codeword |= hex[5-i];
+        for (unsigned int i=0; i<length; i++) {
+            char bit = word[length-1-i];
+            assert(bit == 0 || bit == 1);
             codeword <<= 1;
+            codeword |= bit;
+        }
+        // We only have length bits of data. We fill up the less significant bits of codeword with zeros.
+        if (length < 12) {
+            codeword <<= (12 - length);
         }
-        // We only have 6 bits of data. We fill up the less 6 significant bits of codeword with zeros.
-        codeword <<= 5;
 
         return codeword;
     }
 
-    void adapt_to_hex(unsigned int codeword, char* hex)
+    void adapt_to_word(unsigned int codeword, char* word, unsigned int length)
     {
-        // put back in hex the bits from 6 to 11 of codeword
-        for (unsigned int i=0, mask = 1<<6; i<6; i++, mask<<=1) {
-            hex[i] = (codeword & mask) != 0? 1 : 0;
+        // put back in word the bits from codeword
+        for (unsigned int i=0, mask = 1<<(12-length); i<length; i++, mask<<=1) {
+            word[i] = (codeword & mask) != 0? 1 : 0;
         }
     }
 
+    unsigned int adapt_from_word(char* word, unsigned int length)
+    {
+        unsigned int codeword = 0;
+
+        // encode the hex bits into a codeword
+        for (unsigned int i=0; i<length; i++) {
+            int bit = word[length-1-i];
+            assert(bit == 0 || bit == 1);
+            codeword <<= 1;
+            codeword |= bit;
+        }
+
+        // We only have length bits of data. We fill up the less significant bits of codeword with zeros.
+        if (length < 12) {
+            codeword <<= (12-length);
+        }
+
+        return codeword;
+    }
+
     /**
      * Important method that takes an hex word, a 12 bit parity word and uses the Golay24 implementation
      * to error correct it.
@@ -292,10 +329,10 @@ public:
      *           in this case the original data remains unchanged.
      *         0 if the data was successfully error corrected.
      */
-    int decode(char* hex, const char* parity, int* fixed_errors)
+    int decode_6(char* hex, const char* parity, int* fixed_errors)
     {
-        unsigned int codeword = adapt_to_codeword(hex, parity);
-        // codeword contains:
+        unsigned int codeword = adapt_to_codeword(hex, 6, parity);
+        // codeword now contains:
         // bits  0- 5: zeros
         // bits  6-11: hex bits
         // bits 12-23: golay (24,12) parity bits
@@ -314,10 +351,62 @@ public:
             // discard, don't touch hex
         } else {
             // put it back into our hex format
-            adapt_to_hex(codeword, hex);
+            adapt_to_word(codeword, hex, 6);
             uncorrectable_errors = 0;
         }
 
         return uncorrectable_errors;
     }
+
+    int decode_12(char* dodeca, const char* parity, int* fixed_errors)
+    {
+        unsigned int codeword = adapt_to_codeword(dodeca, 12, parity);
+        // codeword contains:
+        // bits  0-11: dodeca bits
+        // bits 12-23: golay (24,12) parity bits
+
+        // bits are ordered from left to right, so bit 0 is the most significant and bit 23 is the less.
+
+        int uncorrectable_errors = Golay24::decode(fixed_errors, &codeword);
+
+        // codeword is now hopefully fixed
+
+        // If there are uncorrectable errors and the fixed proposal includes ones
+        // in bits 0-5 then probably the fix is not good and we discard it.
+        // Bits 0-5 should always be zero, if there is a problem it's on the other bits
+        // from 6 to 24.
+        if (uncorrectable_errors == 1 && (codeword & 0x3f) != 0) {
+            // discard, don't touch hex
+        } else {
+            // put it back into our hex format
+            adapt_to_word(codeword, dodeca, 12);
+            uncorrectable_errors = 0;
+        }
+
+        return uncorrectable_errors;
+    }
+
+    void encode_6(char* hex, char* out_parity)
+    {
+        unsigned int data = adapt_from_word(hex, 6);
+        unsigned int codeword = Golay24::encode(data);
+
+        // Fill up the parity
+        for (unsigned int i=0, mask = 1<<12; i<12; i++, mask<<=1) {
+            out_parity[i] = (codeword & mask) != 0? 1 : 0;
+        }
+    }
+
+    void encode_12(char* dodeca, char* out_parity)
+    {
+        unsigned int data = adapt_from_word(dodeca, 12);
+        unsigned int codeword = Golay24::encode(data);
+
+        // Fill up the parity
+        for (unsigned int i=0, mask = 1<<12; i<12; i++, mask<<=1) {
+            out_parity[i] = (codeword & mask) != 0? 1 : 0;
+        }
+    }
 };
+
+#endif // GOLAY24_HPP_7a68240afda9406facf81fcad3851111
diff --git a/Hamming.cpp b/Hamming.cpp
index 97a0d43..7aa4d15 100644
--- a/Hamming.cpp
+++ b/Hamming.cpp
@@ -1,5 +1,5 @@
 
-#include <Hamming.hpp>
+#include "Hamming.hpp"
 
 // definition outside class declaration
 Hamming_10_6_3_data Hamming_10_6_3::data;
@@ -50,6 +50,18 @@ int Hamming_10_6_3::decode(std::bitset<10>& input)
     return error_count;
 }
 
+int Hamming_10_6_3::encode(std::bitset<6>& input)
+{
+    // Compute syndromes
+    int s0 = ((data.gt0 & input).count() & 1) << 3;
+    int s1 = ((data.gt1 & input).count() & 1) << 2;
+    int s2 = ((data.gt2 & input).count() & 1) << 1;
+    int s3 = ((data.gt3 & input).count() & 1);
+    int parity = s0 | s1 | s2 | s3;
+
+    return parity;
+}
+
 int Hamming_10_6_3_TableImpl::decode(int input, int* output)
 {
     assert (input < 1024 && input >= 0);
@@ -60,3 +72,12 @@ int Hamming_10_6_3_TableImpl::decode(int input, int* output)
 
     return data.error_counts[input];
 }
+
+int Hamming_10_6_3_TableImpl::encode(int input)
+{
+    assert (input < 64 && input >= 0);
+
+    // Making use of a table...
+
+    return data.encode_parities[input];
+}
diff --git a/Hamming.hpp b/Hamming.hpp
index e64ec6d..b0a0be1 100644
--- a/Hamming.hpp
+++ b/Hamming.hpp
@@ -1,6 +1,6 @@
 
-#ifndef __HAMMING_HPP__
-#define __HAMMING_HPP__
+#ifndef HAMMING_HPP_e7123c3795b94d14b5774d5d8f016a04
+#define HAMMING_HPP_e7123c3795b94d14b5774d5d8f016a04
 
 #include <bitset>
 #include <string>
@@ -22,6 +22,11 @@ public:
     std::bitset<10> g4;
     std::bitset<10> g5;
 
+    std::bitset<6> gt0;
+    std::bitset<6> gt1;
+    std::bitset<6> gt2;
+    std::bitset<6> gt3;
+
     std::bitset<10> h0;
     std::bitset<10> h1;
     std::bitset<10> h2;
@@ -39,6 +44,11 @@ public:
         g4 = std::bitset<10>(std::string("0000100011"));
         g5 = std::bitset<10>(std::string("0000011100"));
 
+        gt0 = std::bitset<6>(std::string("111001"));
+        gt1 = std::bitset<6>(std::string("110101"));
+        gt2 = std::bitset<6>(std::string("101110"));
+        gt3 = std::bitset<6>(std::string("011110"));
+
         // H matrix, calculated transposing G. See wikipedia.
         h0 = std::bitset<10>(std::string("1110011000"));
         h1 = std::bitset<10>(std::string("1101010100"));
@@ -74,6 +84,89 @@ public:
     }
 };
 
+class Hamming_15_11_3_data
+{
+public:
+    std::bitset<15> g0;
+    std::bitset<15> g1;
+    std::bitset<15> g2;
+    std::bitset<15> g3;
+    std::bitset<15> g4;
+    std::bitset<15> g5;
+    std::bitset<15> g6;
+    std::bitset<15> g7;
+    std::bitset<15> g8;
+    std::bitset<15> g9;
+    std::bitset<15> g10;
+
+
+    std::bitset<11> gt0;
+    std::bitset<11> gt1;
+    std::bitset<11> gt2;
+    std::bitset<11> gt3;
+
+    std::bitset<15> h0;
+    std::bitset<15> h1;
+    std::bitset<15> h2;
+    std::bitset<15> h3;
+
+    int bad_bit_table[16];
+
+    Hamming_15_11_3_data()
+    {
+        // G matrix come from the APCO 25 reference documentation:
+        g0  = std::bitset<15>(std::string("100000000001111"));
+        g1  = std::bitset<15>(std::string("010000000001110"));
+        g2  = std::bitset<15>(std::string("001000000001101"));
+        g3  = std::bitset<15>(std::string("000100000001100"));
+        g4  = std::bitset<15>(std::string("000010000001011"));
+        g5  = std::bitset<15>(std::string("000001000001010"));
+        g6  = std::bitset<15>(std::string("000000100001001"));
+        g7  = std::bitset<15>(std::string("000000010000111"));
+        g8  = std::bitset<15>(std::string("000000001000110"));
+        g9  = std::bitset<15>(std::string("000000000100101"));
+        g10 = std::bitset<15>(std::string("000000000010011"));
+
+        gt0 = std::bitset<11>(std::string("11111110000"));
+        gt1 = std::bitset<11>(std::string("11110001110"));
+        gt2 = std::bitset<11>(std::string("11001101101"));
+        gt3 = std::bitset<11>(std::string("10101011011"));
+
+        // H matrix, calculated transposing G. See wikipedia.
+        h0 = std::bitset<15>(std::string("111111100001000"));
+        h1 = std::bitset<15>(std::string("111100011100100"));
+        h2 = std::bitset<15>(std::string("110011011010010"));
+        h3 = std::bitset<15>(std::string("101010110110001"));
+
+        // bad_bit_table indicates which is the bit to correct from the calculated syndromes.
+        // It's calculated from the previous matrices: See the left-most column of the matrix whose rows are
+        // h0,h1,h2 and h3. It's [1 1 1 0]. It means: when the syndrome value is the binary number 1110, the
+        // bit that is wrong is the bit 9 (the left-most column is for the bit 9). Another example with
+        // the right-most column: it's column is [0 0 0 1], so when the syndrome is 0001, the bit to fix is
+        // bit 0 (one of the parity bits).
+        bad_bit_table[ 1] = 0;
+        bad_bit_table[ 2] = 1;
+        bad_bit_table[ 4] = 2;
+        bad_bit_table[ 8] = 3;
+        bad_bit_table[12] = 4;
+        bad_bit_table[ 3] = 5;
+        bad_bit_table[ 7] = 6;
+        bad_bit_table[11] = 7;
+        bad_bit_table[13] = 8;
+        bad_bit_table[14] = 9;
+
+        // If the syndrome is zero, then the data is correct, no errors detected.
+        bad_bit_table[ 0] = -2;
+
+        // Incorrect values, if the syndrome gets one of this values, then we fail to correct the data.
+        bad_bit_table[ 5] = -1;
+        bad_bit_table[ 6] = -1;
+        bad_bit_table[ 9] = -1;
+        bad_bit_table[10] = -1;
+        bad_bit_table[15] = -1;
+    }
+};
+
 /**
  * An interface used by the two implementations defined here.
  */
@@ -101,6 +194,10 @@ public:
      * \return Count of detected errors.
      */
     virtual int decode(char* hex, char* parity) = 0;
+
+    virtual int encode(int input) = 0;
+
+    virtual void encode(char* hex, char* out_parity) = 0;
 };
 
 /**
@@ -145,10 +242,10 @@ public:
         std::bitset<10> value;
         // in the bitset 9 is the left-most and 0 is the right-most
         for (unsigned int i=0; i<6; i++) {
-            value[9-i] = hex[i];
+            value[9-i] = (hex[i] == 1)? true : false;
         }
         for (unsigned int i=0; i<4; i++) {
-            value[3-i] = parity[i];
+            value[3-i] = (parity[i] == 1)? true : false;
         }
 
         int error_count = decode(value);
@@ -164,6 +261,35 @@ public:
 
         return error_count;
     }
+
+    int encode(std::bitset<6>& input);
+
+    int encode(int input)
+    {
+        assert (input < 64 && input >= 0);
+
+        std::bitset<6> bitset_input(input);
+
+        return encode(bitset_input);
+    }
+
+    void encode(char* hex, char* out_parity)
+    {
+        // Make a bitset from hex
+        std::bitset<6> value;
+        // in the bitset 5 is the left-most and 0 is the right-most
+        for (unsigned int i=0; i<6; i++) {
+            value[5-i] = (hex[i] == 1)? true : false;
+        }
+
+        int parity = encode(value);
+
+        // Put the calculated parity in the form of a char array
+        for (int i=3; i>=0; i--) {
+            out_parity[i] = parity & 1;
+            parity >>= 1;
+        }
+    }
 };
 
 /**
@@ -174,6 +300,7 @@ class Hamming_10_6_3_TableImpl_data
 public:
     int fixed_values[1024];
     int error_counts[1024];
+    int encode_parities[64];
 
     /**
      * \brief On initialization it builds a table to calculate every possible outcome (there are 1024) before
@@ -190,6 +317,11 @@ public:
             fixed_values[i] = fixed;
             error_counts[i] = error_count;
         }
+
+        for (int i=0; i<64; i++) {
+            int parity = hamming.encode(i);
+            encode_parities[i] = parity;
+        }
     }
 };
 
@@ -210,6 +342,7 @@ public:
         int value = 0;
         // in the bitset 9 is the left-most and 0 is the right-most
         for (unsigned int i=0; i<6; i++) {
+            assert(hex[i] == 0 || hex[i] == 1);
             value <<= 1;
             value |= hex[i];
         }
@@ -223,10 +356,12 @@ public:
         int value = 0;
         // in the bitset 9 is the left-most and 0 is the right-most
         for (unsigned int i=0; i<6; i++) {
+            assert(hex[i] == 0 || hex[i] == 1);
             value <<= 1;
             value |= hex[i];
         }
         for (unsigned int i=0; i<4; i++) {
+            assert(parity[i] == 0 || parity[i] == 1);
             value <<= 1;
             value |= parity[i];
         }
@@ -258,7 +393,21 @@ public:
 
         return error_count;
     }
+
+    int encode(int input);
+
+    void encode(char* hex, char* out_parity)
+    {
+        int value = convert_hex_to_int(hex);
+        int parity = encode(value);
+
+        // Put the calculated parity in the form of a char array
+        for (int i=3; i>=0; i--) {
+            out_parity[i] = parity & 1;
+            parity >>= 1;
+        }
+    }
 };
 
 
-#endif // __HAMMING_HPP__
+#endif // HAMMING_HPP_e7123c3795b94d14b5774d5d8f016a04
diff --git a/ReedSolomon.hpp b/ReedSolomon.hpp
index 021e34f..49fbecc 100644
--- a/ReedSolomon.hpp
+++ b/ReedSolomon.hpp
@@ -1,5 +1,6 @@
-#ifndef __REED_SOLOMON_HPP__
-#define __REED_SOLOMON_HPP__
+
+#ifndef REEDSOLOMON_HPP_b1405fdab6374ba2a4e65e8d45ec3d80
+#define REEDSOLOMON_HPP_b1405fdab6374ba2a4e65e8d45ec3d80
 
 /**
  * Code taken and adapted from www.eccpage.com/rs.c
@@ -59,15 +60,15 @@
 #include <math.h>
 #include <stdio.h>
 
-// Constants to adapt the code to DSD/P25 needs
-
+template <int TT>
 class ReedSolomon_63
 {
 private:
-    static const int mm = 6;             /* RS code over GF(2**mm) */
-    static const int nn = 63;            /* nn=2**mm -1   length of codeword */
-    int tt;             /* number of errors that can be corrected */
-    int kk;             /* kk = nn-2*tt  */
+    static const int MM = 6;             /* RS code over GF(2**mm) */
+    static const int NN = 63;            /* nn=2**mm -1   length of codeword */
+    //int tt;             /* number of errors that can be corrected */
+    //int kk;             /* kk = nn-2*tt  */
+    static const int KK = NN-2*TT;
     // distance = nn-kk+1 = 2*tt+1
 
     int* alpha_to;
@@ -84,19 +85,19 @@ private:
         register int i, mask;
 
         mask = 1;
-        alpha_to[mm] = 0;
-        for (i = 0; i < mm; i++) {
+        alpha_to[MM] = 0;
+        for (i = 0; i < MM; i++) {
             alpha_to[i] = mask;
             index_of[alpha_to[i]] = i;
             if (generator_polinomial[i] != 0)
-                alpha_to[mm] ^= mask;
+                alpha_to[MM] ^= mask;
             mask <<= 1;
         }
-        index_of[alpha_to[mm]] = mm;
+        index_of[alpha_to[MM]] = MM;
         mask >>= 1;
-        for (i = mm + 1; i < nn; i++) {
+        for (i = MM + 1; i < NN; i++) {
             if (alpha_to[i - 1] >= mask)
-                alpha_to[i] = alpha_to[mm] ^ ((alpha_to[i - 1] ^ mask) << 1);
+                alpha_to[i] = alpha_to[MM] ^ ((alpha_to[i - 1] ^ mask) << 1);
             else
                 alpha_to[i] = alpha_to[i - 1] << 1;
             index_of[alpha_to[i]] = i;
@@ -113,29 +114,26 @@ private:
 
         gg[0] = 2; /* primitive element alpha = 2  for GF(2**mm)  */
         gg[1] = 1; /* g(x) = (X+alpha) initially */
-        for (i = 2; i <= nn - kk; i++) {
+        for (i = 2; i <= NN - KK; i++) {
             gg[i] = 1;
             for (j = i - 1; j > 0; j--)
                 if (gg[j] != 0)
-                    gg[j] = gg[j - 1] ^ alpha_to[(index_of[gg[j]] + i) % nn];
+                    gg[j] = gg[j - 1] ^ alpha_to[(index_of[gg[j]] + i) % NN];
                 else
                     gg[j] = gg[j - 1];
-            gg[0] = alpha_to[(index_of[gg[0]] + i) % nn]; /* gg[0] can never be zero */
+            gg[0] = alpha_to[(index_of[gg[0]] + i) % NN]; /* gg[0] can never be zero */
         }
         /* convert gg[] to index form for quicker encoding */
-        for (i = 0; i <= nn - kk; i++)
+        for (i = 0; i <= NN - KK; i++)
             gg[i] = index_of[gg[i]];
     }
 
 public:
-    ReedSolomon_63(int a_tt)
+    ReedSolomon_63()
     {
-        tt = a_tt;        /* number of errors that can be corrected */
-        kk = nn-2*tt;
-
-        alpha_to = new int[nn + 1];
-        index_of = new int[nn + 1];
-        gg       = new int[nn - kk + 1];
+        alpha_to = new int[NN + 1];
+        index_of = new int[NN + 1];
+        gg       = new int[NN - KK + 1];
 
         // Polynom used in P25 is alpha**6+alpha+1
         int generator_polinomial[] = { 1, 1, 0, 0, 0, 0, 1 }; /* specify irreducible polynomial coeffts */
@@ -152,7 +150,7 @@ public:
         delete[] alpha_to;
     }
 
-    void encode(int* data, int* bb)
+    void encode(const int* data, int* bb)
     /* take the string of symbols in data[i], i=0..(k-1) and encode systematically
      to produce 2*tt parity symbols in bb[0]..bb[2*tt-1]
      data[] is input and bb[] is output in polynomial form.
@@ -163,19 +161,19 @@ public:
         register int i, j;
         int feedback;
 
-        for (i = 0; i < nn - kk; i++)
+        for (i = 0; i < NN - KK; i++)
             bb[i] = 0;
-        for (i = kk - 1; i >= 0; i--) {
-            feedback = index_of[data[i] ^ bb[nn - kk - 1]];
+        for (i = KK - 1; i >= 0; i--) {
+            feedback = index_of[data[i] ^ bb[NN - KK - 1]];
             if (feedback != -1) {
-                for (j = nn - kk - 1; j > 0; j--)
+                for (j = NN - KK - 1; j > 0; j--)
                     if (gg[j] != -1)
-                        bb[j] = bb[j - 1] ^ alpha_to[(gg[j] + feedback) % nn];
+                        bb[j] = bb[j - 1] ^ alpha_to[(gg[j] + feedback) % NN];
                     else
                         bb[j] = bb[j - 1];
-                bb[0] = alpha_to[(gg[0] + feedback) % nn];
+                bb[0] = alpha_to[(gg[0] + feedback) % NN];
             } else {
-                for (j = nn - kk - 1; j > 0; j--)
+                for (j = NN - KK - 1; j > 0; j--)
                     bb[j] = bb[j - 1];
                 bb[0] = 0;
             }
@@ -203,22 +201,22 @@ public:
      can be returned as error flags to the calling routine if desired.   */
     {
         register int i, j, u, q;
-        int elp[nn - kk + 2][nn - kk], d[nn - kk + 2], l[nn - kk + 2], u_lu[nn - kk
-                + 2], s[nn - kk + 1];
-        int count = 0, syn_error = 0, root[tt], loc[tt], z[tt + 1], err[nn], reg[tt
+        int elp[NN - KK + 2][NN - KK], d[NN - KK + 2], l[NN - KK + 2], u_lu[NN - KK
+                + 2], s[NN - KK + 1];
+        int count = 0, syn_error = 0, root[TT], loc[TT], z[TT + 1], err[NN], reg[TT
                 + 1];
 
         int irrecoverable_error = 0;
 
-        for (int i = 0; i < nn; i++)
+        for (int i = 0; i < NN; i++)
             recd[i] = index_of[input[i]]; /* put recd[i] into index form (ie as powers of alpha) */
 
         /* first form the syndromes */
-        for (i = 1; i <= nn - kk; i++) {
+        for (i = 1; i <= NN - KK; i++) {
             s[i] = 0;
-            for (j = 0; j < nn; j++)
+            for (j = 0; j < NN; j++)
                 if (recd[j] != -1)
-                    s[i] ^= alpha_to[(recd[j] + i * j) % nn]; /* recd[j] in index form */
+                    s[i] ^= alpha_to[(recd[j] + i * j) % NN]; /* recd[j] in index form */
             /* convert syndrome from polynomial form to index form  */
             if (s[i] != 0)
                 syn_error = 1; /* set flag if non-zero syndrome => error */
@@ -239,7 +237,7 @@ public:
             d[1] = s[1]; /* index form */
             elp[0][0] = 0; /* index form */
             elp[1][0] = 1; /* polynomial form */
-            for (i = 1; i < nn - kk; i++) {
+            for (i = 1; i < NN - KK; i++) {
                 elp[0][i] = -1; /* index form */
                 elp[1][i] = 0; /* polynomial form */
             }
@@ -281,12 +279,12 @@ public:
                         l[u + 1] = l[q] + u - q;
 
                     /* form new elp(x) */
-                    for (i = 0; i < nn - kk; i++)
+                    for (i = 0; i < NN - KK; i++)
                         elp[u + 1][i] = 0;
                     for (i = 0; i <= l[q]; i++)
                         if (elp[q][i] != -1)
-                            elp[u + 1][i + u - q] = alpha_to[(d[u] + nn - d[q]
-                                    + elp[q][i]) % nn];
+                            elp[u + 1][i + u - q] = alpha_to[(d[u] + NN - d[q]
+                                    + elp[q][i]) % NN];
                     for (i = 0; i <= l[u]; i++) {
                         elp[u + 1][i] ^= elp[u][i];
                         elp[u][i] = index_of[elp[u][i]]; /*convert old elp value to index*/
@@ -295,7 +293,7 @@ public:
                 u_lu[u + 1] = u - l[u + 1];
 
                 /* form (u+1)th discrepancy */
-                if (u < nn - kk) /* no discrepancy computed on last iteration */
+                if (u < NN - KK) /* no discrepancy computed on last iteration */
                 {
                     if (s[u + 1] != -1)
                         d[u + 1] = alpha_to[s[u + 1]];
@@ -304,13 +302,13 @@ public:
                     for (i = 1; i <= l[u + 1]; i++)
                         if ((s[u + 1 - i] != -1) && (elp[u + 1][i] != 0))
                             d[u + 1] ^= alpha_to[(s[u + 1 - i]
-                                    + index_of[elp[u + 1][i]]) % nn];
+                                    + index_of[elp[u + 1][i]]) % NN];
                     d[u + 1] = index_of[d[u + 1]]; /* put d[u+1] into index form */
                 }
-            } while ((u < nn - kk) && (l[u + 1] <= tt));
+            } while ((u < NN - KK) && (l[u + 1] <= TT));
 
             u++;
-            if (l[u] <= tt) /* can correct error */
+            if (l[u] <= TT) /* can correct error */
             {
                 /* put elp into index form */
                 for (i = 0; i <= l[u]; i++)
@@ -320,17 +318,17 @@ public:
                 for (i = 1; i <= l[u]; i++)
                     reg[i] = elp[u][i];
                 count = 0;
-                for (i = 1; i <= nn; i++) {
+                for (i = 1; i <= NN; i++) {
                     q = 1;
                     for (j = 1; j <= l[u]; j++)
                         if (reg[j] != -1) {
-                            reg[j] = (reg[j] + j) % nn;
+                            reg[j] = (reg[j] + j) % NN;
                             q ^= alpha_to[reg[j]];
                         };
                     if (!q) /* store root and error location number indices */
                     {
                         root[count] = i;
-                        loc[count] = nn - i;
+                        loc[count] = NN - i;
                         count++;
                     };
                 };
@@ -350,12 +348,12 @@ public:
                             z[i] = 0;
                         for (j = 1; j < i; j++)
                             if ((s[j] != -1) && (elp[u][i - j] != -1))
-                                z[i] ^= alpha_to[(elp[u][i - j] + s[j]) % nn];
+                                z[i] ^= alpha_to[(elp[u][i - j] + s[j]) % NN];
                         z[i] = index_of[z[i]]; /* put into index form */
                     };
 
                     /* evaluate errors at locations given by error location numbers loc[i] */
-                    for (i = 0; i < nn; i++) {
+                    for (i = 0; i < NN; i++) {
                         err[i] = 0;
                         if (recd[i] != -1) /* convert recd[] to polynomial form */
                             recd[i] = alpha_to[recd[i]];
@@ -367,16 +365,16 @@ public:
                         err[loc[i]] = 1; /* accounts for z[0] */
                         for (j = 1; j <= l[u]; j++)
                             if (z[j] != -1)
-                                err[loc[i]] ^= alpha_to[(z[j] + j * root[i]) % nn];
+                                err[loc[i]] ^= alpha_to[(z[j] + j * root[i]) % NN];
                         if (err[loc[i]] != 0) {
                             err[loc[i]] = index_of[err[loc[i]]];
                             q = 0; /* form denominator of error term */
                             for (j = 0; j < l[u]; j++)
                                 if (j != i)
                                     q += index_of[1
-                                            ^ alpha_to[(loc[j] + root[i]) % nn]];
-                            q = q % nn;
-                            err[loc[i]] = alpha_to[(err[loc[i]] - q + nn) % nn];
+                                            ^ alpha_to[(loc[j] + root[i]) % NN]];
+                            q = q % NN;
+                            err[loc[i]] = alpha_to[(err[loc[i]] - q + NN) % NN];
                             recd[loc[i]] ^= err[loc[i]]; /*recd[i] must be in polynomial form */
                         }
                     }
@@ -392,7 +390,7 @@ public:
 
         } else {
             /* no non-zero syndromes => no errors: output received codeword */
-            for (i = 0; i < nn; i++)
+            for (i = 0; i < NN; i++)
                 if (recd[i] != -1) /* convert recd[] to polynomial form */
                     recd[i] = alpha_to[recd[i]];
                 else
@@ -400,7 +398,7 @@ public:
         }
 
         if (irrecoverable_error) {
-            for (i = 0; i < nn; i++) /* could return error flag if desired */
+            for (i = 0; i < NN; i++) /* could return error flag if desired */
                 if (recd[i] != -1) /* convert recd[] to polynomial form */
                     recd[i] = alpha_to[recd[i]];
                 else
@@ -411,7 +409,7 @@ public:
     }
 
 protected:
-    int bin_to_hex(char* input)
+    int bin_to_hex(const char* input)
     {
         int output = ((input[0] != 0)? 32 : 0) |
                      ((input[1] != 0)? 16 : 0) |
@@ -438,12 +436,12 @@ protected:
  * Convenience class that does a Reed-Solomon (36,20,17) error correction adapting input and output to
  * the DSD data format: hex words packed as char arrays.
  */
-class DSDReedSolomon_36_20_17 : public ReedSolomon_63
+class DSDReedSolomon_36_20_17 : public ReedSolomon_63<8>
 {
 public:
     // tt = (dd-1)/2
     // dd = 17 --> tt = 8
-    DSDReedSolomon_36_20_17() : ReedSolomon_63(8)
+    DSDReedSolomon_36_20_17() : ReedSolomon_63<8>()
     {
         // Does nothing
     }
@@ -451,11 +449,11 @@ public:
     /**
      * Does a Reed-Solomon decode adapting the input and output to the expected DSD data format.
      * \param hex_data Data packed bits, originally a char[20][6], so containing 20 hex works, each char
-     *                 is a bit.
+     *                 is a bit. Bits are corrected in place.
      * \param hex_parity Parity packed bits, originally a char[16][6], 16 hex words.
      * \return 1 if irrecoverable errors have been detected, 0 otherwise.
      */
-    int decode(char* hex_data, char* hex_parity)
+    int decode(char* hex_data, const char* hex_parity)
     {
         int input[63];
         int output[63];
@@ -476,7 +474,7 @@ public:
         }
 
         // Now we can call decode on the base class
-        int irrecoverable_errors = ReedSolomon_63::decode(input, output);
+        int irrecoverable_errors = ReedSolomon_63<8>::decode(input, output);
 
         // Convert it back to binary and put it into hex_data. If decode failed we should have
         // the input unchanged.
@@ -486,18 +484,42 @@ public:
 
         return irrecoverable_errors;
     }
+
+    void encode(const char* hex_data, char* out_hex_parity)
+    {
+        int input[47];
+        int output[63];
+
+        // Put the 20 hex words of data
+        for(unsigned int i=0; i<20; i++) {
+            input[i] = bin_to_hex(hex_data + i*6);
+        }
+
+        // Fill up with zeros to complete the 47 expected hex words of data
+        for(unsigned int i=20; i<47; i++) {
+            input[i] = 0;
+        }
+
+        // Now we can call encode on the base class
+        ReedSolomon_63<8>::encode(input, output);
+
+        // Convert it back to binary form and put it into the parity
+        for(unsigned int i=0; i<16; i++) {
+            hex_to_bin(output[i], out_hex_parity + i*6);
+        }
+    }
 };
 
 /**
  * Convenience class that does a Reed-Solomon (24,12,13) error correction adapting input and output to
  * the DSD data format: hex words packed as char arrays.
  */
-class DSDReedSolomon_24_12_13 : public ReedSolomon_63
+class DSDReedSolomon_24_12_13 : public ReedSolomon_63<6>
 {
 public:
     // tt = (dd-1)/2
     // dd = 13 --> tt = 6
-    DSDReedSolomon_24_12_13() : ReedSolomon_63(6)
+    DSDReedSolomon_24_12_13() : ReedSolomon_63<6>()
     {
         // Does nothing
     }
@@ -505,11 +527,11 @@ public:
     /**
      * Does a Reed-Solomon decode adapting the input and output to the expected DSD data format.
      * \param hex_data Data packed bits, originally a char[12][6], so containing 12 hex works, each char
-     *                 is a bit.
+     *                 is a bit. Bits are corrected in place.
      * \param hex_parity Parity packed bits, originally a char[12][6], 12 hex words.
      * \return 1 if irrecoverable errors have been detected, 0 otherwise.
      */
-    int decode(char* hex_data, char* hex_parity)
+    int decode(char* hex_data, const char* hex_parity)
     {
         int input[63];
         int output[63];
@@ -530,7 +552,7 @@ public:
         }
 
         // Now we can call decode on the base class
-        int irrecoverable_errors = ReedSolomon_63::decode(input, output);
+        int irrecoverable_errors = ReedSolomon_63<6>::decode(input, output);
 
         // Convert it back to binary and put it into hex_data. If decode failed we should have
         // the input unchanged.
@@ -540,18 +562,43 @@ public:
 
         return irrecoverable_errors;
     }
+
+    void encode(const char* hex_data, char* out_hex_parity)
+    {
+        int input[51];
+        int output[63];
+
+        // Put the 12 hex words of data
+        for(unsigned int i=0; i<12; i++) {
+            input[i] = bin_to_hex(hex_data + i*6);
+        }
+
+        // Fill up with zeros to complete the 51 expected hex words of data
+        for(unsigned int i=12; i<51; i++) {
+            input[i] = 0;
+        }
+
+        // Now we can call encode on the base class
+        ReedSolomon_63<6>::encode(input, output);
+
+        // Convert it back to binary form and put it into the parity
+        for(unsigned int i=0; i<12; i++) {
+            hex_to_bin(output[i], out_hex_parity + i*6);
+        }
+    }
+
 };
 
 /**
  * Convenience class that does a Reed-Solomon (24,16,9) error correction adapting input and output to
  * the DSD data format: hex words packed as char arrays.
  */
-class DSDReedSolomon_24_16_9 : public ReedSolomon_63
+class DSDReedSolomon_24_16_9 : public ReedSolomon_63<4>
 {
 public:
     // tt = (dd-1)/2
     // dd = 9 --> tt = 4
-    DSDReedSolomon_24_16_9() : ReedSolomon_63(4)
+    DSDReedSolomon_24_16_9() : ReedSolomon_63<4>()
     {
         // Does nothing
     }
@@ -559,11 +606,11 @@ public:
     /**
      * Does a Reed-Solomon decode adapting the input and output to the expected DSD data format.
      * \param hex_data Data packed bits, originally a char[16][6], so containing 16 hex works, each char
-     *                 is a bit.
+     *                 is a bit. Bits are corrected in place.
      * \param hex_parity Parity packed bits, originally a char[8][6], 8 hex words.
      * \return 1 if irrecoverable errors have been detected, 0 otherwise.
      */
-    int decode(char* hex_data, char* hex_parity)
+    int decode(char* hex_data, const char* hex_parity)
     {
         int input[63];
         int output[63];
@@ -574,26 +621,50 @@ public:
         }
 
         // Then the 16 hex words of data
-        for(unsigned int i=8; i<8+12; i++) {
+        for(unsigned int i=8; i<8+16; i++) {
             input[i] = bin_to_hex(hex_data + (i-8)*6);
         }
 
         // Fill up with zeros to complete the 55 expected hex words of data
-        for(unsigned int i=8+12; i<63; i++) {
+        for(unsigned int i=8+16; i<63; i++) {
             input[i] = 0;
         }
 
         // Now we can call decode on the base class
-        int irrecoverable_errors = ReedSolomon_63::decode(input, output);
+        int irrecoverable_errors = ReedSolomon_63<4>::decode(input, output);
 
         // Convert it back to binary and put it into hex_data. If decode failed we should have
         // the input unchanged.
-        for(unsigned int i=8; i<8+12; i++) {
+        for(unsigned int i=8; i<8+16; i++) {
             hex_to_bin(output[i], hex_data + (i-8)*6);
         }
 
         return irrecoverable_errors;
     }
+
+    void encode(const char* hex_data, char* out_hex_parity)
+    {
+        int input[55];
+        int output[63];
+
+        // Put the 16 hex words of data
+        for(unsigned int i=0; i<16; i++) {
+            input[i] = bin_to_hex(hex_data + i*6);
+        }
+
+        // Fill up with zeros to complete the 55 expected hex words of data
+        for(unsigned int i=16; i<55; i++) {
+            input[i] = 0;
+        }
+
+        // Now we can call encode on the base class
+        ReedSolomon_63<4>::encode(input, output);
+
+        // Convert it back to binary form and put it into the parity
+        for(unsigned int i=0; i<8; i++) {
+            hex_to_bin(output[i], out_hex_parity + i*6);
+        }
+    }
 };
 
-#endif // __REED_SOLOMON_HPP__
+#endif // REEDSOLOMON_HPP_b1405fdab6374ba2a4e65e8d45ec3d80
diff --git a/dsd.h b/dsd.h
index b34f6fc..1accef7 100644
--- a/dsd.h
+++ b/dsd.h
@@ -40,6 +40,9 @@
 #include <math.h>
 #include <mbelib.h>
 #include <sndfile.h>
+
+#include "p25p1_heuristics.h"
+
 /*
  * global variables
  */
@@ -176,6 +179,26 @@ typedef struct
   unsigned int debug_header_errors;
   unsigned int debug_header_critical_errors;
 
+  // Last dibit read
+  int last_dibit;
+
+  // Heuristics state data for +P5 signals
+  P25Heuristics p25_heuristics;
+
+  // Heuristics state data for -P5 signals
+  P25Heuristics inv_p25_heuristics;
+
+#ifdef TRACE_DSD
+  char debug_prefix;
+  char debug_prefix_2;
+  unsigned int debug_sample_index;
+  unsigned int debug_sample_left_edge;
+  unsigned int debug_sample_right_edge;
+  FILE* debug_label_file;
+  FILE* debug_label_dibit_file;
+  FILE* debug_label_imbe_file;
+#endif
+
 } dsd_state;
 
 /*
@@ -189,7 +212,7 @@ typedef struct
 #define X2TDMA_MS_DATA_SYNC  "313113333111111133333313"
 #define X2TDMA_MS_VOICE_SYNC "131331111333333311111131"
 
-#define DSTAR_HD	   "131313131333133113131111"
+#define DSTAR_HD       "131313131333133113131111"
 #define INV_DSTAR_HD   "313131313111311331313333"
 #define DSTAR_SYNC     "313131313133131113313111"
 #define INV_DSTAR_SYNC "131313131311313331131333"
@@ -223,7 +246,10 @@ void writeSynthesizedVoice (dsd_opts * opts, dsd_state * state);
 void playSynthesizedVoice (dsd_opts * opts, dsd_state * state);
 void openAudioOutDevice (dsd_opts * opts, int speed);
 void openAudioInDevice (dsd_opts * opts);
+
 int getDibit (dsd_opts * opts, dsd_state * state);
+int get_dibit_and_analog_signal (dsd_opts * opts, dsd_state * state, int * out_analog_signal);
+
 void skipDibit (dsd_opts * opts, dsd_state * state, int count);
 void saveImbe4400Data (dsd_opts * opts, dsd_state * state, char *imbe_d);
 void saveAmbe2450Data (dsd_opts * opts, dsd_state * state, char *ambe_d);
@@ -260,6 +286,7 @@ void processP25lcw (dsd_opts * opts, dsd_state * state, char *lcformat, char *mf
 void processHDU (dsd_opts * opts, dsd_state * state);
 void processLDU1 (dsd_opts * opts, dsd_state * state);
 void processLDU2 (dsd_opts * opts, dsd_state * state);
+void processTDU (dsd_opts * opts, dsd_state * state);
 void processTDULC (dsd_opts * opts, dsd_state * state);
 void processProVoice (dsd_opts * opts, dsd_state * state);
 void processX2TDMAdata (dsd_opts * opts, dsd_state * state);
diff --git a/dsd_audio.c b/dsd_audio.c
index 4d35384..3a1e335 100644
--- a/dsd_audio.c
+++ b/dsd_audio.c
@@ -90,18 +90,19 @@ processAudio (dsd_opts * opts, dsd_state * state)
       gaindelta = (float) 0;
     }
 
-  if(opts->audio_gain >= 0){
-    // adjust output gain
-    state->audio_out_temp_buf_p = state->audio_out_temp_buf;
-    for (n = 0; n < 160; n++)
-      {
-        *state->audio_out_temp_buf_p = (state->aout_gain + ((float) n * gaindelta)) * (*state->audio_out_temp_buf_p);
-        state->audio_out_temp_buf_p++;
-      }
-    state->aout_gain += ((float) 160 * gaindelta);
-  }
+  if(opts->audio_gain >= 0)
+    {
+      // adjust output gain
+      state->audio_out_temp_buf_p = state->audio_out_temp_buf;
+      for (n = 0; n < 160; n++)
+        {
+          *state->audio_out_temp_buf_p = (state->aout_gain + ((float) n * gaindelta)) * (*state->audio_out_temp_buf_p);
+          state->audio_out_temp_buf_p++;
+        }
+      state->aout_gain += ((float) 160 * gaindelta);
+    }
 
-  // copy audio datat to output buffer and upsample if necessary
+  // copy audio data to output buffer and upsample if necessary
   state->audio_out_temp_buf_p = state->audio_out_temp_buf;
   if (opts->split == 0)
     {
@@ -117,13 +118,13 @@ processAudio (dsd_opts * opts, dsd_state * state)
       // copy to output (short) buffer
       for (n = 0; n < 960; n++)
         {
-          if (*state->audio_out_float_buf_p > (float) 32760)
+          if (*state->audio_out_float_buf_p >  32767.0F)
             {
-              *state->audio_out_float_buf_p = (float) 32760;
+              *state->audio_out_float_buf_p = 32767.0F;
             }
-          else if (*state->audio_out_float_buf_p < (float) -32760)
+          else if (*state->audio_out_float_buf_p < -32768.0F)
             {
-              *state->audio_out_float_buf_p = (float) -32760;
+              *state->audio_out_float_buf_p = -32768.0F;
             }
           *state->audio_out_buf_p = (short) *state->audio_out_float_buf_p;
           state->audio_out_buf_p++;
@@ -135,13 +136,13 @@ processAudio (dsd_opts * opts, dsd_state * state)
     {
       for (n = 0; n < 160; n++)
         {
-          if (*state->audio_out_temp_buf_p > (float) 32760)
+          if (*state->audio_out_temp_buf_p > 32767.0F)
             {
-              *state->audio_out_temp_buf_p = (float) 32760;
+              *state->audio_out_temp_buf_p = 32767.0F;
             }
-          else if (*state->audio_out_temp_buf_p < (float) -32760)
+          else if (*state->audio_out_temp_buf_p < -32768.0F)
             {
-              *state->audio_out_temp_buf_p = (float) -32760;
+              *state->audio_out_temp_buf_p = -32768.0F;
             }
           *state->audio_out_buf_p = (short) *state->audio_out_temp_buf_p;
           state->audio_out_buf_p++;
@@ -240,15 +241,18 @@ openAudioOutDevice (dsd_opts * opts, int speed)
 {
   // get info of device/file
   struct stat stat_buf;
-  if(stat(opts->audio_out_dev, &stat_buf) != 0) {
-    printf("Error, couldn't open %s\n", opts->audio_out_dev);
-    exit(1);
-  }
+  if(stat(opts->audio_out_dev, &stat_buf) != 0)
+    {
+      printf("Error, couldn't open %s\n", opts->audio_out_dev);
+      exit(1);
+    }
 
-  if( !(S_ISCHR(stat_buf.st_mode) || S_ISBLK(stat_buf.st_mode))) { // this is not a device
-    printf("Error, %s is not a device. use -w filename for wav output.\n", opts->audio_out_dev);
-    exit(1);
-  }
+  if( !(S_ISCHR(stat_buf.st_mode) || S_ISBLK(stat_buf.st_mode)))
+    {
+      // this is not a device
+      printf("Error, %s is not a device. use -w filename for wav output.\n", opts->audio_out_dev);
+      exit(1);
+    }
 #ifdef SOLARIS
   sample_info_t aset, aget;
 
@@ -320,22 +324,29 @@ openAudioInDevice (dsd_opts * opts)
 {
   // get info of device/file
   struct stat stat_buf;
-  if (stat(opts->audio_in_dev, &stat_buf) != 0) {
-    printf("Error, couldn't open %s\n", opts->audio_in_dev);
-    exit(1);
-  }
-  if(S_ISREG(stat_buf.st_mode)) { // is this a regular file? then process with libsndfile.
-    opts->audio_in_type = 1;
-    opts->audio_in_file_info = calloc(1, sizeof(SF_INFO));
-    opts->audio_in_file_info->channels = 1;
-    opts->audio_in_file = sf_open(opts->audio_in_dev, SFM_READ, opts->audio_in_file_info);
-    if(opts->audio_in_file == NULL) {
-        printf ("Error, couldn't open file %s\n", opts->audio_in_dev);
-        exit(1);
+  if (stat(opts->audio_in_dev, &stat_buf) != 0)
+    {
+      printf("Error, couldn't open %s\n", opts->audio_in_dev);
+      exit(1);
     }
-  }
-  else { // this is a device, use old handling
-  opts->audio_in_type = 0;
+  if (S_ISREG(stat_buf.st_mode))
+    {
+      // is this a regular file? then process with libsndfile.
+      opts->audio_in_type = 1;
+      opts->audio_in_file_info = calloc(1, sizeof(SF_INFO));
+      opts->audio_in_file_info->channels = 1;
+      opts->audio_in_file = sf_open(opts->audio_in_dev, SFM_READ, opts->audio_in_file_info);
+
+      if(opts->audio_in_file == NULL)
+        {
+          printf ("Error, couldn't open file %s\n", opts->audio_in_dev);
+          exit(1);
+        }
+    }
+  else
+    {
+      // this is a device, use old handling
+      opts->audio_in_type = 0;
 #ifdef SOLARIS
     sample_info_t aset, aget;
     int rgain;
diff --git a/dsd_dibit.c b/dsd_dibit.c
index 62bbd20..ee3ea58 100644
--- a/dsd_dibit.c
+++ b/dsd_dibit.c
@@ -15,29 +15,120 @@
  * PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <assert.h>
+
 #include "dsd.h"
 
-int
-getDibit (dsd_opts * opts, dsd_state * state)
+#include "p25p1_heuristics.h"
+
+static void
+print_datascope(dsd_opts* opts, dsd_state* state, int* sbuf2)
 {
-  // returns one dibit value
-  int i, j, o, symbol;
-  int sbuf2[128];
-  int spectrum[64];
+  int i, j, o;
   char modulation[8];
+  int spectrum[64];
+
+  if (state->rf_mod == 0)
+    {
+      sprintf (modulation, "C4FM");
+    }
+  else if (state->rf_mod == 1)
+    {
+      sprintf (modulation, "QPSK");
+    }
+  else if (state->rf_mod == 2)
+    {
+      sprintf (modulation, "GFSK");
+    }
+
+  for (i = 0; i < 64; i++)
+    {
+      spectrum[i] = 0;
+    }
+  for (i = 0; i < opts->ssize; i++)
+    {
+      o = (sbuf2[i] + 32768) / 1024;
+      spectrum[o]++;
+    }
+  if (state->symbolcnt > (4800 / opts->scoperate))
+    {
+      state->symbolcnt = 0;
+      printf ("\n");
+      printf ("Demod mode:     %s                Nac:                     %4X\n", modulation, state->nac);
+      printf ("Frame Type:    %s        Talkgroup:            %7i\n", state->ftype, state->lasttg);
+      printf ("Frame Subtype: %s       Source:          %12i\n", state->fsubtype, state->lastsrc);
+      printf ("TDMA activity:  %s %s     Voice errors: %s\n", state->slot0light, state->slot1light, state->err_str);
+      printf ("+----------------------------------------------------------------+\n");
+      for (i = 0; i < 10; i++)
+        {
+          printf ("|");
+          for (j = 0; j < 64; j++)
+            {
+              if (i == 0)
+                {
+                  if ((j == ((state->min) + 32768) / 1024) || (j == ((state->max) + 32768) / 1024))
+                    {
+                      printf ("#");
+                    }
+                  else if ((j == ((state->lmid) + 32768) / 1024) || (j == ((state->umid) + 32768) / 1024))
+                    {
+                      printf ("^");
+                    }
+                  else if (j == (state->center + 32768) / 1024)
+                    {
+                      printf ("!");
+                    }
+                  else
+                    {
+                      if (j == 32)
+                        {
+                          printf ("|");
+                        }
+                      else
+                        {
+                          printf (" ");
+                        }
+                    }
+                }
+              else
+                {
+                  if (spectrum[j] > 9 - i)
+                    {
+                      printf ("*");
+                    }
+                  else
+                    {
+                      if (j == 32)
+                        {
+                          printf ("|");
+                        }
+                      else
+                        {
+                          printf (" ");
+                        }
+                    }
+                }
+            }
+          printf ("|\n");
+        }
+      printf ("+----------------------------------------------------------------+\n");
+    }
+}
+
+static void
+use_symbol (dsd_opts* opts, dsd_state* state, int symbol)
+{
+  int i;
+  int sbuf2[128];
   int lmin, lmax, lsum;
 
-  state->numflips = 0;
-
-  symbol = getSymbol (opts, state, 1);
-  state->sbuf[state->sidx] = symbol;
-
   for (i = 0; i < opts->ssize; i++)
     {
       sbuf2[i] = state->sbuf[i];
     }
 
   qsort (sbuf2, opts->ssize, sizeof (int), comp);
+
   // continuous update of min/max in rf_mod=1 (QPSK) mode
   // in c4fm min/max must only be updated during sync
   if (state->rf_mod == 1)
@@ -69,8 +160,8 @@ getDibit (dsd_opts * opts, dsd_state * state)
       state->center = ((state->max) + (state->min)) / 2;
       state->umid = (((state->max) - state->center) * 5 / 8) + state->center;
       state->lmid = (((state->min) - state->center) * 5 / 8) + state->center;
-      state->maxref = ((state->max) * 0.80);
-      state->minref = ((state->min) * 0.80);
+      state->maxref = (int)((state->max) * 0.80F);
+      state->minref = (int)((state->min) * 0.80F);
     }
   else
     {
@@ -78,98 +169,14 @@ getDibit (dsd_opts * opts, dsd_state * state)
       state->minref = state->min;
     }
 
+  // Increase sidx
   if (state->sidx == (opts->ssize - 1))
     {
-
       state->sidx = 0;
 
       if (opts->datascope == 1)
         {
-          if (state->rf_mod == 0)
-            {
-              sprintf (modulation, "C4FM");
-            }
-          else if (state->rf_mod == 1)
-            {
-              sprintf (modulation, "QPSK");
-            }
-          else if (state->rf_mod == 2)
-            {
-              sprintf (modulation, "GFSK");
-            }
-
-          for (i = 0; i < 64; i++)
-            {
-              spectrum[i] = 0;
-            }
-          for (i = 0; i < opts->ssize; i++)
-            {
-              o = (sbuf2[i] + 32768) / 1024;
-              spectrum[o]++;
-            }
-          if (state->symbolcnt > (4800 / opts->scoperate))
-            {
-              state->symbolcnt = 0;
-              printf ("\n");
-              printf ("Demod mode:     %s                Nac:                     %4X\n", modulation, state->nac);
-              printf ("Frame Type:    %s        Talkgroup:            %7i\n", state->ftype, state->lasttg);
-              printf ("Frame Subtype: %s       Source:          %12i\n", state->fsubtype, state->lastsrc);
-              printf ("TDMA activity:  %s %s     Voice errors: %s\n", state->slot0light, state->slot1light, state->err_str);
-              printf ("+----------------------------------------------------------------+\n");
-              for (i = 0; i < 10; i++)
-                {
-                  printf ("|");
-                  for (j = 0; j < 64; j++)
-                    {
-                      if (i == 0)
-                        {
-                          if ((j == ((state->min) + 32768) / 1024) || (j == ((state->max) + 32768) / 1024))
-                            {
-                              printf ("#");
-                            }
-                          else if ((j == ((state->lmid) + 32768) / 1024) || (j == ((state->umid) + 32768) / 1024))
-                            {
-                              printf ("^");
-                            }
-                          else if (j == (state->center + 32768) / 1024)
-                            {
-                              printf ("!");
-                            }
-                          else
-                            {
-                              if (j == 32)
-                                {
-                                  printf ("|");
-                                }
-                              else
-                                {
-                                  printf (" ");
-                                }
-                            }
-                        }
-                      else
-                        {
-                          if (spectrum[j] > 9 - i)
-                            {
-                              printf ("*");
-                            }
-                          else
-                            {
-                              if (j == 32)
-                                {
-                                  printf ("|");
-                                }
-                              else
-                                {
-                                  printf (" ");
-                                }
-                            }
-                        }
-                    }
-                  printf ("|\n");
-                }
-              printf ("+----------------------------------------------------------------+\n");
-            }
+          print_datascope(opts, state, sbuf2);
         }
     }
   else
@@ -179,12 +186,39 @@ getDibit (dsd_opts * opts, dsd_state * state)
 
   if (state->dibit_buf_p > state->dibit_buf + 900000)
     {
-	  state->dibit_buf_p = state->dibit_buf + 200;
+      state->dibit_buf_p = state->dibit_buf + 200;
+    }
+}
+
+static int
+invert_dibit(int dibit)
+{
+    switch (dibit)
+    {
+    case 0:
+        return 2;
+    case 1:
+        return 3;
+    case 2:
+        return 0;
+    case 3:
+        return 1;
     }
 
+    // Error, shouldn't be here
+    assert(0);
+    return -1;
+}
+
+static int digitize (dsd_opts* opts, dsd_state* state, int symbol)
+{
   // determine dibit state
   if ((state->synctype == 6) || (state->synctype == 14)|| (state->synctype == 18))
     {
+      //  6 +D-STAR
+      // 14 +ProVoice
+      // 18 +D-STAR_HD
+
       if (symbol > state->center)
         {
           *state->dibit_buf_p = 1;
@@ -217,7 +251,8 @@ getDibit (dsd_opts * opts, dsd_state * state)
           return (0);               // +1
         }
     }
-  else if ((state->synctype == 1) || (state->synctype == 3) || (state->synctype == 5) || (state->synctype == 9) || (state->synctype == 11) || (state->synctype == 13))
+  else if ((state->synctype == 1) || (state->synctype == 3) || (state->synctype == 5) ||
+          (state->synctype == 9) || (state->synctype == 11) || (state->synctype == 13) || (state->synctype == 17))
     {
       //  1 -P25p1
       //  3 -X2-TDMA (inverted signal voice frame)
@@ -225,37 +260,53 @@ getDibit (dsd_opts * opts, dsd_state * state)
       //  9 -NXDN (inverted voice frame)
       // 11 -DMR (inverted signal voice frame)
       // 13 -DMR (inverted signal data frame)
+      // 17 -NXDN (inverted data frame)
 
-      if (symbol > state->center)
+      int valid;
+      int dibit;
+
+      valid = 0;
+
+      if (state->synctype == 1)
         {
-          if (symbol > state->umid)
+          // Use the P25 heuristics if available
+          valid = estimate_symbol(state->rf_mod, &(state->inv_p25_heuristics), state->last_dibit, symbol, &dibit);
+        }
+
+      if (valid == 0)
+        {
+          // Revert to the original approach: choose the symbol according to the regions delimited
+          // by center, umid and lmid
+          if (symbol > state->center)
             {
-              *state->dibit_buf_p = 1;  // store non-inverted values in dibit_buf
-              state->dibit_buf_p++;
-              return (3);
+              if (symbol > state->umid)
+                {
+                  dibit = 3;               // -3
+                }
+              else
+                {
+                  dibit = 2;               // -1
+                }
             }
           else
             {
-              *state->dibit_buf_p = 0;
-              state->dibit_buf_p++;
-              return (2);
-            }
-        }
-      else
-        {
-          if (symbol < state->lmid)
-            {
-              *state->dibit_buf_p = 3;
-              state->dibit_buf_p++;
-              return (1);
-            }
-          else
-            {
-              *state->dibit_buf_p = 2;
-              state->dibit_buf_p++;
-              return (0);
+              if (symbol < state->lmid)
+                {
+                  dibit = 1;               // +3
+                }
+              else
+                {
+                  dibit = 0;               // +2
+                }
             }
         }
+
+      state->last_dibit = dibit;
+
+      // store non-inverted values in dibit_buf
+      *state->dibit_buf_p = invert_dibit(dibit);
+      state->dibit_buf_p++;
+      return dibit;
     }
   else
     {
@@ -266,41 +317,112 @@ getDibit (dsd_opts * opts, dsd_state * state)
       // 10 +DMR (non inverted signal data frame)
       // 12 +DMR (non inverted signal voice frame)
       // 16 +NXDN (non inverted data frame)
-      // 17 -NXDN (inverted data frame)             ???
 
-      if (symbol > state->center)
+      int valid;
+      int dibit;
+
+      valid = 0;
+
+      if (state->synctype == 0)
         {
-          if (symbol > state->umid)
+          // Use the P25 heuristics if available
+          valid = estimate_symbol(state->rf_mod, &(state->p25_heuristics), state->last_dibit, symbol, &dibit);
+        }
+
+      if (valid == 0)
+        {
+          // Revert to the original approach: choose the symbol according to the regions delimited
+          // by center, umid and lmid
+          if (symbol > state->center)
             {
-              *state->dibit_buf_p = 1;
-              state->dibit_buf_p++;
-              return (1);
+              if (symbol > state->umid)
+                {
+                  dibit = 1;               // +3
+                }
+              else
+                {
+                  dibit = 0;               // +1
+                }
             }
           else
             {
-              *state->dibit_buf_p = 0;
-              state->dibit_buf_p++;
-              return (0);
-            }
-        }
-      else
-        {
-          if (symbol < state->lmid)
-            {
-              *state->dibit_buf_p = 3;
-              state->dibit_buf_p++;
-              return (3);
-            }
-          else
-            {
-              *state->dibit_buf_p = 2;
-              state->dibit_buf_p++;
-              return (2);
+              if (symbol < state->lmid)
+                {
+                  dibit = 3;               // -3
+                }
+              else
+                {
+                  dibit = 2;               // -1
+                }
             }
         }
+
+      state->last_dibit = dibit;
+
+      *state->dibit_buf_p = dibit;
+      state->dibit_buf_p++;
+      return dibit;
     }
 }
 
+int
+get_dibit_and_analog_signal (dsd_opts* opts, dsd_state* state, int* out_analog_signal)
+{
+  int symbol;
+  int dibit;
+
+#ifdef TRACE_DSD
+  unsigned int l, r;
+#endif
+
+  state->numflips = 0;
+
+#ifdef TRACE_DSD
+  l = state->debug_sample_index;
+#endif
+
+  symbol = getSymbol (opts, state, 1);
+
+#ifdef TRACE_DSD
+  r = state->debug_sample_index;
+#endif
+
+  //
+  state->sbuf[state->sidx] = symbol;
+  if (out_analog_signal != NULL)
+    {
+      *out_analog_signal = symbol;
+    }
+
+  use_symbol (opts, state, symbol);
+
+  dibit = digitize (opts, state, symbol);
+
+#ifdef TRACE_DSD
+  {
+      float left, right;
+      if (state->debug_label_dibit_file == NULL) {
+          state->debug_label_dibit_file = fopen ("pp_label_dibit.txt", "w");
+      }
+      left = l / 48000.0;
+      right = r / 48000.0;
+      fprintf(state->debug_label_dibit_file, "%f\t%f\t%i\n", left, right, dibit);
+  }
+#endif
+
+  return dibit;
+}
+
+/**
+ * \brief This important method reads the last analog signal value (getSymbol call) and digitizes it.
+ * Depending of the ongoing transmission it in converted into a bit (0/1) or a di-bit (00/01/10/11).
+ */
+int
+getDibit (dsd_opts* opts, dsd_state* state)
+{
+  return get_dibit_and_analog_signal (opts, state, NULL);
+}
+
 void
 skipDibit (dsd_opts * opts, dsd_state * state, int count)
 {
diff --git a/dsd_frame.c b/dsd_frame.c
index 7b59728..539c391 100644
--- a/dsd_frame.c
+++ b/dsd_frame.c
@@ -447,10 +447,7 @@ processFrame (dsd_opts * opts, dsd_state * state)
       state->err_str[0] = 0;
       sprintf (state->fsubtype, " TDU          ");
 
-      // Now processing NID
-
-      skipDibit (opts, state, 14);    // 28 null bits
-      status_1 = getDibit (opts, state) + '0';
+      processTDU (opts, state);
     }
   else if (strcmp (duid, "13") == 0)
     {
diff --git a/dsd_main.c b/dsd_main.c
index d59704c..74dca94 100644
--- a/dsd_main.c
+++ b/dsd_main.c
@@ -25,6 +25,7 @@
 #include "dmr_const.h"
 #include "provoice_const.h"
 #include "git_ver.h"
+#include "p25p1_heuristics.h"
 
 int
 comp (const void *a, const void *b)
@@ -224,6 +225,15 @@ initState (dsd_state * state)
   state->debug_audio_errors = 0;
   state->debug_header_errors = 0;
   state->debug_header_critical_errors = 0;
+
+#ifdef TRACE_DSD
+  state->debug_sample_index = 0;
+  state->debug_label_file = NULL;
+  state->debug_label_dibit_file = NULL;
+  state->debug_label_imbe_file = NULL;
+#endif
+
+  initialize_p25_heuristics(&state->p25_heuristics);
 }
 
 void
@@ -305,7 +315,17 @@ liveScanner (dsd_opts * opts, dsd_state * state)
       while (state->synctype != -1)
         {
           processFrame (opts, state);
+
+#ifdef TRACE_DSD
+          state->debug_prefix = 'S';
+#endif
+
           state->synctype = getFrameSync (opts, state);
+
+#ifdef TRACE_DSD
+          state->debug_prefix = '\0';
+#endif
+
           // recalibrate center/umid/lmid
           state->center = ((state->max) + (state->min)) / 2;
           state->umid = (((state->max) - state->center) * 5 / 8) + state->center;
@@ -328,6 +348,28 @@ cleanupAndExit (dsd_opts * opts, dsd_state * state)
   printf("Total header errors: %i\n", state->debug_header_errors);
   printf("Total irrecoverable header errors: %i\n", state->debug_header_critical_errors);
 
+  //debug_print_heuristics(&(state->p25_heuristics));
+
+  printf("\n");
+  printf("+P25 BER estimate: %.2f%%\n", get_P25_BER_estimate(&state->p25_heuristics));
+  printf("-P25 BER estimate: %.2f%%\n", get_P25_BER_estimate(&state->inv_p25_heuristics));
+  printf("\n");
+
+#ifdef TRACE_DSD
+  if (state->debug_label_file != NULL) {
+      fclose(state->debug_label_file);
+      state->debug_label_file = NULL;
+  }
+  if(state->debug_label_dibit_file != NULL) {
+      fclose(state->debug_label_dibit_file);
+      state->debug_label_dibit_file = NULL;
+  }
+  if(state->debug_label_imbe_file != NULL) {
+      fclose(state->debug_label_imbe_file);
+      state->debug_label_imbe_file = NULL;
+  }
+#endif
+
   printf ("Exiting.\n");
   exit (0);
 }
@@ -342,7 +384,6 @@ sigfun (int sig)
 int
 main (int argc, char **argv)
 {
-
   int c;
   extern char *optarg;
   extern int optind, opterr, optopt;
diff --git a/dsd_symbol.c b/dsd_symbol.c
index 8915b55..be830bb 100644
--- a/dsd_symbol.c
+++ b/dsd_symbol.c
@@ -20,7 +20,6 @@
 int
 getSymbol (dsd_opts * opts, dsd_state * state, int have_sync)
 {
-
   short sample;
   int i, sum, symbol, count;
   ssize_t result;
@@ -78,6 +77,8 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync)
             }
           state->jitter = -1;
         }
+
+      // Read the new sample from the input
       if(opts->audio_in_type == 0) {
           result = read (opts->audio_in_fd, &sample, 2);
       }
@@ -87,15 +88,20 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync)
               cleanupAndExit (opts, state);
           }
       }
-     // printf("res: %zd\n, offset: %lld", result, sf_seek(opts->audio_in_file, 0, SEEK_CUR));
+
+#ifdef TRACE_DSD
+      state->debug_sample_index++;
+#endif
+
+      // printf("res: %zd\n, offset: %lld", result, sf_seek(opts->audio_in_file, 0, SEEK_CUR));
       if (opts->use_cosine_filter)
-      {
-        if (state->lastsynctype >= 10 && state->lastsynctype <= 13)
-          sample = dmr_filter(sample);
-        else if (state->lastsynctype == 8 || state->lastsynctype == 9 ||
+        {
+          if (state->lastsynctype >= 10 && state->lastsynctype <= 13)
+              sample = dmr_filter(sample);
+          else if (state->lastsynctype == 8 || state->lastsynctype == 9 ||
                  state->lastsynctype == 16 || state->lastsynctype == 17)
-          sample = nxdn_filter(sample);
-      }
+              sample = nxdn_filter(sample);
+        }
 
       if ((sample > state->max) && (have_sync == 1) && (state->rf_mod == 0))
         {
@@ -190,14 +196,57 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync)
         }
       else
         {
-          if (((i >= state->symbolCenter - 1) && (i <= state->symbolCenter + 2) && (state->rf_mod == 0)) || (((i == state->symbolCenter) || (i == state->symbolCenter + 1)) && (state->rf_mod != 0)))
+          if (state->rf_mod == 0)
             {
-              sum += sample;
-              count++;
+              // 0: C4FM modulation
+
+              if ((i >= state->symbolCenter - 1) && (i <= state->symbolCenter + 2))
+                {
+                  sum += sample;
+                  count++;
+                }
+
+#ifdef TRACE_DSD
+              if (i == state->symbolCenter - 1) {
+                  state->debug_sample_left_edge = state->debug_sample_index - 1;
+              }
+              if (i == state->symbolCenter + 2) {
+                  state->debug_sample_right_edge = state->debug_sample_index - 1;
+              }
+#endif
+            }
+          else
+            {
+              // 1: QPSK modulation
+              // 2: GFSK modulation
+              // Note: this has been changed to use an additional symbol to the left
+              // On the p25_raw_unencrypted.flac it is evident that the timing
+              // comes one sample too late.
+              // This change makes a significant improvement in the BER, at least for
+              // this file.
+              //if ((i == state->symbolCenter) || (i == state->symbolCenter + 1))
+              if ((i == state->symbolCenter - 1) || (i == state->symbolCenter + 1))
+                {
+                  sum += sample;
+                  count++;
+                }
+
+#ifdef TRACE_DSD
+              //if (i == state->symbolCenter) {
+              if (i == state->symbolCenter - 1) {
+                  state->debug_sample_left_edge = state->debug_sample_index - 1;
+              }
+              if (i == state->symbolCenter + 1) {
+                  state->debug_sample_right_edge = state->debug_sample_index - 1;
+              }
+#endif
             }
         }
+
+
       state->lastsample = sample;
-    }
+    }   // for
+
   symbol = (sum / count);
 
   if ((opts->symboltiming == 1) && (have_sync == 0) && (state->lastsynctype != -1))
@@ -212,6 +261,27 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync)
         }
     }
 
+#ifdef TRACE_DSD
+  if (state->samplesPerSymbol == 10) {
+      float left, right;
+      if (state->debug_label_file == NULL) {
+          state->debug_label_file = fopen ("pp_label.txt", "w");
+      }
+      left = state->debug_sample_left_edge / 48000.0;
+      right = state->debug_sample_right_edge / 48000.0;
+      if (state->debug_prefix != '\0') {
+          if (state->debug_prefix == 'I') {
+              fprintf(state->debug_label_file, "%f\t%f\t%c%c %i\n", left, right, state->debug_prefix, state->debug_prefix_2, symbol);
+          } else {
+              fprintf(state->debug_label_file, "%f\t%f\t%c %i\n", left, right, state->debug_prefix, symbol);
+          }
+      } else {
+          fprintf(state->debug_label_file, "%f\t%f\t%i\n", left, right, symbol);
+      }
+  }
+#endif
+
+
   state->symbolcnt++;
   return (symbol);
 }
diff --git a/fcs.h b/fcs.h
index 314ebac..09eae08 100644
--- a/fcs.h
+++ b/fcs.h
@@ -65,12 +65,10 @@ uint16_t calc_fcs (unsigned char * dvstartframe, int size) {
 // radio header (fields flag1, flag2, flag3, destination, departure, companion,
 // own1 and own2)
 uint16_t m_crc;
-
-m_crc=0xFFFF;
-
 int loop;
 unsigned short tmp;
 
+m_crc=0xFFFF;
 
 for (loop=0; loop < size; loop++) {
 	tmp = (m_crc & 0x00ff) ^ dvstartframe[loop];
diff --git a/p25p1_check_hdu.cpp b/p25p1_check_hdu.cpp
index 0a58cf5..cf81289 100644
--- a/p25p1_check_hdu.cpp
+++ b/p25p1_check_hdu.cpp
@@ -3,13 +3,19 @@
 #include "ReedSolomon.hpp"
 #include "Golay24.hpp"
 
-//#define __CHECK_HDU_DEBUG__
+// Uncomment for very verbose trace messages
+//#define CHECK_HDU_DEBUG
 
-int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors)
+// The following methods are just a C bridge for the C++ implementations of the Golay and ReedSolomon
+// algorithms.
+
+static DSDGolay24 golay24;
+
+static DSDReedSolomon_36_20_17 reed_solomon_36_20_17;
+
+int check_and_fix_golay_24_6(char* hex, char* parity, int* fixed_errors)
 {
-    static DSDGolay24 golay24;
-
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_HDU_DEBUG
     printf("[");
     for(unsigned int i=0; i<6; i++) {
         printf("%c", (hex[i] != 0)? 'X': ' ');
@@ -21,9 +27,9 @@ int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors)
     printf("]");
 #endif
 
-    int irrecoverable_errors = golay24.decode(hex, parity, fixed_errors);
+    int irrecoverable_errors = golay24.decode_6(hex, parity, fixed_errors);
 
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_HDU_DEBUG
     printf(" -> [");
     for(unsigned int i=0; i<6; i++) {
         printf("%c", (hex[i] != 0)? 'X': ' ');
@@ -40,11 +46,53 @@ int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors)
     return irrecoverable_errors;
 }
 
+int check_and_fix_golay_24_12(char* dodeca, char* parity, int* fixed_errors)
+{
+#ifdef CHECK_HDU_DEBUG
+    printf("[");
+    for(unsigned int i=0; i<12; i++) {
+        printf("%c", (dodeca[i] != 0)? 'X': ' ');
+    }
+    printf("]  [");
+    for(unsigned int i=12; i<24; i++) {
+        printf("%c", (parity[i-12] != 0)? 'X': ' ');
+    }
+    printf("]");
+#endif
+
+    int irrecoverable_errors = golay24.decode_12(dodeca, parity, fixed_errors);
+
+#ifdef CHECK_HDU_DEBUG
+    printf(" -> [");
+    for(unsigned int i=0; i<12; i++) {
+        printf("%c", (dodeca[i] != 0)? 'X': ' ');
+    }
+    printf("]");
+    if (irrecoverable_errors) {
+        printf("  Errors: >4");
+    } else {
+        printf("  Errors: %i", *fixed_errors);
+    }
+    printf("\n");
+#endif
+
+    return irrecoverable_errors;
+}
+
+void encode_golay_24_6(char* hex, char* out_parity)
+{
+    golay24.encode_6(hex, out_parity);
+}
+
+void encode_golay_24_12(char* dodeca, char* out_parity)
+{
+    golay24.encode_12(dodeca, out_parity);
+}
+
+
 int check_and_fix_redsolomon_36_20_17(char* data, char* parity)
 {
-    static DSDReedSolomon_36_20_17 reed_solomon_36_20_17;
-
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_HDU_DEBUG
     char original[20][6];
     for (int i = 0; i < 20; i++) {
         for (int j = 0; j < 6; j++) {
@@ -55,7 +103,7 @@ int check_and_fix_redsolomon_36_20_17(char* data, char* parity)
 
     int irrecoverable_errors = reed_solomon_36_20_17.decode(data, parity);
 
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_HDU_DEBUG
     printf("Results for Reed-Solomon code (36,20,17)\n\n");
     if (irrecoverable_errors == 0) {
         printf("  i  original fixed\n");
@@ -86,3 +134,9 @@ int check_and_fix_redsolomon_36_20_17(char* data, char* parity)
 
     return irrecoverable_errors;
 }
+
+void encode_reedsolomon_36_20_17(char* hex_data, char* fixed_parity)
+{
+    reed_solomon_36_20_17.encode(hex_data, fixed_parity);
+}
+
diff --git a/p25p1_check_hdu.h b/p25p1_check_hdu.h
index ef74a6e..ff60d88 100644
--- a/p25p1_check_hdu.h
+++ b/p25p1_check_hdu.h
@@ -1,18 +1,42 @@
-#ifndef __P25P1_CHECK_HDU_H__
-#define __P25P1_CHECK_HDU_H__
+
+#ifndef P25P1_CHECK_HDU_H_f5f079faf2d64cf5831e1da1ab83b9ba
+#define P25P1_CHECK_HDU_H_f5f079faf2d64cf5831e1da1ab83b9ba
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 /**
- * Attempts to correct an hex (6 bit) word using the Golay(24,12,8) FEC.
- * \param hex The 6 bit word to error correct, given as a char array of 6 elements, each one bit.
- * \param parity A 12 bit word with the Golay parity data, given as a char array of 12 elements.
+ * Attempts to correct an hex (6-bit) word using the Golay(24,12,8) FEC.
+ * \param hex The 6-bit word to error correct, given as a char array of 6 elements, one bit each.
+ * \param parity A 12-bit word with the Golay parity data, given as a char array of 12 elements.
  * \param fixed_error Output. Filled with the number of errors fixed in the input.
  * \return 1 if there were uncorrectable errors in the input, 0 otherwise.
  */
-int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors);
+int check_and_fix_golay_24_6(char* hex, char* parity, int* fixed_errors);
+
+/**
+ * Attempts to correct a 12-bit word using the Golay(24,12,8) FEC.
+ * \param dodeca The 12-bit word to error correct, given as a char array of 12 elements, one bit each.
+ * \param parity A 12-bit word with the Golay parity data, given as a char array of 12 elements.
+ * \param fixed_error Output. Filled with the number of errors fixed in the input.
+ * \return 1 if there were uncorrectable errors in the input, 0 otherwise.
+ */
+int check_and_fix_golay_24_12(char* dodeca, char* parity, int* fixed_errors);
+
+/**
+ * Encodes an hex word using the Golay(24,12,8) FEC.
+ * \param hex A 6-bit word to encode.
+ * \param out_parity The address to be filled with the calculated Golay parity.
+ */
+void encode_golay_24_6(char* hex, char* out_parity);
+
+/**
+ * Encodes an 12-bitword using the Golay(24,12,8) FEC.
+ * \param dodeca A 12-bit word to encode.
+ * \param out_parity The address to be filled with the calculated Golay parity.
+ */
+void encode_golay_24_12(char* dodeca, char* out_parity);
 
 /**
  * Attempts to correct 20 hex words using the Reed-Solomon(36,20,17) FEC.
@@ -22,8 +46,15 @@ int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors);
  */
 int check_and_fix_redsolomon_36_20_17(char* data, char* parity);
 
+/**
+ * Calculates the Reed-Solomon parity of 20 words of 6 bits each.
+ * \param hex_data The address with the words whose parity we want to calculate.
+ * \param fixed_parity The address to be filled with the calculated parity. 16 words of 6 bits are produced.
+ */
+void encode_reedsolomon_36_20_17(char* hex_data, char* fixed_parity);
+
 #ifdef __cplusplus
 }
 #endif
 
-#endif // __P25P1_CHECK_HDU_H__
+#endif // P25P1_CHECK_HDU_H_f5f079faf2d64cf5831e1da1ab83b9ba
diff --git a/p25p1_check_ldu.cpp b/p25p1_check_ldu.cpp
index 82ec0ee..15efdd3 100644
--- a/p25p1_check_ldu.cpp
+++ b/p25p1_check_ldu.cpp
@@ -4,22 +4,29 @@
 #include "Hamming.hpp"
 #include "ReedSolomon.hpp"
 
+// Uncomment for very verbose trace messages
+//#define CHECK_LDU_DEBUG
 
-//#define __CHECK_HDU_DEBUG__
+// The following methods are just a C bridge for the C++ implementations of the Golay and ReedSolomon
+// algorithms.
 
+static Hamming_10_6_3_TableImpl hamming;
+static DSDReedSolomon_24_12_13 reed_solomon_24_12_13;
+static DSDReedSolomon_24_16_9 reed_solomon_24_16_9;
 
-int check_and_fix_hamming_10_12_13(char* hex, char* parity)
+int check_and_fix_hamming_10_6_3(char* hex, char* parity)
 {
-    static Hamming_10_6_3_TableImpl hamming;
-
     return hamming.decode(hex, parity);
 }
 
-int check_and_fix_redsolomon_24_12_13(char* data, char* parity)
+void encode_hamming_10_6_3(char* hex, char* out_parity)
 {
-    static DSDReedSolomon_24_12_13 reed_solomon_24_12_13;
+    hamming.encode(hex, out_parity);
+}
 
-#ifdef __CHECK_HDU_DEBUG__
+int check_and_fix_reedsolomon_24_12_13(char* data, char* parity)
+{
+#ifdef CHECK_LDU_DEBUG
     char original[12][6];
     for (int i = 0; i < 12; i++) {
         for (int j = 0; j < 6; j++) {
@@ -30,7 +37,7 @@ int check_and_fix_redsolomon_24_12_13(char* data, char* parity)
 
     int irrecoverable_error = reed_solomon_24_12_13.decode(data, parity);
 
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_LDU_DEBUG
     printf("Results for Reed-Solomon code (24,12,13)\n\n");
     if (irrecoverable_error == 0) {
         printf("  i  original fixed\n");
@@ -62,11 +69,14 @@ int check_and_fix_redsolomon_24_12_13(char* data, char* parity)
     return irrecoverable_error;
 }
 
-int check_and_fix_redsolomon_24_16_9(char* data, char* parity)
+void encode_reedsolomon_24_12_13(char* hex_data, char* fixed_parity)
 {
-    static DSDReedSolomon_24_16_9 reed_solomon_24_16_9;
+    reed_solomon_24_12_13.encode(hex_data, fixed_parity);
+}
 
-#ifdef __CHECK_HDU_DEBUG__
+int check_and_fix_reedsolomon_24_16_9(char* data, char* parity)
+{
+#ifdef CHECK_LDU_DEBUG
     char original[16][6];
     for (int i = 0; i < 16; i++) {
         for (int j = 0; j < 6; j++) {
@@ -77,7 +87,7 @@ int check_and_fix_redsolomon_24_16_9(char* data, char* parity)
 
     int irrecoverable_error = reed_solomon_24_16_9.decode(data, parity);
 
-#ifdef __CHECK_HDU_DEBUG__
+#ifdef CHECK_LDU_DEBUG
     printf("Results for Reed-Solomon code (24,16,9)\n\n");
     if (irrecoverable_error == 0) {
         printf("  i  original fixed\n");
@@ -108,3 +118,8 @@ int check_and_fix_redsolomon_24_16_9(char* data, char* parity)
 
     return irrecoverable_error;
 }
+
+void encode_reedsolomon_24_16_9(char* hex_data, char* fixed_parity)
+{
+    reed_solomon_24_16_9.encode(hex_data, fixed_parity);
+}
diff --git a/p25p1_check_ldu.h b/p25p1_check_ldu.h
index 7cc22bf..aac1220 100644
--- a/p25p1_check_ldu.h
+++ b/p25p1_check_ldu.h
@@ -1,11 +1,25 @@
-#ifndef __P25P1_CHECK_LDU_H__
-#define __P25P1_CHECK_LDU_H__
+
+#ifndef P25P1_CHECK_LDU_H_c1734445a67e47caa25673d7a4ce7520
+#define P25P1_CHECK_LDU_H_c1734445a67e47caa25673d7a4ce7520
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-int check_and_fix_hamming_10_12_13(char* hex, char* parity);
+/**
+ * Attempts to error correct a hex (6-bit) word using the Hamming(10,6,3) FEC.
+ * \param hex The 6-bit word to error correct.
+ * \param parity The parity to use in the error correction. 4 bits.
+ * \return The estimated error count. It might be erroneous.
+ */
+int check_and_fix_hamming_10_6_3(char* hex, char* parity);
+
+/**
+ * Encodes an hex word with the Hamming(10,6,3) FEC.
+ * \param hex The 6-bit word to error correct.
+ * \param out_parity The address where to store the calculated parity. 4 bytes are used, one for each bit.
+ */
+void encode_hamming_10_6_3(char* hex, char* out_parity);
 
 /**
  * Attempts to correct 12 hex words using the Reed-Solomon(24,12,13) FEC.
@@ -13,18 +27,32 @@ int check_and_fix_hamming_10_12_13(char* hex, char* parity);
  * \param parity The corresponding 12 hex words with the parity information.
  * \return 1 if irrecoverable errors have been detected, 0 otherwise.
  */
-int check_and_fix_redsolomon_24_12_13(char* data, char* parity);
+int check_and_fix_reedsolomon_24_12_13(char* data, char* parity);
+
+/**
+ * Encodes 12 hex words with the  Reed-Solomon(24,12,13) FEC.
+ * \param hex_data The packed 12 hex words to encode.
+ * \fixed_parity The address were to store the 12 hex words with the Reed-Solomon parity.
+ */
+void encode_reedsolomon_24_12_13(char* hex_data, char* fixed_parity);
 
 /**
  * Attempts to correct 16 hex words using the Reed-Solomon(24,16,9) FEC.
- * \param data The packed hex words, each of 6 chars, one after the other.
+ * \param data The packed 16 hex words, each of 6 chars, one after the other.
  * \param parity The corresponding 8 hex words with the parity information.
  * \return 1 if irrecoverable errors have been detected, 0 otherwise.
  */
-int check_and_fix_redsolomon_24_16_9(char* data, char* parity);
+int check_and_fix_reedsolomon_24_16_9(char* data, char* parity);
+
+/**
+ * Encodes 12 hex words with the  Reed-Solomon(24,16,9) FEC.
+ * \param hex_data The packed 16 hex words to encode.
+ * \fixed_parity The address were to store the 8 hex words with the Reed-Solomon parity.
+ */
+void encode_reedsolomon_24_16_9(char* hex_data, char* fixed_parity);
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif // __P25P1_CHECK_LDU_H__
+#endif // P25P1_CHECK_LDU_H_c1734445a67e47caa25673d7a4ce7520
diff --git a/p25p1_check_nid.cpp b/p25p1_check_nid.cpp
index 9a4e305..668471d 100644
--- a/p25p1_check_nid.cpp
+++ b/p25p1_check_nid.cpp
@@ -13,7 +13,8 @@ using namespace itpp;
 
 itpp::BCH bch(63, 16, 11, "6 3 3 1 1 4 1 3 6 7 2 3 5 4 5 3", true);
 
-/** Convenience class to calculate the parity of the DUID values. Keeps a table with the expected outcomes
+/**
+ * Convenience class to calculate the parity of the DUID values. Keeps a table with the expected outcomes
  * for fast lookup.
  */
 class ParityTable
diff --git a/p25p1_check_nid.h b/p25p1_check_nid.h
index 99b8f66..28e83e0 100644
--- a/p25p1_check_nid.h
+++ b/p25p1_check_nid.h
@@ -1,13 +1,15 @@
-#ifndef __P25P1_CHECK_NID_H__
-#define __P25P1_CHECK_NID_H__
+
+#ifndef P25P1_CHECK_NID_H_3af071e917ea43fdb51326e2cbfbde0a
+#define P25P1_CHECK_NID_H_3af071e917ea43fdb51326e2cbfbde0a
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-/** Checks a NID value, returns the fixed NAC, DUID and also an indication if it failed to decode the NID.
- *
- *  \param bch_code Input. An array to the 63 bytes, each containing one bit of the NID.
+/**
+ * Checks a NID value, returns the fixed NAC, DUID and also an indication if it failed to decode the NID.
+ *  \param bch_code Input. An array to the 63 bytes, each containing one bit of the NID. This includes the
+ *  NAC (12 bits), DUID (4 bits) and 47 bits of BCH parity.
  *  \param new_nac Output. An address where to store the calculated NAC value after error correction. Should
  *                 be large enough to accommodate for an integer.
  *  \param new_duid Output. An address where to store the calculated DUID value after error correction. Should
@@ -21,4 +23,4 @@ int check_NID(char* bch_code, int* new_nac, char* new_duid, unsigned char parity
 }
 #endif
 
-#endif // __P25P1_CHECK_NID_H__
+#endif // P25P1_CHECK_NID_H_3af071e917ea43fdb51326e2cbfbde0a
diff --git a/p25p1_hdu.c b/p25p1_hdu.c
index a9039c0..bf0135e 100644
--- a/p25p1_hdu.c
+++ b/p25p1_hdu.c
@@ -17,72 +17,108 @@
 
 #include "dsd.h"
 
-
+#include "p25p1_hdu.h"
 #include "p25p1_check_hdu.h"
+#include "p25p1_heuristics.h"
 
-/**
- * Read two bits from the stream of data. Takes into account the periodic occurrence of status bits.
- * Those are discarded.
- */
-void
-read_dibit (dsd_opts* opts, dsd_state* state, char* output, int* status_count)
+int
+read_dibit (dsd_opts* opts, dsd_state* state, char* output, int* status_count, int* analog_signal, int* did_read_status)
 {
     int dibit;
     int status;
 
     if (*status_count == 35) {
+
+#ifdef TRACE_DSD
+        char prev_prefix = state->debug_prefix;
+        state->debug_prefix = 's';
+#endif
+
         // Status bits now
         status = getDibit (opts, state);
         // TODO: do something useful with the status bits...
+        if (did_read_status != NULL) {
+            *did_read_status = 1;
+        }
         *status_count = 1;
+
+#ifdef TRACE_DSD
+        state->debug_prefix = prev_prefix;
+#endif
+
     } else {
+        if (did_read_status != NULL) {
+            *did_read_status = 0;
+        }
         (*status_count)++;
     }
 
-    dibit = getDibit (opts, state);
+    dibit = get_dibit_and_analog_signal (opts, state, analog_signal);
     output[0] = (1 & (dibit >> 1));      // bit 1
     output[1] = (1 & dibit);             // bit 0
+
+    return dibit;
 }
 
-/**
- * Read an hex word from the stream of data.
- */
 void
-read_hex (dsd_opts* opts, dsd_state* state, char* hex, int* status_count)
+read_dibit_update_analog_data (dsd_opts* opts, dsd_state* state, char* output, unsigned int count, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
 {
   unsigned int i;
+  unsigned int debug_left, debug_right;
 
-  // Read 3 times a dibit = read 6 bits.
-  for (i=0; i<=4; i+=2)
+  for (i=0; i<count; i+=2)
     {
       // We read two bits on each call
-      read_dibit(opts, state, hex + i, status_count);
+      int analog_signal;
+      int did_read_status;
+      int dibit;
+
+      dibit = read_dibit(opts, state, output + i, status_count, &analog_signal, &did_read_status);
+
+      if (analog_signal_array != NULL)
+        {
+          // Fill up the AnalogSignal struct
+          analog_signal_array[*analog_signal_index].value = analog_signal;
+          analog_signal_array[*analog_signal_index].dibit = dibit;
+          analog_signal_array[*analog_signal_index].sequence_broken = did_read_status;
+          (*analog_signal_index)++;
+        }
     }
 }
 
+void
+read_word (dsd_opts* opts, dsd_state* state, char* word, unsigned int length, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
+{
+  read_dibit_update_analog_data(opts, state, word, length, status_count, analog_signal_array, analog_signal_index);
+}
+
+void
+read_golay24_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
+{
+  read_dibit_update_analog_data(opts, state, parity, 12, status_count, analog_signal_array, analog_signal_index);
+}
+
+void
+read_hamm_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
+{
+  // Read 2 dibits = read 4 bits.
+  read_dibit_update_analog_data(opts, state, parity, 4, status_count, analog_signal_array, analog_signal_index);
+}
+
 /**
- * Read 12 bits from the stream of data. Those are the parity bits that follow each hex word.
+ * Corrects a hex (6 bit) word  using the Golay 24 FEC.
  */
-static void
-read_golay24_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count)
-{
-  unsigned int i;
-
-  // Read 6 times a dibit = read 12 bits.
-  for (i=0; i<=10; i+=2)
-    {
-      // We read two bits on each call
-      read_dibit(opts, state, parity + i, status_count);
-    }
-}
-
 static void
 correct_hex_word (dsd_state* state, char* hex, char* parity)
 {
   int fixed_errors;
   int irrecoverable_errors;
 
-  irrecoverable_errors = check_and_fix_golay24(hex, parity, &fixed_errors);
+  irrecoverable_errors = check_and_fix_golay_24_6(hex, parity, &fixed_errors);
 
   state->debug_header_errors += fixed_errors;
   if (irrecoverable_errors != 0)
@@ -92,30 +128,98 @@ correct_hex_word (dsd_state* state, char* hex, char* parity)
 }
 
 /**
- * Read an hex word, its parity bits and attempts to error correct it using the Golay24 algorithm.
+ * Reads an hex word, its parity bits and attempts to error correct it using the Golay24 algorithm.
  */
 static void
-read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count)
+read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
 {
   char parity[12];
 
   // Read the hex word
-  read_hex (opts, state, hex, status_count);
+  read_word (opts, state, hex, 6, status_count, analog_signal_array, analog_signal_index);
   // Read the parity
-  read_golay24_parity (opts, state, parity, status_count);
+  read_golay24_parity (opts, state, parity, status_count, analog_signal_array, analog_signal_index);
   // Use the Golay24 FEC to correct it. This call modifies the content of hex to fix it, hopefully
   correct_hex_word (state, hex, parity);
 }
 
+/**
+ * Uses the information from a corrected sequence of hex words to update the AnalogSignal data.
+ * The proper Golay 24 parity is calculated from the corrected hex word so we can also fix the Golay parity
+ * that we read originally from the signal.
+ * \param corrected_hex_data Pointer to a sequence of hex words that has been error corrected and therefore
+ * we trust it's correct. Typically this are hex words that has been decoded successfully using a
+ * Reed-Solomon variant.
+ * \param hex_count The number of hex words in the sequence.
+ * \param analog_signal_array A pointer to the AnalogSignal information for the sequence of hex words.
+ */
+static void
+correct_golay_dibits_6(char* corrected_hex_data, int hex_count, AnalogSignal* analog_signal_array)
+{
+  int i, j;
+  int analog_signal_index;
+  int dibit;
+  char parity[12];
+
+
+  analog_signal_index = 0;
+
+  for (i=hex_count-1; i>=0; i--)
+    {
+      for (j=0; j<6; j+=2)  // 3 iterations -> 3 dibits
+        {
+          // Given the bits, calculates the dibit
+          dibit = (corrected_hex_data[i*6+j] << 1) | corrected_hex_data[i*6+j+1];
+          // Now we know the dibit we should have read from the signal
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("HDU data word corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+
+      // Calculate the Golay 24 parity for the corrected hex word
+      encode_golay_24_6(corrected_hex_data+i*6, parity);
+
+      // Now we know the parity we should have read from the signal. Use this information
+      for (j=0; j<12; j+=2) // 6 iterations -> 6 dibits
+        {
+          // Given the bits, calculates the dibit
+          dibit = (parity[j] << 1) | parity[j+1];
+          // Now we know the dibit we should have read from the signal
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("HDU parity corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+    }
+}
+
+/**
+ * The important method that processes a full P25 HD unit.
+ */
 void
-processHDU (dsd_opts * opts, dsd_state * state)
+processHDU(dsd_opts* opts, dsd_state* state)
 {
   char mi[73], mfid[9], algid[9], kid[17], tgid[17], tmpstr[255];
-  int dibit, i, j;
+  int i, j;
   long talkgroup;
   int algidhex, kidhex;
   char hex[6];
-
   int status_count;
   int status;
 
@@ -124,12 +228,20 @@ processHDU (dsd_opts * opts, dsd_state * state)
 
   int irrecoverable_errors;
 
-  status_count = 35 - 14;
+  AnalogSignal analog_signal_array[20*(3+6)+16*(3+6)];
+  int analog_signal_index;
 
-  // Read 20 hex words, correct them using their Golay24 parity data.
+  analog_signal_index = 0;
+
+  // we skip the status dibits that occur every 36 symbols
+  // the next status symbol comes in 14 dibits from here
+  // so we start counter at 36-14-1 = 21
+  status_count = 21;
+
+  // Read 20 hex words, correct them using their Golay 24 parity data.
   for (i=19; i>=0; i--)
     {
-      read_and_correct_hex_word (opts, state, hex, &status_count);
+      read_and_correct_hex_word (opts, state, hex, &status_count, analog_signal_array, &analog_signal_index);
       // Store the corrected hex word into the hex_data store:
       for (j=0; j<6; j++)
         {
@@ -137,24 +249,57 @@ processHDU (dsd_opts * opts, dsd_state * state)
         }
     }
 
-
   // Read the 16 parity hex word. These are used to FEC the 20 hex words using Reed-Solomon.
   for (i=15; i>=0; i--)
     {
-      read_and_correct_hex_word (opts, state, hex, &status_count);
+      read_and_correct_hex_word (opts, state, hex, &status_count, analog_signal_array, &analog_signal_index);
       // Store the corrected hex word into the hex_parity store:
       for (j=0; j<6; j++)
         {
           hex_parity[i][j] = hex[j];
         }
     }
+  // Don't forget to mark the first element as the start of a new sequence
+  analog_signal_array[0].sequence_broken = 1;
 
   // Use the Reed-Solomon algorithm to correct the data. hex_data is modified in place
   irrecoverable_errors = check_and_fix_redsolomon_36_20_17((char*)hex_data, (char*)hex_parity);
   if (irrecoverable_errors != 0)
     {
+      // The hex words failed the Reed-Solomon check. There were too many errors. Still we can use this
+      // information to update an estimate of the BER.
       state->debug_header_critical_errors++;
+
+      // We can correct (17-1)/2 = 8 errors. If we failed, it means that there were more than 8 errors in
+      // these 20+16 words. But take into account that each hex word was already error corrected with
+      // Golay 24, which can correct 3 bits on each sequence of (6+12) bits. We could say that there were
+      // 9 errors of 4 bits.
+      update_error_stats(&state->p25_heuristics, 20*6+16*6, 9*4);
     }
+  else
+    {
+      // The hex words passed the Reed-Solomon check. This means that very likely they are correct and we
+      // can trust that the digitizer did a good job with them. In other words, each analog value was
+      // correctly assigned to a dibit. This is extremely useful information for the digitizer and we are
+      // going to exploit it.
+      char fixed_parity[16*6];
+
+      // Correct the dibits that we did read according with the newly corrected hex_data values
+      correct_golay_dibits_6((char*)hex_data, 20, analog_signal_array);
+
+      // Generate again the Reed-Solomon parity for the corrected data
+      encode_reedsolomon_36_20_17((char*)hex_data, fixed_parity);
+
+      // Correct the dibits that we read according with the corrected parity values
+      correct_golay_dibits_6(fixed_parity, 16, analog_signal_array+20*(3+6));
+
+      // Now we have a bunch of dibits (composed of data and parity of different kinds) that we trust are all
+      // correct. We also keep a record of the analog values from where each dibit is coming from.
+      // This information is gold for the heuristics module.
+      contribute_to_heuristics(state->rf_mod, &(state->p25_heuristics), analog_signal_array, 20*(3+6)+16*(3+6));
+    }
+
+  // Now put the corrected data on the DSD structures
 
   mi[72] = 0;
   mfid[8] = 0;
@@ -162,147 +307,145 @@ processHDU (dsd_opts * opts, dsd_state * state)
   kid[16] = 0;
   tgid[16] = 0;
 
-  // Now put the corrected data on the DSD structures
+  mi[ 0]   = hex_data[19][0] + '0';
+  mi[ 1]   = hex_data[19][1] + '0';
+  mi[ 2]   = hex_data[19][2] + '0';
+  mi[ 3]   = hex_data[19][3] + '0';
+  mi[ 4]   = hex_data[19][4] + '0';
+  mi[ 5]   = hex_data[19][5] + '0';
 
-  mi[ 0] = hex_data[19][0] + '0';
-  mi[ 1] = hex_data[19][1] + '0';
-  mi[ 2] = hex_data[19][2] + '0';
-  mi[ 3] = hex_data[19][3] + '0';
-  mi[ 4] = hex_data[19][4] + '0';
-  mi[ 5] = hex_data[19][5] + '0';
+  mi[ 6]   = hex_data[18][0] + '0';
+  mi[ 7]   = hex_data[18][1] + '0';
+  mi[ 8]   = hex_data[18][2] + '0';
+  mi[ 9]   = hex_data[18][3] + '0';
+  mi[10]   = hex_data[18][4] + '0';
+  mi[11]   = hex_data[18][5] + '0';
 
-  mi[ 6] = hex_data[18][0] + '0';
-  mi[ 7] = hex_data[18][1] + '0';
-  mi[ 8] = hex_data[18][2] + '0';
-  mi[ 9] = hex_data[18][3] + '0';
-  mi[10] = hex_data[18][4] + '0';
-  mi[11] = hex_data[18][5] + '0';
+  mi[12]   = hex_data[17][0] + '0';
+  mi[13]   = hex_data[17][1] + '0';
+  mi[14]   = hex_data[17][2] + '0';
+  mi[15]   = hex_data[17][3] + '0';
+  mi[16]   = hex_data[17][4] + '0';
+  mi[17]   = hex_data[17][5] + '0';
 
-  mi[12] = hex_data[17][0] + '0';
-  mi[13] = hex_data[17][1] + '0';
-  mi[14] = hex_data[17][2] + '0';
-  mi[15] = hex_data[17][3] + '0';
-  mi[16] = hex_data[17][4] + '0';
-  mi[17] = hex_data[17][5] + '0';
+  mi[18]   = hex_data[16][0] + '0';
+  mi[19]   = hex_data[16][1] + '0';
+  mi[20]   = hex_data[16][2] + '0';
+  mi[21]   = hex_data[16][3] + '0';
+  mi[22]   = hex_data[16][4] + '0';
+  mi[23]   = hex_data[16][5] + '0';
 
-  mi[18] = hex_data[16][0] + '0';
-  mi[19] = hex_data[16][1] + '0';
-  mi[20] = hex_data[16][2] + '0';
-  mi[21] = hex_data[16][3] + '0';
-  mi[22] = hex_data[16][4] + '0';
-  mi[23] = hex_data[16][5] + '0';
+  mi[24]   = hex_data[15][0] + '0';
+  mi[25]   = hex_data[15][1] + '0';
+  mi[26]   = hex_data[15][2] + '0';
+  mi[27]   = hex_data[15][3] + '0';
+  mi[28]   = hex_data[15][4] + '0';
+  mi[29]   = hex_data[15][5] + '0';
 
-  mi[24] = hex_data[15][0] + '0';
-  mi[25] = hex_data[15][1] + '0';
-  mi[26] = hex_data[15][2] + '0';
-  mi[27] = hex_data[15][3] + '0';
-  mi[28] = hex_data[15][4] + '0';
-  mi[29] = hex_data[15][5] + '0';
+  mi[30]   = hex_data[14][0] + '0';
+  mi[31]   = hex_data[14][1] + '0';
+  mi[32]   = hex_data[14][2] + '0';
+  mi[33]   = hex_data[14][3] + '0';
+  mi[34]   = hex_data[14][4] + '0';
+  mi[35]   = hex_data[14][5] + '0';
 
-  mi[30] = hex_data[14][0] + '0';
-  mi[31] = hex_data[14][1] + '0';
-  mi[32] = hex_data[14][2] + '0';
-  mi[33] = hex_data[14][3] + '0';
-  mi[34] = hex_data[14][4] + '0';
-  mi[35] = hex_data[14][5] + '0';
+  mi[36]   = hex_data[13][0] + '0';
+  mi[37]   = hex_data[13][1] + '0';
+  mi[38]   = hex_data[13][2] + '0';
+  mi[39]   = hex_data[13][3] + '0';
+  mi[40]   = hex_data[13][4] + '0';
+  mi[41]   = hex_data[13][5] + '0';
 
-  mi[36] = hex_data[13][0] + '0';
-  mi[37] = hex_data[13][1] + '0';
-  mi[38] = hex_data[13][2] + '0';
-  mi[39] = hex_data[13][3] + '0';
-  mi[40] = hex_data[13][4] + '0';
-  mi[41] = hex_data[13][5] + '0';
+  mi[42]   = hex_data[12][0] + '0';
+  mi[43]   = hex_data[12][1] + '0';
+  mi[44]   = hex_data[12][2] + '0';
+  mi[45]   = hex_data[12][3] + '0';
+  mi[46]   = hex_data[12][4] + '0';
+  mi[47]   = hex_data[12][5] + '0';
 
-  mi[42] = hex_data[12][0] + '0';
-  mi[43] = hex_data[12][1] + '0';
-  mi[44] = hex_data[12][2] + '0';
-  mi[45] = hex_data[12][3] + '0';
-  mi[46] = hex_data[12][4] + '0';
-  mi[47] = hex_data[12][5] + '0';
+  mi[48]   = hex_data[11][0] + '0';
+  mi[49]   = hex_data[11][1] + '0';
+  mi[50]   = hex_data[11][2] + '0';
+  mi[51]   = hex_data[11][3] + '0';
+  mi[52]   = hex_data[11][4] + '0';
+  mi[53]   = hex_data[11][5] + '0';
 
-  mi[48] = hex_data[11][0] + '0';
-  mi[49] = hex_data[11][1] + '0';
-  mi[50] = hex_data[11][2] + '0';
-  mi[51] = hex_data[11][3] + '0';
-  mi[52] = hex_data[11][4] + '0';
-  mi[53] = hex_data[11][5] + '0';
+  mi[54]   = hex_data[10][0] + '0';
+  mi[55]   = hex_data[10][1] + '0';
+  mi[56]   = hex_data[10][2] + '0';
+  mi[57]   = hex_data[10][3] + '0';
+  mi[58]   = hex_data[10][4] + '0';
+  mi[59]   = hex_data[10][5] + '0';
 
-  mi[54] = hex_data[10][0] + '0';
-  mi[55] = hex_data[10][1] + '0';
-  mi[56] = hex_data[10][2] + '0';
-  mi[57] = hex_data[10][3] + '0';
-  mi[58] = hex_data[10][4] + '0';
-  mi[59] = hex_data[10][5] + '0';
+  mi[60]   = hex_data[ 9][0] + '0';
+  mi[61]   = hex_data[ 9][1] + '0';
+  mi[62]   = hex_data[ 9][2] + '0';
+  mi[63]   = hex_data[ 9][3] + '0';
+  mi[64]   = hex_data[ 9][4] + '0';
+  mi[65]   = hex_data[ 9][5] + '0';
 
-  mi[60] = hex_data[9][0] + '0';
-  mi[61] = hex_data[9][1] + '0';
-  mi[62] = hex_data[9][2] + '0';
-  mi[63] = hex_data[9][3] + '0';
-  mi[64] = hex_data[9][4] + '0';
-  mi[65] = hex_data[9][5] + '0';
+  mi[66]   = hex_data[ 8][0] + '0';
+  mi[67]   = hex_data[ 8][1] + '0';
+  mi[68]   = hex_data[ 8][2] + '0';
+  mi[69]   = hex_data[ 8][3] + '0';
+  mi[70]   = hex_data[ 8][4] + '0';
+  mi[71]   = hex_data[ 8][5] + '0';
 
-  mi[66] = hex_data[8][0] + '0';
-  mi[67] = hex_data[8][1] + '0';
-  mi[68] = hex_data[8][2] + '0';
-  mi[69] = hex_data[8][3] + '0';
-  mi[70] = hex_data[8][4] + '0';
-  mi[71] = hex_data[8][5] + '0';
+  mfid[0]  = hex_data[ 7][0] + '0';
+  mfid[1]  = hex_data[ 7][1] + '0';
+  mfid[2]  = hex_data[ 7][2] + '0';
+  mfid[3]  = hex_data[ 7][3] + '0';
+  mfid[4]  = hex_data[ 7][4] + '0';
+  mfid[5]  = hex_data[ 7][5] + '0';
 
-  mfid[0]  = hex_data[7][0] + '0';
-  mfid[1]  = hex_data[7][1] + '0';
-  mfid[2]  = hex_data[7][2] + '0';
-  mfid[3]  = hex_data[7][3] + '0';
-  mfid[4]  = hex_data[7][4] + '0';
-  mfid[5]  = hex_data[7][5] + '0';
+  mfid[6]  = hex_data[ 6][0] + '0';
+  mfid[7]  = hex_data[ 6][1] + '0';
+  algid[0] = hex_data[ 6][2] + '0';  // The important algorithm ID. This indicates whether the data is
+  algid[1] = hex_data[ 6][3] + '0';  // encrypted and if so what is the encryption algorithm used.
+  algid[2] = hex_data[ 6][4] + '0';  // A code 0x80 here means that the data is unencrypted.
+  algid[3] = hex_data[ 6][5] + '0';
 
-  mfid[6]  = hex_data[6][0] + '0';
-  mfid[7]  = hex_data[6][1] + '0';
-  algid[0] = hex_data[6][2] + '0';  // The important algorithm ID. This indicates whether the data is
-  algid[1] = hex_data[6][3] + '0';  // encrypted and if so what is the encryption algorithm used.
-  algid[2] = hex_data[6][4] + '0';  // A code 0x80 here means that the data is unencrypted.
-  algid[3] = hex_data[6][5] + '0';
+  algid[4] = hex_data[ 5][0] + '0';
+  algid[5] = hex_data[ 5][1] + '0';
+  algid[6] = hex_data[ 5][2] + '0';
+  algid[7] = hex_data[ 5][3] + '0';
+  kid[ 0]  = hex_data[ 5][4] + '0';
+  kid[ 1]  = hex_data[ 5][5] + '0';
 
-  algid[4] = hex_data[5][0] + '0';
-  algid[5] = hex_data[5][1] + '0';
-  algid[6] = hex_data[5][2] + '0';
-  algid[7] = hex_data[5][3] + '0';
-  kid[ 0]  = hex_data[5][4] + '0';
-  kid[ 1]  = hex_data[5][5] + '0';
+  kid[ 2]  = hex_data[ 4][0] + '0';  // The encryption key ID
+  kid[ 3]  = hex_data[ 4][1] + '0';
+  kid[ 4]  = hex_data[ 4][2] + '0';
+  kid[ 5]  = hex_data[ 4][3] + '0';
+  kid[ 6]  = hex_data[ 4][4] + '0';
+  kid[ 7]  = hex_data[ 4][5] + '0';
 
-  kid[ 2]  = hex_data[4][0] + '0';  // The encryption key ID
-  kid[ 3]  = hex_data[4][1] + '0';
-  kid[ 4]  = hex_data[4][2] + '0';
-  kid[ 5]  = hex_data[4][3] + '0';
-  kid[ 6]  = hex_data[4][4] + '0';
-  kid[ 7]  = hex_data[4][5] + '0';
+  kid[ 8]  = hex_data[ 3][0] + '0';
+  kid[ 9]  = hex_data[ 3][1] + '0';
+  kid[10]  = hex_data[ 3][2] + '0';
+  kid[11]  = hex_data[ 3][3] + '0';
+  kid[12]  = hex_data[ 3][4] + '0';
+  kid[13]  = hex_data[ 3][5] + '0';
 
-  kid[ 8]  = hex_data[3][0] + '0';
-  kid[ 9]  = hex_data[3][1] + '0';
-  kid[10]  = hex_data[3][2] + '0';
-  kid[11]  = hex_data[3][3] + '0';
-  kid[12]  = hex_data[3][4] + '0';
-  kid[13]  = hex_data[3][5] + '0';
+  kid[14]  = hex_data[ 2][0] + '0';
+  kid[15]  = hex_data[ 2][1] + '0';
+  tgid[ 0] = hex_data[ 2][2] + '0';  // Talk group ID
+  tgid[ 1] = hex_data[ 2][3] + '0';
+  tgid[ 2] = hex_data[ 2][4] + '0';
+  tgid[ 3] = hex_data[ 2][5] + '0';
 
-  kid[14]  = hex_data[2][0] + '0';
-  kid[15]  = hex_data[2][1] + '0';
-  tgid[ 0] = hex_data[2][2] + '0';  // Talk group ID
-  tgid[ 1] = hex_data[2][3] + '0';
-  tgid[ 2] = hex_data[2][4] + '0';
-  tgid[ 3] = hex_data[2][5] + '0';
+  tgid[ 4] = hex_data[ 1][0] + '0';
+  tgid[ 5] = hex_data[ 1][1] + '0';
+  tgid[ 6] = hex_data[ 1][2] + '0';
+  tgid[ 7] = hex_data[ 1][3] + '0';
+  tgid[ 8] = hex_data[ 1][4] + '0';
+  tgid[ 9] = hex_data[ 1][5] + '0';
 
-  tgid[ 4] = hex_data[1][0] + '0';
-  tgid[ 5] = hex_data[1][1] + '0';
-  tgid[ 6] = hex_data[1][2] + '0';
-  tgid[ 7] = hex_data[1][3] + '0';
-  tgid[ 8] = hex_data[1][4] + '0';
-  tgid[ 9] = hex_data[1][5] + '0';
-
-  tgid[10] = hex_data[0][0] + '0';
-  tgid[11] = hex_data[0][1] + '0';
-  tgid[12] = hex_data[0][2] + '0';
-  tgid[13] = hex_data[0][3] + '0';
-  tgid[14] = hex_data[0][4] + '0';
-  tgid[15] = hex_data[0][5] + '0';
+  tgid[10] = hex_data[ 0][0] + '0';
+  tgid[11] = hex_data[ 0][1] + '0';
+  tgid[12] = hex_data[ 0][2] + '0';
+  tgid[13] = hex_data[ 0][3] + '0';
+  tgid[14] = hex_data[ 0][4] + '0';
+  tgid[15] = hex_data[ 0][5] + '0';
 
   state->p25kid = strtol(kid, NULL, 2);
 
diff --git a/p25p1_hdu.h b/p25p1_hdu.h
index b5ee4d3..e2be95e 100644
--- a/p25p1_hdu.h
+++ b/p25p1_hdu.h
@@ -1,15 +1,73 @@
 
-#ifndef __P25P1_HDU_H__
-#define __P25P1_HDU_H__
+#ifndef P25P1_HDU_H_9f95c3a5072842e8aaf94444e1452d20
+#define P25P1_HDU_H_9f95c3a5072842e8aaf94444e1452d20
+
+
+#include "p25p1_heuristics.h"
 
 /**
- * Reads a dibit keeping track of the status dibits that shows up every 70 bits
+ * Reads a dibit (two bits) from the stream of data. Takes into account the periodic occurrence of status
+ * bits that show up every 36 dibits. Status bits are discarded.
+ * \param opts A pointer the the DSD options.
+ * \param state A pointer the the DSD state structure.
+ * \param output A pointer to be filled with the dibit read. Two bytes used for each dibit.
+ * \param status_count Variable used to keep track of the status symbols in the dibit stream. There is one
+ * status every 36 dibits. With this counter we can skip the status.
+ * \param analog_signal A pointer with the actual analog value read from the input signal and that has been
+ * interpreted as the dibit we are returning in output.
+ * \param did_read_status Address were a boolean is returned. This is set to true when one status symbol was
+ * read (and skipped). This indicates that the sequence on dibits has been broken and is used by the P25
+ * heuristics.
  */
-void read_dibit (dsd_opts* opts, dsd_state* state, char* output, int* status_count);
+int read_dibit (dsd_opts* opts, dsd_state* state, char* output, int* status_count, int* analog_signal,
+        int* did_read_status);
 
 /**
- * Read an "hex" word. An hex word is made up of 3 dibits. Also keeps track of the status dibits.
+ * Reads a number of dibits and stores its value together with the actual analog value that has been
+ * intrepreted as the dibit into an AnalogSignal structure. This is used later in the P25 heuristics module
+ * to improve the accuracy of the digitizer.
+ * \param opts A pointer the the DSD options.
+ * \param state A pointer the the DSD state structure.
+ * \param output A pointer to be filled with the dibits read.
+ * \param count The number of bits to read (remember: 1 dibit = 2 bits).
+ * \param status_count Variable used to keep track of the status symbols in the dibit stream. There is one
+ * status every 36 dibits. With this counter we can skip the status.
+ * \param analog_signal_array The pointer to the start of a sequence of AnalogSignal structures. The
+ * information is updated for each dibit read.
+ * \param analog_signal_index The actual index into the AnalogSignal array. This is increased here wich each
+ * dibit read.
  */
-void read_hex (dsd_opts* opts, dsd_state* state, char* hex, int* status_count);
+void read_dibit_update_analog_data (dsd_opts* opts, dsd_state* state, char* buffer, unsigned int count,
+        int* status_count, AnalogSignal* analog_signal_array, int* analog_signal_index);
 
-#endif // __P25P1_HDU_H__
+/**
+ * Reads a word, made up of length bits or length/2 dibits. Also keeps track of the status dibits.
+ */
+void read_word (dsd_opts* opts, dsd_state* state, char* word, unsigned int length, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index);
+
+/**
+ * Reads 4 bits from the stream of data. Those are the parity bits that follow each hex word.
+ */
+void read_hamm_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index);
+
+/**
+ * Reads 12 bits from the stream of data. Those are the extended Golay parity bits that follow a word of data.
+ */
+void read_golay24_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count, AnalogSignal* analog_signal_array, int* analog_signal_index);
+
+/**
+ * Reads bits from the data stream that should all be zeros. This is valuable information for the
+ * heuristics module and we update it.
+ * \param opts A pointer the the DSD options.
+ * \param state A pointer the the DSD state structure.
+ * \param analog_signal_array The pointer to the start of a sequence of AnalogSignal structures.
+ * \param length Number of bits to read.
+ * \param status_count Variable used to keep track of the status symbols in the dibit stream.
+ * \param new_sequence Bool used to update the sequence_broken value of the first AnalogSignal element.
+ */
+void read_zeros(dsd_opts* opts, dsd_state* state, AnalogSignal* analog_signal_array, unsigned int length,
+        int* status_count, int new_sequence);
+
+#endif // P25P1_HDU_H_9f95c3a5072842e8aaf94444e1452d20
diff --git a/p25p1_heuristics.c b/p25p1_heuristics.c
new file mode 100644
index 0000000..ea48d5e
--- /dev/null
+++ b/p25p1_heuristics.c
@@ -0,0 +1,386 @@
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#include "dsd.h"
+
+#include "p25p1_heuristics.h"
+
+/**
+ * This module is dedicated to improve the accuracy of the digitizer. The digitizer is the piece of code that
+ * translates an analog value to an actual symbol, in the case of P25, a dibit.
+ * It implements a simple Gaussian classifier. It's based in the assumption that the analog values from the
+ * signal follow normal distributions, one single distribution for each symbol.
+ * Analog values for the dibit "0" will fit into a Gaussian bell curve with a characteristic mean and std
+ * distribution and the same goes for dibits "1", "2" and "3."
+ * Hopefully those bell curves are well separated from each other so we can accurately discriminate dibits.
+ * If we could model the Gaussian of each dibit, then given an analog value, the dibit whose Gaussian fits
+ * better is the most likely interpretation for that value. By better fit we can calculate the PDF
+ * (probability density function) for the Gaussian, the one with the highest value is the best fit.
+ *
+ * The approach followed here to model the Gaussian for each dibit is to use the error corrected information
+ * as precise oracles. P25 uses strong error correction, some dibits are doubly protected by Hamming/Golay
+ * and powerful Reed-Solomon codes. If a sequence of dibits clears the last Reed-Solomon check, we can be
+ * quite confident that those values are correct. We can use the analog values for those cleared dibits to
+ * calculate mean and std deviation of our Gaussians. With this we are ready to calculate the PDF of a new
+ * unknown analog value when required.
+ * Values that don't clear the Reed-Solomon check are discarded.
+ * This implementation uses a circular buffer to keep track of the N latest cleared analog dibits so we can
+ * adapt to changes in the signal.
+ * A modification was made to improve results for C4FM signals. See next block comment.
+ */
+
+/**
+ * In the C4FM P25 recorded files from the "samples" repository, it can be observed that there is a
+ * correlation between the correct dibit associated for a given analog value and the value of the previous
+ * dibit. For instance, in one P25 recording, the dibits "0" come with an average analog signal of
+ * 3829 when the previous dibit was also "0," but if the previous dibit was a "3" then the average
+ * analog signal is 6875. These are the mean and std deviations for the full 4x4 combinations of previous and
+ * current dibits:
+ *
+ * 00: count: 200 mean:    3829.12 sd:     540.43    <-
+ * 01: count: 200 mean:   13352.45 sd:     659.74
+ * 02: count: 200 mean:   -5238.56 sd:    1254.70
+ * 03: count: 200 mean:  -13776.50 sd:     307.41
+ * 10: count: 200 mean:    3077.74 sd:    1059.00
+ * 11: count: 200 mean:   11935.11 sd:     776.20
+ * 12: count: 200 mean:   -6079.46 sd:    1003.94
+ * 13: count: 200 mean:  -13845.43 sd:     264.42
+ * 20: count: 200 mean:    5574.33 sd:    1414.71
+ * 21: count: 200 mean:   13687.75 sd:     727.68
+ * 22: count: 200 mean:   -4753.38 sd:     765.95
+ * 23: count: 200 mean:  -12342.17 sd:    1372.77
+ * 30: count: 200 mean:    6875.23 sd:    1837.38    <-
+ * 31: count: 200 mean:   14527.99 sd:     406.85
+ * 32: count: 200 mean:   -3317.61 sd:    1089.02
+ * 33: count: 200 mean:  -12576.08 sd:    1161.77
+ * ||          |             |              |
+ * ||          |             |              \_std deviation
+ * ||          |             |
+ * ||          |             \_mean of the current dibit
+ * ||          |
+ * ||          \_number of dibits used to calculate mean and std deviation
+ * ||
+ * |\_current dibit
+ * |
+ * \_previous dibit
+ *
+ * This effect is not observed on QPSK or GFSK signals, there the mean values are quite consistent regardless
+ * of the previous dibit.
+ *
+ * The following define enables taking the previous dibit into account for C4FM signals. Comment out
+ * to disable.
+ */
+#define USE_PREVIOUS_DIBIT
+
+/**
+ * There is a minimum of cleared analog values we need to produce a meaningful mean and std deviation.
+ */
+#define MIN_ELEMENTS_FOR_HEURISTICS 10
+
+
+//Uncomment to disable the behaviour of this module.
+//#define DISABLE_HEURISTICS
+
+/**
+ * The value of the previous dibit is only taken into account on the C4FM modulation. QPSK and GFSK are
+ * not improved by this technique.
+ */
+static int use_previous_dibit(int rf_mod)
+{
+    // 0: C4FM modulation
+    // 1: QPSK modulation
+    // 2: GFSK modulation
+
+    // Use previoud dibit information when on C4FM
+    return (rf_mod == 0)? 1 : 0;
+}
+
+/**
+ * Update the model of a symbol's Gaussian with new information.
+ * \param heuristics Pointer to the P25Heuristics module with all the needed state information.
+ * \param previous_dibit The cleared previous dibit value.
+ * \param original_dibit The current dibit as it was interpreted initially.
+ * \param dibit The current dibit. Will be different from original_dibit if the FEC fixed it.
+ * \param analog_value The actual analog signal value from which the original_dibit was derived.
+ */
+static void update_p25_heuristics(P25Heuristics* heuristics, int previous_dibit, int original_dibit, int dibit, int analog_value)
+{
+    float mean;
+    int old_value;
+    float old_mean;
+
+    SymbolHeuristics* sh;
+    int number_errors;
+
+#ifndef USE_PREVIOUS_DIBIT
+    previous_dibit = 0;
+#endif
+
+    // Locate the Gaussian (SymbolHeuristics structure) we are going to update
+    sh = &(heuristics->symbols[previous_dibit][dibit]);
+
+    // Update the circular buffers of values
+    old_value = sh->values[sh->index];
+    old_mean = sh->means[sh->index];
+
+    // Update the BER statistics
+    number_errors = 0;
+    if (original_dibit != dibit) {
+        if ((original_dibit == 0 && dibit == 3) || (original_dibit == 3 && dibit == 0) ||
+            (original_dibit == 1 && dibit == 2) || (original_dibit == 2 && dibit == 1)) {
+            // Interpreting a "00" as "11", "11" as "00", "01" as "10" or "10" as "01" counts as 2 errors
+            number_errors = 2;
+        } else {
+            // The other 12 combinations count as 1 error.
+            number_errors = 1;
+        }
+    }
+    update_error_stats(heuristics, 2, number_errors);
+
+    // Update the running mean and variance. This is to calculate the PDF faster when required
+    if (sh->count >= HEURISTICS_SIZE) {
+        sh->sum -= old_value;
+        sh->var_sum -= (((float)old_value) - old_mean) * (((float)old_value) - old_mean);
+    }
+    sh->sum += analog_value;
+
+    sh->values[sh->index] = analog_value;
+    if (sh->count < HEURISTICS_SIZE) {
+        sh->count++;
+    }
+    mean = sh->sum / ((float)sh->count);
+    sh->means[sh->index] = mean;
+    if (sh->index >= (HEURISTICS_SIZE-1)) {
+        sh->index = 0;
+    } else {
+        sh->index++;
+    }
+
+    sh->var_sum += (((float)analog_value) - mean) * (((float)analog_value) - mean);
+}
+
+void contribute_to_heuristics(int rf_mod, P25Heuristics* heuristics, AnalogSignal* analog_signal_array, int count)
+{
+    int i;
+    int use_prev_dibit;
+
+#ifdef USE_PREVIOUS_DIBIT
+    use_prev_dibit = use_previous_dibit(rf_mod);
+#else
+    use_prev_dibit = 0;
+#endif
+
+    for (i=0; i<count; i++) {
+        int use;
+        int prev_dibit;
+
+        if (use_prev_dibit) {
+            if (analog_signal_array[i].sequence_broken) {
+                // The sequence of dibits was broken here so we don't have reliable information on the actual
+                // value of the previous dibit. Don't use this value.
+                use = 0;
+            } else {
+                use = 1;
+                // The previous dibit is the corrected_dibit of the previous element
+                prev_dibit = analog_signal_array[i-1].corrected_dibit;
+            }
+        } else {
+            use = 1;
+            prev_dibit = 0;
+        }
+
+        if (use) {
+            update_p25_heuristics(heuristics, prev_dibit, analog_signal_array[i].dibit,
+                    analog_signal_array[i].corrected_dibit, analog_signal_array[i].value);
+        }
+    }
+}
+
+/**
+ * Initializes the symbol's heuristics state.
+ * \param sh The SymbolHeuristics structure to initialize.
+ */
+static void initialize_symbol_heuristics(SymbolHeuristics* sh)
+{
+    sh->count = 0;
+    sh->index = 0;
+    sh->sum = 0;
+    sh->var_sum = 0;
+}
+
+void initialize_p25_heuristics(P25Heuristics* heuristics)
+{
+    int i, j;
+    for (i=0; i<4; i++) {
+        for (j=0; j<4; j++) {
+            initialize_symbol_heuristics(&(heuristics->symbols[i][j]));
+        }
+    }
+    heuristics->bit_count = 0;
+    heuristics->bit_error_count = 0;
+}
+
+/**
+ * Important method to calculate the PDF (probability density function) of the Gaussian.
+ * TODO: improve performance. Since we are calculating this value to compare it with other PDF we can
+ * simplify very much. We don't really need to know the actual PDF value, just which Gaussian's got the
+ * highest PDF, which is a simpler problem.
+ */
+static float evaluate_pdf(SymbolHeuristics* se, int value)
+{
+    float x = (se->count*((float)value) - se->sum);
+    float y = -0.5F*x*x/(se->count*se->var_sum);
+    float pdf = sqrtf(se->count / se->var_sum) * expf(y) / sqrtf(2.0F * ((float) M_PI));
+
+    return pdf;
+}
+
+
+/**
+ * Logging of the internal PDF values for a given analog value and previous dibit.
+ */
+static void debug_log_pdf(P25Heuristics* heuristics, int previous_dibit, int analog_value)
+{
+    int i;
+    float pdfs[4];
+
+    for (i=0; i<4; i++) {
+        pdfs[i] = evaluate_pdf(&(heuristics->symbols[previous_dibit][i]), analog_value);
+    }
+
+    printf("v: %i, (%e, %e, %e, %e)\n", analog_value, pdfs[0], pdfs[1], pdfs[2], pdfs[3]);
+}
+
+int estimate_symbol(int rf_mod, P25Heuristics* heuristics, int previous_dibit, int analog_value, int* dibit)
+{
+    int valid;
+    int i;
+    float pdfs[4];
+
+
+#ifdef USE_PREVIOUS_DIBIT
+    int use_prev_dibit = use_previous_dibit(rf_mod);
+
+    if (use_prev_dibit == 0)
+      {
+        // Ignore
+        previous_dibit = 0;
+      }
+#else
+	// Use previous_dibit as it comes.
+#endif
+
+    valid = 1;
+
+    // Check if we have enough values to model the Gaussians for each symbol involved.
+    for (i=0; i<4; i++) {
+        if (heuristics->symbols[previous_dibit][i].count >= MIN_ELEMENTS_FOR_HEURISTICS) {
+            pdfs[i] = evaluate_pdf(&(heuristics->symbols[previous_dibit][i]), analog_value);
+        } else {
+            // Not enough data, we don't trust this result
+            valid = 0;
+            break;
+        }
+    }
+
+    if (valid) {
+        // Find the highest pdf
+        int max_index;
+        float max;
+
+        max_index = 0;
+        max = pdfs[0];
+        for (i=1; i<4; i++) {
+            if (pdfs[i] > max) {
+                max_index = i;
+                max = pdfs[i];
+            }
+        }
+
+        // The symbol is the one with the highest pdf
+        *dibit = max_index;
+    }
+
+#ifdef DISABLE_HEURISTICS
+    valid = 0;
+#endif
+
+    return valid;
+}
+
+/**
+ * Logs the internal state of the heuristic's state. Good for debugging.
+ */
+static void debug_print_symbol_heuristics(int previous_dibit, int dibit, SymbolHeuristics* sh)
+{
+    float mean, sd;
+    int k;
+    int n;
+
+    n = sh->count;
+    if (n == 0)
+      {
+        mean = 0;
+        sd = 0;
+      }
+    else
+      {
+        mean = sh->sum/n;
+        sd = sqrtf(sh->var_sum / ((float) n));
+      }
+    printf("%i%i: count: %2i mean: % 10.2f sd: % 10.2f", previous_dibit, dibit, sh->count, mean, sd);
+    /*
+    printf("(");
+    for (k=0; k<n; k++)
+      {
+        if (k != 0)
+          {
+            printf(", ");
+          }
+        printf("%i", sh->values[k]);
+      }
+    printf(")");
+    */
+    printf("\n");
+
+}
+
+void debug_print_heuristics(P25Heuristics* heuristics)
+{
+  int i,j;
+
+  printf("\n");
+
+  for(i=0; i<4; i++)
+    {
+      for(j=0; j<4; j++)
+        {
+          debug_print_symbol_heuristics(i, j, &(heuristics->symbols[i][j]));
+        }
+    }
+}
+
+void update_error_stats(P25Heuristics* heuristics, int bits, int errors)
+{
+    heuristics->bit_count += bits;
+    heuristics->bit_error_count += errors;
+
+    // Normalize to avoid overflow in the counters
+    if ((heuristics->bit_count & 1) == 0 && (heuristics->bit_error_count & 1) == 0) {
+        // We can divide both values by 2 safely. We just care about their ratio, not the actual value
+        heuristics->bit_count >>= 1;
+        heuristics->bit_error_count >>= 1;
+    }
+}
+
+float get_P25_BER_estimate(P25Heuristics* heuristics)
+{
+    float ber;
+    if (heuristics->bit_count == 0) {
+        ber = 0.0F;
+    } else {
+        ber = ((float)heuristics->bit_error_count) * 100.0F / ((float)heuristics->bit_count);
+    }
+    return ber;
+}
diff --git a/p25p1_heuristics.h b/p25p1_heuristics.h
new file mode 100644
index 0000000..a424596
--- /dev/null
+++ b/p25p1_heuristics.h
@@ -0,0 +1,91 @@
+
+#ifndef P25P1_HEURISTICS_H_030dd3530b7546abbb56f8dd1e66a2f6
+#define P25P1_HEURISTICS_H_030dd3530b7546abbb56f8dd1e66a2f6
+
+#define HEURISTICS_SIZE 200
+typedef struct
+{
+  int values[HEURISTICS_SIZE];
+  float means[HEURISTICS_SIZE];
+  int index;
+  int count;
+  float sum;
+  float var_sum;
+} SymbolHeuristics;
+
+typedef struct
+{
+  unsigned int bit_count;
+  unsigned int bit_error_count;
+  SymbolHeuristics symbols[4][4];
+} P25Heuristics;
+
+typedef struct
+{
+    int value;
+    int dibit;
+    int corrected_dibit;
+    int sequence_broken;
+} AnalogSignal;
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+/**
+ * Initializes the heuristics state.
+ * \param heuristics The P25Heuristics structure to initialize.
+ */
+void initialize_p25_heuristics(P25Heuristics* heuristics);
+
+/**
+ * Important method that estimates the most likely symbol for a given analog signal value and previous dibit.
+ * This is called by the digitizer.
+ * \param rf_mod Indicates the modulation used. The previous dibit is only used on C4FM.
+ * \param heuristics Pointer to the P25Heuristics module with all the needed state information.
+ * \param previous_dibit The previous dibit.
+ * \param analog_value The signal's analog value we want to interpret as a dibit.
+ * \param dibit Address were to store the estimated dibit.
+ * \return A boolean set to true if we are able to estimate a dibit. The reason why we might not be able
+ * to estimate it is because we don't have enough information to model the Gaussians (not enough data
+ * has been passed to contribute_to_heuristics).
+ */
+int estimate_symbol(int rf_mod, P25Heuristics* heuristics, int previous_dibit, int analog_value, int* dibit);
+
+/**
+ * Log some useful information on the heuristics state.
+ */
+void debug_print_heuristics(P25Heuristics* heuristics);
+
+/**
+ * This method contributes valuable information from dibits whose value we are confident is correct. We take
+ * the dibits and corresponding analog signal values to model the Gaussians for each dibit (and previous
+ * dibit if enabled).
+ * \param rf_mod Indicates the modulation used. The previous dibit is only used on C4FM.
+ * \param heuristics Pointer to the P25Heuristics module with all the needed state information.
+ * \param analog_signal_array Sequence of AnalogSignal which contain the cleared dibits and analog values.
+ * \param count number of cleared dibits passed (= number of elements to use from analog_signal_array).
+ */
+void contribute_to_heuristics(int rf_mod, P25Heuristics* heuristics, AnalogSignal* analog_signal_array, int count);
+
+/**
+ * Updates the estimate for the BER (bit error rate). Mind this is method is not called for every single
+ * bit in the data stream but only for those bits over which we have an estimate of its error rate,
+ * specifically the bits that are protected by Reed-Solomon codes.
+ * \param heuristics The heuristics state.
+ * \param bits The number of bits we have read.
+ * \param errors The number of errors we estimate in those bits.
+ */
+void update_error_stats(P25Heuristics* heuristics, int bits, int errors);
+
+/**
+ * Returns the estimate for the BER (bit error rate).
+ * \return The estimated BER. This is just the percentage of errors over the processed bits.
+ */
+float get_P25_BER_estimate(P25Heuristics* heuristics);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // P25P1_HEURISTICS_H_030dd3530b7546abbb56f8dd1e66a2f6
diff --git a/p25p1_ldu.c b/p25p1_ldu.c
index 990e229..c7dc85b 100644
--- a/p25p1_ldu.c
+++ b/p25p1_ldu.c
@@ -2,8 +2,65 @@
 #include "dsd.h"
 #include "p25p1_const.h"
 #include "p25p1_hdu.h"
+#include "p25p1_check_ldu.h"
+#include "p25p1_heuristics.h"
+
+#ifdef TRACE_DSD
+static void
+debug_write_label_imbe(dsd_state* state, unsigned int cc, int bitindex, char bit)
+{
+  float left, right;
+
+  if (state->debug_label_imbe_file == NULL)
+    {
+      state->debug_label_imbe_file = fopen ("pp_label_imbe.txt", "w");
+    }
+  left = (state->debug_sample_index-10) / 48000.0F;
+  right = (state->debug_sample_index) / 48000.0F;
+  fprintf(state->debug_label_imbe_file, "%f\t%f\tC_%i[%i]=%c\n", left, right, cc, bitindex, (bit + '0'));
+}
+#endif
+
+/**
+ * Logs the IMBE's c0-c6 words.
+ */
+static void
+debug_log_imbe(char imbe_fr[8][23])
+{
+  int i, j;
+  printf("    ");
+
+  for (j=0; j<4; j++)
+    {
+        printf("{");
+        for (i=0; i<23; i++)
+          {
+            if (i>0)
+              {
+                printf(", ");
+              }
+            printf("%c", (imbe_fr[j][i] + '0'));
+          }
+        printf("}; ");
+    }
+
+  for (j=4; j<7; j++)
+    {
+      printf("{");
+      for (i=0; i<15; i++)
+        {
+          if (i>0)
+            {
+              printf(", ");
+            }
+          printf("%c", (imbe_fr[j][i] + '0'));
+        }
+      printf("}; ");
+    }
+
+  printf("\n");
+}
 
-// separate imbe frames and deinterleave
 void
 process_IMBE (dsd_opts* opts, dsd_state* state, int* status_count)
 {
@@ -16,13 +73,25 @@ process_IMBE (dsd_opts* opts, dsd_state* state, int* status_count)
   y = iY;
   z = iZ;
 
+#ifdef TRACE_DSD
+  state->debug_prefix = 'I';
+#endif
+
   for (j = 0; j < 72; j++)
     {
       if (*status_count == 35)
         {
+          // Skip the status symbol
+#ifdef TRACE_DSD
+          state->debug_prefix = 's';
+#endif
           status = getDibit (opts, state);
           // TODO: do something useful with the status bits...
           *status_count = 1;
+
+#ifdef TRACE_DSD
+          state->debug_prefix = 'I';
+#endif
         }
       else
         {
@@ -31,77 +100,170 @@ process_IMBE (dsd_opts* opts, dsd_state* state, int* status_count)
       dibit = getDibit (opts, state);
       imbe_fr[*w][*x] = (1 & (dibit >> 1)); // bit 1
       imbe_fr[*y][*z] = (1 & dibit);        // bit 0
+
+#ifdef TRACE_DSD
+      if (*w == 0) {
+          debug_write_label_imbe(state, 0, *x, (1 & (dibit >> 1)));
+      }
+      if (*y == 0) {
+          debug_write_label_imbe(state, 0, *z, (1 & dibit));
+      }
+#endif
+
       w++;
       x++;
       y++;
       z++;
     }
 
+#ifdef TRACE_DSD
+  state->debug_prefix = '\0';
+#endif
+
   if (state->p25kid == 0 || opts->unmute_encrypted_p25 == 1)
     {
-      processMbeFrame (opts, state, imbe_fr, NULL, NULL);
+      {
+          // Check for a non-standard c0 transmitted
+          // This is explained here: https://github.com/szechyjs/dsd/issues/24
+          char non_standard_word[23] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0};
+          int match = 1;
+          unsigned int i;
+          for (i=0; i<23; i++) {
+              if (imbe_fr[0][i] != non_standard_word[i]) {
+                  match = 0;
+                  break;
+              }
+          }
+
+          if (match) {
+              // Skip this particular value. If we let it pass it will be signaled as an erroneus IMBE
+              printf("(Non-standard IMBE c0 detected, skipped)");
+          } else {
+              processMbeFrame (opts, state, imbe_fr, NULL, NULL);
+          }
+      }
     }
 }
 
-/**
- * Read 4 bits from the stream of data. Those are the parity bits that follow each hex word.
- */
-static void
-read_hamm_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count)
-{
-  // Read 2 dibits = read 4 bits.
-  read_dibit(opts, state, parity, status_count);
-  read_dibit(opts, state, parity + 2, status_count);
-}
+// Uncomment this line for verbose information on the error correction of the LDU bits.
+//#define LDU_DEBUG
 
-//#define __LDU_DEBUG__
-
-/**
- * Read an hex word, its parity bits and attempts to error correct it using the Hamming algorithm.
- */
 void
-read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count)
+read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
 {
   char parity[4];
-  int i;
+  int error_count;
 
   // Read the hex word
-  read_hex (opts, state, hex, status_count);
+  read_word (opts, state, hex, 6, status_count, analog_signal_array, analog_signal_index);
   // Read the parity
-  read_hamm_parity (opts, state, parity, status_count);
+  read_hamm_parity (opts, state, parity, status_count, analog_signal_array, analog_signal_index);
 
-#ifdef __LDU_DEBUG__
+#ifdef LDU_DEBUG
   printf("[");
-  for (i = 0; i < 6; i++) {
+  for (i = 0; i < 6; i++)
+    {
       printf("%c", (hex[i] == 1)? 'X' : ' ');
-  }
+    }
   printf("-");
-  for (i = 0; i < 4; i++) {
+  for (i = 0; i < 4; i++)
+    {
       printf("%c", (parity[i] == 1)? 'X' : ' ');
-  }
+    }
   printf("]");
 #endif
 
   // Use Hamming to error correct the hex word
-  int error_count = check_and_fix_hamming_10_12_13(hex, parity);
+  error_count = check_and_fix_hamming_10_6_3(hex, parity);
 
-  if (error_count == 1) {
+  if (error_count == 1)
+    {
       state->debug_header_errors++;
-  } else if (error_count ==2) {
+    }
+  else if (error_count ==2)
+    {
       state->debug_header_critical_errors++;
-  }
+    }
 
-#ifdef __LDU_DEBUG__
+#ifdef LDU_DEBUG
   printf(" -> [");
-  for (i = 0; i < 6; i++) {
+  for (i = 0; i < 6; i++)
+    {
       printf("%c", (hex[i] == 1)? 'X' : ' ');
-  }
+    }
   printf("]");
-  if (error_count == 1) {
+  if (error_count == 1)
+    {
       printf(" fixed!");
-  } else if (error_count ==2) {
+    }
+  else if (error_count ==2)
+    {
       printf(" IRRECOVERABLE");
-  }
+    }
   printf("\n");
 #endif
 }
+
+void
+correct_hamming_dibits(char* data, int count, AnalogSignal* analog_signal_array)
+{
+  char parity[4];
+  int i, j;
+  int analog_signal_index;
+
+  analog_signal_index = 0;
+
+  for (i=count-1; i>=0; i--)
+    {
+      // Take the next 3 dibits (1 hex word) as they are
+      for (j=0; j<6; j+=2)  // 3 iterations -> 3 dibits
+        {
+          int dibit = (data[i*6+j] << 1) | data[i*6+j+1];
+          // This dibit is the correct value we should have read in the first place
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("LDU word corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+
+      // The next two dibits are calculated has the hamming parity of the hex word
+      encode_hamming_10_6_3(data+i*6, parity);
+
+      for (j=0; j<4; j+=2)  // 2 iterations -> 2 dibits
+        {
+          int dibit = (parity[j] << 1) | parity[j+1];
+          // Again, this dibit is the correct value we should have read in the first place
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("LDU-HM parity corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+    }
+}
+
+void
+debug_ldu_header(dsd_state* state)
+{
+#ifdef TRACE_DSD
+  float s = state->debug_sample_index / 48000.0F;
+  printf("Start of LDU at sample %f\n", s);
+#endif
+
+  debug_print_heuristics(&state->p25_heuristics);
+}
+
diff --git a/p25p1_ldu.h b/p25p1_ldu.h
index fa73a19..eed8b70 100644
--- a/p25p1_ldu.h
+++ b/p25p1_ldu.h
@@ -1,18 +1,49 @@
 
-#ifndef __P25P1_LDU_H__
-#define __P25P1_LDU_H__
+#ifndef P25P1_LDU_H_a3c417fcb7804991b0e6d828066bd133
+#define P25P1_LDU_H_a3c417fcb7804991b0e6d828066bd133
 
 #include "dsd.h"
-#include "p25p1_const.h"
 
-// separate imbe frames and deinterleave
-void
-process_IMBE (dsd_opts* opts, dsd_state* state, int* status_count);
+#include "p25p1_hdu.h"
+#include "p25p1_const.h"
+#include "p25p1_heuristics.h"
 
 /**
- * Read an hex word, its parity bits and attempts to error correct it using the Hamming algorithm.
+ * Separate imbe frames and deinterleave.
+ * This important methods read the IMBE data from the stream and passes it to the vocoder to produce audio.
+ * \param opts The DSD options.
+ * \param state The DSD state.
+ * \status_count An index that allows us to skip the status words interleaved every 36 dibit in the data
+ * stream.
  */
-void
-read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count);
+void process_IMBE (dsd_opts* opts, dsd_state* state, int* status_count);
 
-#endif // __P25P1_LDU_H__
+/**
+ * Reads an hex word, its parity bits and attempts to error correct it using the Hamming FEC.
+ * \param opts The DSD options.
+ * \param state The DSD state.
+ * \param hex Pointer where to store the read hex word. Six bytes, one per bit.
+ * \param status_count An index that allows us to skip the status words interleaved every 36 dibit in the data
+ * stream.
+ * \param analog_singal_array Pointer to a sequence of AnalogSignal elements.
+ * \param analog_signal_index The current index in the analog_singal_array. This value is increased on each
+ * dibit read.
+ */
+void read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index);
+
+/**
+ * Correct the information in analog_signal_array according with the content of data, which has been
+ * error corrected and should be valid.
+ * \param Dibits that have already been error corrected and we trust are correct.
+ * \count Number of dibits.
+ * \param analog_signal_array Pointer to a sequence of AnalogSignal elements, as many as the value of count.
+ */
+void correct_hamming_dibits(char* hex_data, int hex_count, AnalogSignal* analog_signal_array);
+
+/**
+ * Logs some debug info.
+ */
+void debug_ldu_header(dsd_state* state);
+
+#endif // P25P1_LDU_H_a3c417fcb7804991b0e6d828066bd133
diff --git a/p25p1_ldu1.c b/p25p1_ldu1.c
index 99060d7..fcafbfd 100644
--- a/p25p1_ldu1.c
+++ b/p25p1_ldu1.c
@@ -20,14 +20,14 @@
 
 #include "p25p1_ldu.h"
 #include "p25p1_check_ldu.h"
-
+#include "p25p1_hdu.h"
 
 
 void
 processLDU1 (dsd_opts* opts, dsd_state* state)
 {
   // extracts IMBE frames from LDU frame
-  int i, j, k, dibit;
+  int i;
   char lcformat[9], mfid[9], lcinfo[57];
   char lsd1[9], lsd2[9];
 
@@ -38,15 +38,15 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
 
   int irrecoverable_errors;
 
-  lcformat[8] = 0;
-  mfid[8] = 0;
-  lcinfo[56] = 0;
-  lsd1[8] = 0;
-  lsd2[8] = 0;
+  AnalogSignal analog_signal_array[12*(3+2)+12*(3+2)];
+  int analog_signal_index;
+
+
+  analog_signal_index = 0;
 
   // we skip the status dibits that occur every 36 symbols
   // the first IMBE frame starts 14 symbols before next status
-  // so we start counter at 22
+  // so we start counter at 36-14-1 = 21
   status_count = 21;
 
   if (opts->errorbars == 1)
@@ -55,63 +55,93 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
     }
 
   // IMBE 1
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '0';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // IMBE 2
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '1';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 2
-  read_and_correct_hex_word (opts, state, &(hex_data[11][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[10][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 9][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 8][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[11][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[10][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 9][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 8][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[0*5].sequence_broken = 1;
 
   // IMBE 3
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '2';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 3
-  read_and_correct_hex_word (opts, state, &(hex_data[ 7][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 6][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 5][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 4][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 7][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 6][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 5][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 4][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[4*5].sequence_broken = 1;
 
   // IMBE 4
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '3';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 4
-  read_and_correct_hex_word (opts, state, &(hex_data[ 3][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 2][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 1][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 0][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 3][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 2][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 1][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 0][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[8*5].sequence_broken = 1;
 
   // IMBE 5
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '4';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 5
-  read_and_correct_hex_word (opts, state, &(hex_parity[11][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[10][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 9][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 8][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_parity[11][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[10][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 9][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 8][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[12*5].sequence_broken = 1;
 
   // IMBE 6
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '5';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 6
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 7][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 6][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 5][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 4][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 7][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 6][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 5][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 4][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[16*5].sequence_broken = 1;
 
   // IMBE 7
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '6';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 7
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 3][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 2][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 1][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 0][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 3][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 2][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 1][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 0][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[20*5].sequence_broken = 1;
 
   // IMBE 8
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '7';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 8: LSD (low speed data)
@@ -121,11 +151,11 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
 
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, lsd+i, &status_count);
+        read_dibit(opts, state, lsd+i, &status_count, NULL, NULL);
       }
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, cyclic_parity+i, &status_count);
+        read_dibit(opts, state, cyclic_parity+i, &status_count, NULL, NULL);
       }
     for (i=0; i<8; i++)
       {
@@ -134,11 +164,11 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
 
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, lsd+i, &status_count);
+        read_dibit(opts, state, lsd+i, &status_count, NULL, NULL);
       }
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, cyclic_parity+i, &status_count);
+        read_dibit(opts, state, cyclic_parity+i, &status_count, NULL, NULL);
       }
     for (i=0; i<8; i++)
       {
@@ -150,6 +180,9 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
   }
 
   // IMBE 9
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '8';
+#endif
   process_IMBE (opts, state, &status_count);
 
   if (opts->errorbars == 1)
@@ -165,18 +198,54 @@ processLDU1 (dsd_opts* opts, dsd_state* state)
   // trailing status symbol
   {
       int status;
-      status = getDibit (opts, state) + 48;
+      status = getDibit (opts, state) + '0';
       // TODO: do something useful with the status bits...
   }
 
   // Error correct the hex_data using Reed-Solomon hex_parity
-  irrecoverable_errors = check_and_fix_redsolomon_24_12_13((char*)hex_data, (char*)hex_parity);
-  if (irrecoverable_errors == 1) {
+  irrecoverable_errors = check_and_fix_reedsolomon_24_12_13((char*)hex_data, (char*)hex_parity);
+  if (irrecoverable_errors == 1)
+    {
       state->debug_header_critical_errors++;
-  }
 
+      // We can correct (13-1)/2 = 6 errors. If we failed, it means that there were more than 6 errors in
+      // these 12+12 words. But take into account that each hex word was already error corrected with
+      // Hamming(10,6,3), which can correct 1 bits on each sequence of (6+4) bits. We could say that there
+      // were 7 errors of 2 bits.
+      update_error_stats(&state->p25_heuristics, 12*6+12*6, 7*2);
+    }
+  else
+    {
+      // Same comments as in processHDU. See there.
+
+      char fixed_parity[12*6];
+
+      // Correct the dibits that we read according with hex_data values
+      correct_hamming_dibits((char*)hex_data, 12, analog_signal_array);
+
+      // Generate again the Reed-Solomon parity
+      encode_reedsolomon_24_12_13((char*)hex_data, fixed_parity);
+
+      // Correct the dibits that we read according with the fixed parity values
+      correct_hamming_dibits(fixed_parity, 12, analog_signal_array+12*(3+2));
+
+      // Once corrected, contribute this information to the heuristics module
+      contribute_to_heuristics(state->rf_mod, &(state->p25_heuristics), analog_signal_array, 12*(3+2)+12*(3+2));
+    }
+
+#ifdef HEURISTICS_DEBUG
+  printf("(audio errors, header errors, critical header errors) (%i,%i,%i)\n",
+          state->debug_audio_errors, state->debug_header_errors, state->debug_header_critical_errors);
+#endif
+
+  // Now put the corrected data into the DSD structures
+
+  lcformat[8] = 0;
+  mfid[8] = 0;
+  lcinfo[56] = 0;
+  lsd1[8] = 0;
+  lsd2[8] = 0;
 
-  // Put the corrected data into the DSD structures
   lcformat[0] = hex_data[11][0] + '0';
   lcformat[1] = hex_data[11][1] + '0';
   lcformat[2] = hex_data[11][2] + '0';
diff --git a/p25p1_ldu2.c b/p25p1_ldu2.c
index 5b7fe15..238993f 100644
--- a/p25p1_ldu2.c
+++ b/p25p1_ldu2.c
@@ -20,6 +20,7 @@
 
 #include "p25p1_ldu.h"
 #include "p25p1_check_ldu.h"
+#include "p25p1_hdu.h"
 
 
 
@@ -27,27 +28,26 @@ void
 processLDU2 (dsd_opts * opts, dsd_state * state)
 {
   // extracts IMBE frames from LDU frame
-  int i, j, k, dibit;
+  int i;
   char mi[73], algid[9], kid[17];
   char lsd1[9], lsd2[9];
   int algidhex, kidhex;
 
   int status_count;
 
-  char hex_data[16][6];   // Data in hex-words (6 bit words). A total of 12 hex words.
-  char hex_parity[8][6];  // Parity of the data, again in hex-word format. A total of 12 parity hex words.
+  char hex_data[16][6];    // Data in hex-words (6 bit words). A total of 16 hex words.
+  char hex_parity[8][6];   // Parity of the data, again in hex-word format. A total of 12 parity hex words.
 
   int irrecoverable_errors;
 
-  mi[72] = 0;
-  algid[8] = 0;
-  kid[16] = 0;
-  lsd1[8] = 0;
-  lsd2[8] = 0;
+  AnalogSignal analog_signal_array[16*(3+2)+8*(3+2)];
+  int analog_signal_index;
+
+  analog_signal_index = 0;
 
   // we skip the status dibits that occur every 36 symbols
   // the first IMBE frame starts 14 symbols before next status
-  // so we start counter at 22
+  // so we start counter at 36-14-1 = 21
   status_count = 21;
 
   if (opts->errorbars == 1)
@@ -56,63 +56,93 @@ processLDU2 (dsd_opts * opts, dsd_state * state)
     }
 
   // IMBE 1
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '0';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // IMBE 2
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '1';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 2
-  read_and_correct_hex_word (opts, state, &(hex_data[15][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[14][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[13][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[12][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[15][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[14][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[13][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[12][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[0*5].sequence_broken = 1;
 
   // IMBE 3
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '2';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 3
-  read_and_correct_hex_word (opts, state, &(hex_data[11][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[10][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 9][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 8][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[11][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[10][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 9][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 8][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[4*5].sequence_broken = 1;
 
   // IMBE 4
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '3';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 4
-  read_and_correct_hex_word (opts, state, &(hex_data[ 7][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 6][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 5][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 4][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 7][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 6][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 5][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 4][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[8*5].sequence_broken = 1;
 
   // IMBE 5
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '4';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 5
-  read_and_correct_hex_word (opts, state, &(hex_data[ 3][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 2][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 1][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_data[ 0][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 3][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 2][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 1][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_data[ 0][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[12*5].sequence_broken = 1;
 
   // IMBE 6
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '5';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 6
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 7][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 6][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 5][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 4][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 7][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 6][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 5][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 4][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[16*5].sequence_broken = 1;
 
   // IMBE 7
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '6';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 7
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 3][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 2][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 1][0]), &status_count);
-  read_and_correct_hex_word (opts, state, &(hex_parity[ 0][0]), &status_count);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 3][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 2][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 1][0]), &status_count, analog_signal_array, &analog_signal_index);
+  read_and_correct_hex_word (opts, state, &(hex_parity[ 0][0]), &status_count, analog_signal_array, &analog_signal_index);
+  analog_signal_array[20*5].sequence_broken = 1;
 
   // IMBE 8
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '7';
+#endif
   process_IMBE (opts, state, &status_count);
 
   // Read data after IMBE 8: LSD (low speed data)
@@ -122,11 +152,11 @@ processLDU2 (dsd_opts * opts, dsd_state * state)
 
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, lsd+i, &status_count);
+        read_dibit(opts, state, lsd+i, &status_count, NULL, NULL);
       }
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, cyclic_parity+i, &status_count);
+        read_dibit(opts, state, cyclic_parity+i, &status_count, NULL, NULL);
       }
     for (i=0; i<8; i++)
       {
@@ -135,11 +165,11 @@ processLDU2 (dsd_opts * opts, dsd_state * state)
 
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, lsd+i, &status_count);
+        read_dibit(opts, state, lsd+i, &status_count, NULL, NULL);
       }
     for (i=0; i<=6; i+=2)
       {
-        read_dibit(opts, state, cyclic_parity+i, &status_count);
+        read_dibit(opts, state, cyclic_parity+i, &status_count, NULL, NULL);
       }
     for (i=0; i<8; i++)
       {
@@ -151,6 +181,9 @@ processLDU2 (dsd_opts * opts, dsd_state * state)
   }
 
   // IMBE 9
+#ifdef TRACE_DSD
+  state->debug_prefix_2 = '8';
+#endif
   process_IMBE (opts, state, &status_count);
 
   if (opts->errorbars == 1)
@@ -166,18 +199,55 @@ processLDU2 (dsd_opts * opts, dsd_state * state)
   // trailing status symbol
   {
       int status;
-      status = getDibit (opts, state) + 48;
+      status = getDibit (opts, state) + '0';
       // TODO: do something useful with the status bits...
   }
 
   // Error correct the hex_data using Reed-Solomon hex_parity
-  irrecoverable_errors = check_and_fix_redsolomon_24_16_9((char*)hex_data, (char*)hex_parity);
-  if (irrecoverable_errors == 1) {
+  irrecoverable_errors = check_and_fix_reedsolomon_24_16_9((char*)hex_data, (char*)hex_parity);
+  if (irrecoverable_errors == 1)
+    {
       state->debug_header_critical_errors++;
-  }
+
+      // We can correct (9-1)/2 = 4 errors. If we failed, it means that there were more than 4 errors in
+      // these 12+12 words. But take into account that each hex word was already error corrected with
+      // Hamming(10,6,3), which can correct 1 bits on each sequence of (6+4) bits. We could say that there
+      // were 5 errors of 2 bits.
+      update_error_stats(&state->p25_heuristics, 12*6+12*6, 5*2);
+    }
+  else
+    {
+      // Same comments as in processHDU. See there.
+
+      char fixed_parity[8*6];
+
+      // Correct the dibits that we read according with hex_data values
+      correct_hamming_dibits((char*)hex_data, 16, analog_signal_array);
+
+      // Generate again the Reed-Solomon parity
+      encode_reedsolomon_24_16_9((char*)hex_data, fixed_parity);
+
+      // Correct the dibits that we read according with the fixed parity values
+      correct_hamming_dibits(fixed_parity, 8, analog_signal_array+16*(3+2));
+
+      // Once corrected, contribute this information to the heuristics module
+      contribute_to_heuristics(state->rf_mod, &(state->p25_heuristics), analog_signal_array, 16*(3+2)+8*(3+2));
+    }
 
 
-  // Put the corrected data into the DSD structures
+#ifdef HEURISTICS_DEBUG
+  printf("(audio errors, header errors, critical header errors) (%i,%i,%i)\n",
+          state->debug_audio_errors, state->debug_header_errors, state->debug_header_critical_errors);
+#endif
+
+  // Now put the corrected data into the DSD structures
+
+  mi[72] = 0;
+  algid[8] = 0;
+  kid[16] = 0;
+  lsd1[8] = 0;
+  lsd2[8] = 0;
+
   mi[ 0]   = hex_data[15][0] + '0';
   mi[ 1]   = hex_data[15][1] + '0';
   mi[ 2]   = hex_data[15][2] + '0';
diff --git a/p25p1_tdu.c b/p25p1_tdu.c
new file mode 100644
index 0000000..2401910
--- /dev/null
+++ b/p25p1_tdu.c
@@ -0,0 +1,34 @@
+
+
+#include "dsd.h"
+
+#include "p25p1_hdu.h"
+#include "p25p1_heuristics.h"
+
+
+void
+processTDU (dsd_opts* opts, dsd_state* state)
+{
+    AnalogSignal analog_signal_array[14];
+    int status_count;
+
+    // we skip the status dibits that occur every 36 symbols
+    // the first IMBE frame starts 14 symbols before next status
+    // so we start counter at 36-14-1 = 21
+    status_count = 21;
+
+    // Next 14 dibits should be zeros
+    read_zeros(opts, state, analog_signal_array, 28, &status_count, 1);
+
+    // Next we should find an status dibit
+    if (status_count != 35) {
+        printf("*** SYNC ERROR\n");
+    }
+
+    // trailing status symbol
+    {
+        int status;
+        status = getDibit (opts, state) + '0';
+        // TODO: do something useful with the status bits...
+    }
+}
diff --git a/p25p1_tdulc.c b/p25p1_tdulc.c
index 097970d..56ff081 100644
--- a/p25p1_tdulc.c
+++ b/p25p1_tdulc.c
@@ -17,141 +17,374 @@
 
 #include "dsd.h"
 
-void
-processTDULC (dsd_opts * opts, dsd_state * state)
-{
+#include "p25p1_hdu.h"
+#include "p25p1_check_hdu.h"
+#include "p25p1_check_ldu.h"
+#include "p25p1_heuristics.h"
 
-  char lcinfo[57], lcformat[9], mfid[9];
+// Uncomment for some verbose debug info
+//#define TDULC_DEBUG
+
+/**
+ * Reverse the order of bits in a 12-bit word. We need this to accommodate to the expected bit order in
+ * some algorithms.
+ * \param dodeca The 12-bit word to reverse.
+ */
+static void
+swap_hex_words_bits(char* dodeca)
+{
+  int j;
+  for(j=0; j<6; j++)
+    {
+      int swap;
+      swap = dodeca[j];
+      dodeca[j] = dodeca[j+6];
+      dodeca[j+6] = swap;
+    }
+}
+
+/**
+ * Reverse the order of bits in a sequence of six pairs of 12-bit words and their parities.
+ * \param dodeca_data Pointer to the start of the 12-bit words sequence.
+ * \param dodeca_parity Pointer to the parities sequence.
+ */
+static void
+swap_hex_words(char* dodeca_data, char* dodeca_parity)
+{
+  int i;
+  for(i=0; i<6; i++)
+    {
+      swap_hex_words_bits(dodeca_data + i*12);
+      swap_hex_words_bits(dodeca_parity + i*12);
+    }
+}
+
+/**
+ * Read an hex word, its parity bits and attempts to error correct it using the Hamming algorithm.
+ */
+static void
+read_and_correct_dodeca_word (dsd_opts* opts, dsd_state* state, char* dodeca, int* status_count,
+        AnalogSignal* analog_signal_array, int* analog_signal_index)
+{
+  char parity[12];
+  int fixed_errors;
+  int irrecoverable_errors;
+
+  // Read the hex word
+  read_word (opts, state, dodeca, 12, status_count, analog_signal_array, analog_signal_index);
+  // Read the parity
+  read_golay24_parity (opts, state, parity, status_count, analog_signal_array, analog_signal_index);
+
+#ifdef TDULC_DEBUG
+  printf("[");
+  for (i = 0; i < 12; i++)
+    {
+      printf("%c", (dodeca[i] == 1)? 'X' : ' ');
+    }
+  printf("-");
+  for (i = 0; i < 12; i++)
+    {
+      printf("%c", (parity[i] == 1)? 'X' : ' ');
+    }
+  printf("]");
+#endif
+
+  // Use extended golay to error correct the dodeca word
+  irrecoverable_errors = check_and_fix_golay_24_12(dodeca, parity, &fixed_errors);
+
+  state->debug_header_errors += fixed_errors;
+  if (irrecoverable_errors != 0)
+    {
+      state->debug_header_critical_errors++;
+    }
+
+#ifdef TDULC_DEBUG
+  printf(" -> [");
+  for (i = 0; i < 12; i++)
+    {
+      printf("%c", (dodeca[i] == 1)? 'X' : ' ');
+    }
+  printf("]");
+  if (irrecoverable_errors == 0)
+    {
+      if (fixed_errors > 0)
+        {
+          printf(" fixed!");
+        }
+    }
+  else
+    {
+      printf(" IRRECOVERABLE");
+    }
+  printf("\n");
+#endif
+}
+
+/**
+ * Correct the information in analog_signal_array according with the content of data, which has been
+ * error corrected and should be valid.
+ * \param data A sequence of 12-bit words.
+ * \param count Number of words in the sequence.
+ * \param analog_signal_array Pointer to a sequence of AnalogSignal elements, as many as the value of count.
+ */
+static void
+correct_golay_dibits_12(char* data, int count, AnalogSignal* analog_signal_array)
+{
+  int i, j;
+  int analog_signal_index;
   int dibit;
+  char parity[12];
+
+
+  analog_signal_index = 0;
+
+  for (i=count-1; i>=0; i--)
+    {
+      for (j=0; j<12; j+=2)  // 6 iterations -> 6 dibits
+        {
+          dibit = (data[i*12+j] << 1) | data[i*12+j+1];
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("TDULC data word corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+
+      // Calculate the golay parity for the hex word
+      encode_golay_24_12(data+i*12, parity);
+
+      for (j=0; j<12; j+=2) // 6 iterations -> 6 dibits
+        {
+          dibit = (parity[j] << 1) | parity[j+1];
+          analog_signal_array[analog_signal_index].corrected_dibit = dibit;
+
+#ifdef HEURISTICS_DEBUG
+          if (analog_signal_array[analog_signal_index].dibit != dibit)
+            {
+              printf("TDULC parity corrected from %i to %i, analog value %i\n",
+                      analog_signal_array[analog_signal_index].dibit, dibit, analog_signal_array[analog_signal_index].value);
+            }
+#endif
+
+          analog_signal_index++;
+        }
+    }
+}
+
+void
+read_zeros(dsd_opts* opts, dsd_state* state, AnalogSignal* analog_signal_array, unsigned int length,
+        int* status_count, int new_sequence)
+{
+  char* buffer;
+  unsigned int i;
+  int analog_signal_index;
+
+  analog_signal_index = 0;
+  buffer = malloc(length);
+  read_dibit_update_analog_data (opts, state, buffer, length, status_count, analog_signal_array, &analog_signal_index);
+  free(buffer);
+  if (new_sequence)
+    {
+      analog_signal_array[0].sequence_broken = 1;
+    }
+
+  for (i=0;i<length/2;i++)
+    {
+      analog_signal_array[i].corrected_dibit = 0;
+#ifdef HEURISTICS_DEBUG
+      if (analog_signal_array[i].corrected_dibit != analog_signal_array[i].dibit)
+        {
+          printf("TDULC ending zeros corrected from %i to %i, analog value %i\n",
+                  analog_signal_array[i].dibit, 0, analog_signal_array[i].value);
+        }
+#endif
+    }
+
+  // We know that all these bits should be zero. Use this information for the heuristics module
+  contribute_to_heuristics(state->rf_mod, &(state->p25_heuristics), analog_signal_array, length/2);
+}
+
+void
+processTDULC (dsd_opts* opts, dsd_state* state)
+{
+  int i;
+  char lcinfo[57], lcformat[9], mfid[9];
+
+  int status_count;
+
+  char dodeca_data[6][12];    // Data in 12-bit words. A total of 6 words.
+  char dodeca_parity[6][12];  // Reed-Solomon parity of the data. A total of 6 parity 12-bit words.
+
+  int irrecoverable_errors;
+
+  AnalogSignal analog_signal_array[6*(6+6)+6*(6+6)+10];
+  int analog_signal_index;
+
+
+  analog_signal_index = 0;
+
+  // we skip the status dibits that occur every 36 symbols
+  // the first IMBE frame starts 14 symbols before next status
+  // so we start counter at 36-14-1 = 21
+  status_count = 21;
+
+  for(i=5; i>=0; i--) {
+      read_and_correct_dodeca_word (opts, state, &(dodeca_data[i][0]), &status_count, analog_signal_array, &analog_signal_index);
+  }
+
+  for(i=5; i>=0; i--) {
+      read_and_correct_dodeca_word (opts, state, &(dodeca_parity[i][0]), &status_count, analog_signal_array, &analog_signal_index);
+  }
+
+  // Swap the two 6-bit words to accommodate for the expected word order of the Reed-Solomon decoding
+  swap_hex_words((char*)dodeca_data, (char*)dodeca_parity);
+
+  // Error correct the hex_data using Reed-Solomon hex_parity
+  irrecoverable_errors = check_and_fix_reedsolomon_24_12_13((char*)dodeca_data, (char*)dodeca_parity);
+
+  // Recover the original order
+  swap_hex_words((char*)dodeca_data, (char*)dodeca_parity);
+
+  if (irrecoverable_errors == 1)
+    {
+      state->debug_header_critical_errors++;
+
+      // We can correct (13-1)/2 = 6 errors. If we failed, it means that there were more than 6 errors in
+      // these 12+12 words. But take into account that each hex word was already error corrected with
+      // Golay 24, which can correct 3 bits on each sequence of (12+12) bits. We could say that there were
+      // 7 errors of 4 bits.
+      update_error_stats(&state->p25_heuristics, 12*6+12*6, 7*4);
+
+    }
+  else
+    {
+      // Same comments as in processHDU. See there.
+
+      char fixed_parity[6*12];
+
+      // Correct the dibits that we read according with hex_data values
+      correct_golay_dibits_12((char*)dodeca_data, 6, analog_signal_array);
+
+      // Generate again the Reed-Solomon parity
+      // Now, swap again for Reed-Solomon
+      swap_hex_words((char*)dodeca_data, (char*)dodeca_parity);
+      encode_reedsolomon_24_12_13((char*)dodeca_data, fixed_parity);
+      // Swap again to recover the original order
+      swap_hex_words((char*)dodeca_data, fixed_parity);
+
+      // Correct the dibits that we read according with the fixed parity values
+      correct_golay_dibits_12(fixed_parity, 6, analog_signal_array+6*(6+6));
+
+      // Once corrected, contribute this information to the heuristics module
+      analog_signal_array[0].sequence_broken = 1;
+      contribute_to_heuristics(state->rf_mod, &(state->p25_heuristics), analog_signal_array, 6*(6+6)+6*(6+6));
+  }
+
+
+  // Next 10 dibits should be zeros
+  read_zeros(opts, state, analog_signal_array + 6*(6+6)+6*(6+6), 20, &status_count, 0);
+
+  // Next we should find an status dibit
+  if (status_count != 35) {
+      printf("*** SYNC ERROR\n");
+  }
+
+  // trailing status symbol
+  {
+      int status;
+      status = getDibit (opts, state) + '0';
+      // TODO: do something useful with the status bits...
+  }
+
+  // Put the corrected data into the DSD structures
 
   lcformat[8] = 0;
   mfid[8] = 0;
   lcinfo[56] = 0;
 
-  // Now processing NID
+  lcformat[0] = dodeca_data[5][ 0] + '0';
+  lcformat[1] = dodeca_data[5][ 1] + '0';
+  lcformat[2] = dodeca_data[5][ 2] + '0';
+  lcformat[3] = dodeca_data[5][ 3] + '0';
+  lcformat[4] = dodeca_data[5][ 4] + '0';
+  lcformat[5] = dodeca_data[5][ 5] + '0';
+  lcformat[6] = dodeca_data[5][ 6] + '0';
+  lcformat[7] = dodeca_data[5][ 7] + '0';
+  mfid[0]     = dodeca_data[5][ 8] + '0';
+  mfid[1]     = dodeca_data[5][ 9] + '0';
+  mfid[2]     = dodeca_data[5][10] + '0';
+  mfid[3]     = dodeca_data[5][11] + '0';
 
-  dibit = getDibit (opts, state);
-  lcformat[0] = (1 & (dibit >> 1)) + 48;        // bit 1
-  lcformat[1] = (1 & dibit) + 48;       // bit 0
-  dibit = getDibit (opts, state);
-  lcformat[2] = (1 & (dibit >> 1)) + 48;        // bit 1
-  lcformat[3] = (1 & dibit) + 48;       // bit 0
-  dibit = getDibit (opts, state);
-  lcformat[4] = (1 & (dibit >> 1)) + 48;        // bit 1
-  lcformat[5] = (1 & dibit) + 48;       // bit 0
-  dibit = getDibit (opts, state);
-  lcformat[6] = (1 & (dibit >> 1)) + 48;        // bit 1
-  lcformat[7] = (1 & dibit) + 48;       // bit 0
-  dibit = getDibit (opts, state);
-  mfid[0] = (1 & (dibit >> 1)) + 48;    // bit 1
-  mfid[1] = (1 & dibit) + 48;   // bit 0
-  dibit = getDibit (opts, state);
-  mfid[2] = (1 & (dibit >> 1)) + 48;    // bit 1
-  mfid[3] = (1 & dibit) + 48;   // bit 0
-  skipDibit (opts, state, 6);
+  mfid[4]     = dodeca_data[4][ 0] + '0';
+  mfid[5]     = dodeca_data[4][ 1] + '0';
+  mfid[6]     = dodeca_data[4][ 2] + '0';
+  mfid[7]     = dodeca_data[4][ 3] + '0';
+  lcinfo[0]   = dodeca_data[4][ 4] + '0';
+  lcinfo[1]   = dodeca_data[4][ 5] + '0';
+  lcinfo[2]   = dodeca_data[4][ 6] + '0';
+  lcinfo[3]   = dodeca_data[4][ 7] + '0';
+  lcinfo[4]   = dodeca_data[4][ 8] + '0';
+  lcinfo[5]   = dodeca_data[4][ 9] + '0';
+  lcinfo[6]   = dodeca_data[4][10] + '0';
+  lcinfo[7]   = dodeca_data[4][11] + '0';
 
-  dibit = getDibit (opts, state);
-  mfid[4] = (1 & (dibit >> 1)) + 48;    // bit 1
-  mfid[5] = (1 & dibit) + 48;   // bit 0
-  dibit = getDibit (opts, state);
-  mfid[6] = (1 & (dibit >> 1)) + 48;    // bit 1
-  mfid[7] = (1 & dibit) + 48;   // bit 0
-  skipDibit (opts, state, 1);
-  dibit = getDibit (opts, state);
-  lcinfo[0] = (1 & (dibit >> 1)) + 48;  // bit 1
-  lcinfo[1] = (1 & dibit) + 48; // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[2] = (1 & (dibit >> 1)) + 48;  // bit 1
-  lcinfo[3] = (1 & dibit) + 48; // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[4] = (1 & (dibit >> 1)) + 48;  // bit 1
-  lcinfo[5] = (1 & dibit) + 48; // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[6] = (1 & (dibit >> 1)) + 48;  // bit 1
-  lcinfo[7] = (1 & dibit) + 48; // bit 0
-  skipDibit (opts, state, 6);
+  lcinfo[8]   = dodeca_data[3][ 0] + '0';
+  lcinfo[9]   = dodeca_data[3][ 1] + '0';
+  lcinfo[10]  = dodeca_data[3][ 2] + '0';
+  lcinfo[11]  = dodeca_data[3][ 3] + '0';
+  lcinfo[12]  = dodeca_data[3][ 4] + '0';
+  lcinfo[13]  = dodeca_data[3][ 5] + '0';
+  lcinfo[14]  = dodeca_data[3][ 6] + '0';
+  lcinfo[15]  = dodeca_data[3][ 7] + '0';
+  lcinfo[16]  = dodeca_data[3][ 8] + '0';
+  lcinfo[17]  = dodeca_data[3][ 9] + '0';
+  lcinfo[18]  = dodeca_data[3][10] + '0';
+  lcinfo[19]  = dodeca_data[3][11] + '0';
 
-  dibit = getDibit (opts, state);
-  lcinfo[8] = (1 & (dibit >> 1)) + 48;  // bit 1
-  lcinfo[9] = (1 & dibit) + 48; // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[10] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[11] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[12] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[13] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[14] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[15] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[16] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[17] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[18] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[19] = (1 & dibit) + 48;        // bit 0
-  skipDibit (opts, state, 6);
+  lcinfo[20]  = dodeca_data[2][ 0] + '0';
+  lcinfo[21]  = dodeca_data[2][ 1] + '0';
+  lcinfo[22]  = dodeca_data[2][ 2] + '0';
+  lcinfo[23]  = dodeca_data[2][ 3] + '0';
+  lcinfo[24]  = dodeca_data[2][ 4] + '0';
+  lcinfo[25]  = dodeca_data[2][ 5] + '0';
+  lcinfo[26]  = dodeca_data[2][ 6] + '0';
+  lcinfo[27]  = dodeca_data[2][ 7] + '0';
+  lcinfo[28]  = dodeca_data[2][ 8] + '0';
+  lcinfo[29]  = dodeca_data[2][ 9] + '0';
+  lcinfo[30]  = dodeca_data[2][10] + '0';
+  lcinfo[31]  = dodeca_data[2][11] + '0';
 
-  dibit = getDibit (opts, state);
-  lcinfo[20] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[21] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[22] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[23] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[24] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[25] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[26] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[27] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[28] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[29] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[30] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[31] = (1 & dibit) + 48;        // bit 0
-  skipDibit (opts, state, 6);
+  lcinfo[32]  = dodeca_data[1][ 0] + '0';
+  lcinfo[33]  = dodeca_data[1][ 1] + '0';
+  lcinfo[34]  = dodeca_data[1][ 2] + '0';
+  lcinfo[35]  = dodeca_data[1][ 3] + '0';
+  lcinfo[36]  = dodeca_data[1][ 4] + '0';
+  lcinfo[37]  = dodeca_data[1][ 5] + '0';
+  lcinfo[38]  = dodeca_data[1][ 6] + '0';
+  lcinfo[39]  = dodeca_data[1][ 7] + '0';
+  lcinfo[40]  = dodeca_data[1][ 8] + '0';
+  lcinfo[41]  = dodeca_data[1][ 9] + '0';
+  lcinfo[42]  = dodeca_data[1][10] + '0';
+  lcinfo[43]  = dodeca_data[1][11] + '0';
 
-  dibit = getDibit (opts, state);
-  lcinfo[32] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[33] = (1 & dibit) + 48;        // bit 0
-  skipDibit (opts, state, 1);
-  dibit = getDibit (opts, state);
-  lcinfo[34] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[35] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[36] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[37] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[38] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[39] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[40] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[41] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[42] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[43] = (1 & dibit) + 48;        // bit 0
-  skipDibit (opts, state, 6);
-
-  dibit = getDibit (opts, state);
-  lcinfo[44] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[45] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[46] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[47] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[48] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[49] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[50] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[51] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[52] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[53] = (1 & dibit) + 48;        // bit 0
-  dibit = getDibit (opts, state);
-  lcinfo[54] = (1 & (dibit >> 1)) + 48; // bit 1
-  lcinfo[55] = (1 & dibit) + 48;        // bit 0
-
-  skipDibit (opts, state, 91);
+  lcinfo[44]  = dodeca_data[0][ 0] + '0';
+  lcinfo[45]  = dodeca_data[0][ 1] + '0';
+  lcinfo[46]  = dodeca_data[0][ 2] + '0';
+  lcinfo[47]  = dodeca_data[0][ 3] + '0';
+  lcinfo[48]  = dodeca_data[0][ 4] + '0';
+  lcinfo[49]  = dodeca_data[0][ 5] + '0';
+  lcinfo[50]  = dodeca_data[0][ 6] + '0';
+  lcinfo[51]  = dodeca_data[0][ 7] + '0';
+  lcinfo[52]  = dodeca_data[0][ 8] + '0';
+  lcinfo[53]  = dodeca_data[0][ 9] + '0';
+  lcinfo[54]  = dodeca_data[0][10] + '0';
+  lcinfo[55]  = dodeca_data[0][11] + '0';
 
   processP25lcw (opts, state, lcformat, mfid, lcinfo);
 }
diff --git a/test/golay24-test.cpp b/test/golay24-test.cpp
index 14950de..dbae866 100644
--- a/test/golay24-test.cpp
+++ b/test/golay24-test.cpp
@@ -40,12 +40,12 @@ protected:
   {
       DSDGolay24 golay24;
 
-      unsigned int trashed_codeword = golay24.adapt_to_codeword(hex, sgp);
+      unsigned int trashed_codeword = golay24.adapt_to_codeword(hex, 6, sgp);
 
       //printf("[%s]", hex_to_bin24(trashed_codeword));
 
       int fixed_errors = 0;
-      int many_errors = golay24.decode(hex, sgp, &fixed_errors); // try to correct bit errors
+      int many_errors = golay24.decode_6(hex, sgp, &fixed_errors); // try to correct bit errors
 
       /*
       printf(" -> ");
@@ -72,6 +72,18 @@ protected:
 
       //printf("\n");
   }
+
+  static void check_encode(char* hex, char* expected_parity)
+  {
+      DSDGolay24 golay24;
+      char parity[12];
+
+      golay24.encode_6(hex, parity);
+
+      for(unsigned int i=0; i<12; i++) {
+          EXPECT_EQ(expected_parity[i], parity[i]);
+      }
+  }
 };
 
 TEST_F(Golay24Test, Test1)
@@ -136,3 +148,27 @@ TEST_F(Golay24Test, Test7)
 
     fix_golay24(hex, parity, expected, 2);
 }
+
+TEST_F(Golay24Test, Test_encode_1)
+{
+    char hex[6] = { 1, 0, 1, 0, 1, 0};
+    char expected_parity[12] = { 0, 0, 1, 0, 0, 0, 1, 1, 0, 1, 0, 1};
+
+    check_encode(hex, expected_parity);
+}
+
+TEST_F(Golay24Test, Test_encode_2)
+{
+    char hex[6] = { 0, 0, 1, 0, 0, 0};
+    char expected_parity[12] = { 1, 1, 0, 1, 1, 1, 0, 0, 0, 1, 1, 0};
+
+    check_encode(hex, expected_parity);
+}
+
+TEST_F(Golay24Test, Test_encode_3)
+{
+    char hex[6] = { 0, 0, 0, 0, 0, 0};
+    char expected_parity[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+    check_encode(hex, expected_parity);
+}
diff --git a/test/hamming-test.cpp b/test/hamming-test.cpp
index c842c76..36f2a0f 100644
--- a/test/hamming-test.cpp
+++ b/test/hamming-test.cpp
@@ -30,7 +30,7 @@ protected:
         std::cout << std::endl;
     }
 
-    static void check_hamming(int v, int expected_v, int expected_errors)
+    static void check_decode(int v, int expected_v, int expected_errors)
     {
         Hamming_10_6_3 hamming;
         Hamming_10_6_3_TableImpl hamming_table;
@@ -43,17 +43,17 @@ protected:
         //print_debug(v, output_1, error_count_1);
 
         if (expected_errors != -1) {
-            EXPECT_THAT(expected_errors, error_count_1);
+            EXPECT_EQ(expected_errors, error_count_1);
         }
-        EXPECT_THAT(error_count_1, error_count_2);
+        EXPECT_EQ(error_count_1, error_count_2);
 
         if (expected_v != -1) {
-            EXPECT_THAT(expected_v, output_1);
+            EXPECT_EQ(expected_v, output_1);
         }
-        EXPECT_THAT(output_1, output_2);
+        EXPECT_EQ(output_1, output_2);
     }
 
-    static void check_hamming(Hamming_Inteface* hamming, char* hex, char* parity, int expected_errors, char* expected)
+    static void check_decode(Hamming_Inteface* hamming, char* hex, char* parity, int expected_errors, char* expected)
     {
         int value = Hamming_10_6_3_TableImpl::convert_hex_parity_to_int(hex, parity);
         int error_count = hamming->decode(hex, parity);
@@ -61,12 +61,34 @@ protected:
 
         //print_debug(value, fixed, error_count);
 
-        EXPECT_THAT(expected_errors, error_count);
+        EXPECT_EQ(expected_errors, error_count);
         for (unsigned int i=0; i<6; i++) {
-            EXPECT_THAT(expected[i], hex[i]);
+            EXPECT_EQ(expected[i], hex[i]);
         }
     }
 
+    static void check_encode(Hamming_Inteface* hamming, int input)
+    {
+        int parity = hamming->encode(input);
+
+        // Check it
+        EXPECT_LT(parity, 16);
+        int v = (input << 4) | parity;
+        int output;
+        int error_count = hamming->decode(v, &output);
+        EXPECT_EQ(error_count, 0);
+        EXPECT_EQ(output, input);
+    }
+
+    static void check_encode(Hamming_Inteface* hamming, char* hex)
+    {
+        char parity[4];
+        hamming->encode(hex, parity);
+
+        // Check it
+        int error_count = hamming->decode(hex, parity);
+        EXPECT_EQ(error_count, 0);
+    }
 };
 
 TEST_F(HammingTest, Test1)
@@ -74,7 +96,7 @@ TEST_F(HammingTest, Test1)
     // OK
     std::bitset<10>    value(std::string("1010100110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 0);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 0);
 }
 
 TEST_F(HammingTest, Test2)
@@ -82,7 +104,7 @@ TEST_F(HammingTest, Test2)
     // Error on parity bit 0 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010100111"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test3)
@@ -90,7 +112,7 @@ TEST_F(HammingTest, Test3)
     // Error on parity bit 1 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010100100"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test4)
@@ -98,7 +120,7 @@ TEST_F(HammingTest, Test4)
     // Error on parity bit 2 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010100010"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test5)
@@ -106,7 +128,7 @@ TEST_F(HammingTest, Test5)
     // Error on parity bit 3 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010101110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test6)
@@ -114,7 +136,7 @@ TEST_F(HammingTest, Test6)
     // Error on bit 4 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010110110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test7)
@@ -122,7 +144,7 @@ TEST_F(HammingTest, Test7)
     // Error on bit 5 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1010000110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test8)
@@ -130,7 +152,7 @@ TEST_F(HammingTest, Test8)
     // Error on bit 6 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1011100110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test9)
@@ -138,7 +160,7 @@ TEST_F(HammingTest, Test9)
     // Error on bit 7 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1000100110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test10)
@@ -146,7 +168,7 @@ TEST_F(HammingTest, Test10)
     // Error on bit 8 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("1110100110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test11)
@@ -154,7 +176,7 @@ TEST_F(HammingTest, Test11)
     // Error on bit 9 (9 is the left-most and 0 is the right-most)
     std::bitset<10>    value(std::string("0010100110"));
     std::bitset< 6> expected(std::string("101010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test12)
@@ -162,7 +184,7 @@ TEST_F(HammingTest, Test12)
     // Two errors
     std::bitset<10>    value(std::string("0010100111"));
     std::bitset< 6> expected(std::string("001010"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 2);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 2);
 }
 
 TEST_F(HammingTest, Test13)
@@ -170,7 +192,7 @@ TEST_F(HammingTest, Test13)
     // Two errors, in this case they go undetected, thinks it's a parity error
     std::bitset<10>    value(std::string("1011000110"));
     std::bitset< 6> expected(std::string("101100"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 1);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 1);
 }
 
 TEST_F(HammingTest, Test14)
@@ -178,14 +200,14 @@ TEST_F(HammingTest, Test14)
     // OK
     std::bitset<10>    value(std::string("0001000111"));
     std::bitset< 6> expected(std::string("000100"));
-    check_hamming((int)value.to_ulong(), (int)expected.to_ulong(), 0);
+    check_decode((int)value.to_ulong(), (int)expected.to_ulong(), 0);
 }
 
 TEST_F(HammingTest, TestEveryValue)
 {
     // Test every posible value
     for (int v=0; v<1023; v++) {
-        check_hamming(v, -1, -1);
+        check_decode(v, -1, -1);
     }
 }
 
@@ -196,10 +218,10 @@ TEST_F(HammingTest, Test_hex_parity_interface_no_errors)
     char parity[4] = {1,0,1,1};
 
     Hamming_10_6_3 hamming;
-    check_hamming(&hamming, hex, parity, 0, fixed);
+    check_decode(&hamming, hex, parity, 0, fixed);
 
     Hamming_10_6_3_TableImpl hamming_table;
-    check_hamming(&hamming_table, hex, parity, 0, fixed);
+    check_decode(&hamming_table, hex, parity, 0, fixed);
 }
 
 TEST_F(HammingTest, Test_hex_parity_interface_one_error_1)
@@ -210,11 +232,11 @@ TEST_F(HammingTest, Test_hex_parity_interface_one_error_1)
     char parity[4] = {1,0,1,1};
 
     Hamming_10_6_3 hamming;
-    check_hamming(&hamming, hex_1, parity, 1, fixed);
+    check_decode(&hamming, hex_1, parity, 1, fixed);
     // hex is modified in place
 
     Hamming_10_6_3_TableImpl hamming_table;
-    check_hamming(&hamming_table, hex_2, parity, 1, fixed);
+    check_decode(&hamming_table, hex_2, parity, 1, fixed);
     // hex is modified in place
 }
 
@@ -226,11 +248,11 @@ TEST_F(HammingTest, Test_hex_parity_interface_one_error_2)
     char parity[4] = {0,0,0,0};
 
     Hamming_10_6_3 hamming;
-    check_hamming(&hamming, hex_1, parity, 1, fixed);
+    check_decode(&hamming, hex_1, parity, 1, fixed);
     // hex is modified in place
 
     Hamming_10_6_3_TableImpl hamming_table;
-    check_hamming(&hamming_table, hex_2, parity, 1, fixed);
+    check_decode(&hamming_table, hex_2, parity, 1, fixed);
     // hex is modified in place
 }
 
@@ -242,10 +264,41 @@ TEST_F(HammingTest, Test_hex_parity_interface_one_error_3)
     char parity[4] = {0,0,0,0};
 
     Hamming_10_6_3 hamming;
-    check_hamming(&hamming, hex_1, parity, 1, fixed);
+    check_decode(&hamming, hex_1, parity, 1, fixed);
     // hex is modified in place
 
     Hamming_10_6_3_TableImpl hamming_table;
-    check_hamming(&hamming_table, hex_2, parity, 1, fixed);
+    check_decode(&hamming_table, hex_2, parity, 1, fixed);
     // hex is modified in place
 }
+
+TEST_F(HammingTest, Test_encoding_1)
+{
+    Hamming_10_6_3 hamming;
+    Hamming_10_6_3_TableImpl hamming_table;
+
+    for (int i=0; i<64; i++) {
+        check_encode(&hamming, i);
+        check_encode(&hamming_table, i);
+    }
+}
+
+TEST_F(HammingTest, Test_encoding_char)
+{
+    Hamming_10_6_3 hamming;
+    Hamming_10_6_3_TableImpl hamming_table;
+
+    char hex[6];
+
+    for (int i=0; i<64; i++) {
+
+        // Convert i to char array
+        int input = i;
+        for (int j=5; j>=0; j--) {
+            hex[j] = input & 1;
+            input >>= 1;
+        }
+        check_encode(&hamming, hex);
+        check_encode(&hamming_table, hex);
+    }
+}
diff --git a/test/reedsolomon-test.cpp b/test/reedsolomon-test.cpp
index 3bf6786..ad5b9d6 100644
--- a/test/reedsolomon-test.cpp
+++ b/test/reedsolomon-test.cpp
@@ -7,19 +7,9 @@
 
 #include "ReedSolomon.hpp"
 
-static void byte_to_binary(char* b, int x)
-{
-    int z, i;
-
-    for (z = 32, i=0; z > 0; z >>= 1, i++) {
-        b[i] = ((x & z) != 0) ? 'X' : ' ';
-    }
-    b[6] = 0;
-}
-
 TEST(ReedSolomonTest, Test1)
 {
-    ReedSolomon_63_47_17 rs;
+    ReedSolomon_63<8> rs;
     int recd[63] = {};
     int expected[63] = {};
     int output[63];
@@ -90,3 +80,68 @@ TEST(ReedSolomonTest, Test1)
 
     EXPECT_THAT(expected, testing::ElementsAreArray(output, 63));
 }
+
+static void generate_random_bits(char* p, unsigned int count)
+{
+    for (unsigned int i=0; i<count; i++) {
+        p[i] = (rand() > RAND_MAX/2)? 1 : 0;
+    }
+}
+
+TEST(ReedSolomonTest, Test_encode_24_12_13)
+{
+    char hex[12*6];
+    char parity[12*6];
+
+    DSDReedSolomon_24_12_13 rs;
+
+    for (unsigned i=0; i<1000; i++) {
+        generate_random_bits((char*)hex, 12*6);
+
+        // Make a copy
+        char expected[12*6];
+
+        memcpy(expected, hex, 12*6);
+
+
+
+        // Encode
+        rs.encode(hex, parity);
+
+        // Decode back, should have the same result and no errors
+        int irrecoverable_errors = rs.decode(hex, parity);
+
+        EXPECT_EQ(0, irrecoverable_errors);
+
+        EXPECT_THAT(expected, testing::ElementsAreArray(hex, 12*6));
+    }
+}
+
+TEST(ReedSolomonTest, Test_encode_24_16_9)
+{
+    char hex[16*6];
+    char parity[8*6];
+
+    DSDReedSolomon_24_16_9 rs;
+
+    for (unsigned i=0; i<1000; i++) {
+        generate_random_bits((char*)hex, 16*6);
+
+        // Make a copy
+        char expected[16*6];
+
+        memcpy(expected, hex, 16*6);
+
+
+
+        // Encode
+        rs.encode(hex, parity);
+
+        // Decode back, should have the same result and no errors
+        int irrecoverable_errors = rs.decode(hex, parity);
+
+        EXPECT_EQ(0, irrecoverable_errors);
+
+        EXPECT_THAT(expected, testing::ElementsAreArray(hex, 16*6));
+    }
+}