/* UA1ZBE Custom Firmware - BCH(31,21) Decoder for POCSAG
 *
 * POCSAG uses a shortened BCH(31,21) code:
 * - 31-bit codeword (n=31)
 * - 21 data bits (k=21)
 * - 10 parity/check bits (n-k=10)
 * - Can correct up to 2 bit errors per codeword
 *
 * Generator polynomial: g(x) = x^10 + x^9 + x^8 + x^6 + x^5 + x^3 + 1
 *                        = 0x72D (binary: 111 0010 1101)
 *
 * The full 32-bit word includes:
 * - Bit 31: even parity bit (bit 0 of the word after inversion)
 * - Bits 30-0: 31-bit BCH codeword
 *
 * This implementation avoids hardware division (Cortex-M0 has no DIV unit).
 * All operations use shifts and XOR (GF(2) arithmetic).
 */

#include "pocsag.h"
#include <stdint.h>
#include <stdbool.h>

/* Generator polynomial for BCH(31,21): x^10 + x^9 + x^8 + x^6 + x^5 + x^3 + 1 */
#define BCH31_GEN_POLY  0x72DU  /* 11100101101 binary */

/* POCSAG sync word (32-bit) */
#define POCSAG_SYNC_WORD  0x7CD21538U

/* POCSAG idle word */
#define POCSAG_IDLE_WORD  0x7A89C197U

/* Parity check mask for even parity */
#define BCH31_PARITY_MASK 0x80000000U

/*
 * Calculate syndrome of a 31-bit BCH codeword.
 * The syndrome is the remainder of dividing the received word by g(x).
 * If syndrome == 0, the word is valid (no errors or undetectable errors).
 *
 * word: 31-bit codeword (bits 30:0, parity bit excluded)
 * Returns: 10-bit syndrome value
 */
static uint16_t bch31_syndrome(uint32_t word)
{
    uint32_t reg = word & 0x7FFFFFFFU;  /* Mask to 31 bits */
    int i;

    /* Polynomial division in GF(2) using shift-and-XOR */
    /* We process from MSB to LSB, XORing with generator when MSB is 1 */
    for (i = 30; i >= 10; i--) {
        if (reg & ((uint32_t)1 << i)) {
            reg ^= (BCH31_GEN_POLY << (i - 10));
        }
    }

    /* The remainder is in the lower 10 bits */
    return (uint16_t)(reg & 0x03FFU);
}

/*
 * Check even parity of a 32-bit word.
 * Returns true if parity is correct (even number of 1-bits).
 */
static bool bch31_check_parity(uint32_t word)
{
    /* Count set bits using a lookup-free method (no division needed) */
    uint32_t v = word;
    v = v - ((v >> 1) & 0x55555555U);
    v = (v & 0x33333333U) + ((v >> 2) & 0x33333333U);
    v = (v + (v >> 4)) & 0x0F0F0F0FU;
    v = (v * 0x01010101U) >> 24;  /* Sum of all bytes */

    return (v & 1U) == 0;  /* Even parity = even number of 1-bits */
}

/*
 * Find error position from syndrome.
 * For single-bit errors, the syndrome directly maps to the error position.
 * For double-bit errors, we need more complex correction.
 *
 * syndrome: 10-bit syndrome value
 * Returns: bit position (0-30) if single error, 0 if no error,
 *          or a special value for double errors.
 */
static int bch31_find_single_error(uint16_t syndrome)
{
    if (syndrome == 0)
        return -1;  /* No error */

    /* For single-bit errors, syndrome = x^i mod g(x) for error at position i.
     * We try each position by computing the expected syndrome.
     * This avoids division — just shift and XOR. */
    uint32_t test_syn = 1;  /* Start with x^0 mod g(x) = 1 */

    for (int i = 0; i < 31; i++) {
        if (test_syn == syndrome)
            return i;  /* Error at position i */

        /* Multiply by x in GF(2^10 / g(x)):
         * Shift left; if bit 10 is set, XOR with generator */
        test_syn <<= 1;
        if (test_syn & 0x0400U) {  /* Bit 10 set */
            test_syn ^= BCH31_GEN_POLY;
        }
        test_syn &= 0x03FFU;  /* Keep 10 bits */
    }

    return -2;  /* Not a single-bit error */
}

/*
 * Attempt to correct double-bit errors using syndrome decoding.
 * For a (31,21) BCH code with d_min=5, we can correct up to 2 errors.
 *
 * This uses a simplified approach: try all pairs of error positions.
 * For performance on Cortex-M0, we use a precomputed approach.
 *
 * word: pointer to the 32-bit word (will be modified in place if corrected)
 * Returns: 0 = no error, 1 = single error corrected,
 *          2 = double error corrected, -1 = uncorrectable
 */
int bch31_correct(uint32_t *word)
{
    uint32_t data = *word;

    /* Step 1: Check parity */
    bool parity_ok = bch31_check_parity(data);

    /* Extract 31-bit codeword (strip parity bit 31) */
    uint32_t codeword = data & 0x7FFFFFFFU;

    /* Step 2: Calculate syndrome */
    uint16_t syn = bch31_syndrome(codeword);

    if (syn == 0 && parity_ok) {
        /* No errors detected */
        return 0;
    }

    /* Step 3: Try single-bit error correction */
    int err_pos = bch31_find_single_error(syn);
    if (err_pos >= 0) {
        /* Single-bit error at position err_pos */
        codeword ^= ((uint32_t)1 << err_pos);
        /* Fix parity bit too */
        *word = codeword | ((uint32_t)bch31_check_parity(codeword) << 31);
        return 1;
    }

    /* Step 4: Try double-bit error correction
     *
     * For double errors at positions i and j:
     * syndrome S = x^i + x^j (mod g(x))
     *
     * We use a brute-force search over all pairs (i, j) where i > j.
     * For 31 bits, this is 31*30/2 = 465 pairs — acceptable.
     */
    if (err_pos == -2) {
        /* Precompute all single-error syndromes */
        uint16_t single_syn[31];
        uint32_t test = 1;
        for (int i = 0; i < 31; i++) {
            single_syn[i] = (uint16_t)test;
            test <<= 1;
            if (test & 0x0400U)
                test ^= BCH31_GEN_POLY;
            test &= 0x03FFU;
        }

        /* Search for pair (i, j) where syn_i XOR syn_j == syn */
        for (int i = 1; i < 31; i++) {
            for (int j = 0; j < i; j++) {
                if ((single_syn[i] ^ single_syn[j]) == syn) {
                    /* Found double error at positions i and j */
                    codeword ^= ((uint32_t)1 << i);
                    codeword ^= ((uint32_t)1 << j);
                    *word = codeword | ((uint32_t)bch31_check_parity(codeword) << 31);
                    return 2;
                }
            }
        }
    }

    /* Step 5: If syndrome != 0 but we couldn't find error positions,
     * check if it might still be valid (parity might catch it).
     * POCSAG spec says words with uncorrectable errors should be discarded.
     */
    return -1;  /* Uncorrectable */
}

/*
 * Extract 21 data bits from a corrected 32-bit POCSAG word.
 * Returns the 21-bit data value.
 */
uint32_t bch31_get_data(uint32_t word)
{
    /* Data bits are in positions 30:10 of the 31-bit codeword
     * (bit 31 is parity, bits 9:0 are check bits)
     * So data = bits [30:10] = (word >> 10) & 0x1FFFFF */
    return (word >> 10) & 0x001FFFFFU;
}

/*
 * Get function bits from a POCSAG address word.
 * Address words have function code in bits 11:10 of the data portion.
 */
uint8_t bch31_get_func(uint32_t word)
{
    /* Function bits are data bits [11:10] = bits [21:20] of full word */
    return (uint8_t)((word >> 20) & 0x03U);
}

/*
 * Check if a word is a sync word.
 */
bool bch31_is_sync(uint32_t word)
{
    return word == POCSAG_SYNC_WORD;
}

/*
 * Check if a word is an idle word.
 */
bool bch31_is_idle(uint32_t word)
{
    return word == POCSAG_IDLE_WORD;
}