/* UA1ZBE Custom Firmware - POCSAG Decoder Implementation
 *
 * Implements POCSAG decoding from FM discriminator audio samples.
 * Uses zero-crossing detection for bit slicing.
 *
 * Signal chain:
 *   BK4819 FM demod -> AF output -> ADC sampling
 *   -> zero-crossing detection -> bit stream -> POCSAG decode
 *
 * Cortex-M0 optimized: no hardware division in hot path.
 * All memory statically allocated.
 */

#include "pocsag.h"
#include "bch31.h"
#include "driver/bk4819.h"
#include "driver/system.h"
#include "driver/st7565.h"
#include "misc.h"
#include "radio.h"
#include <string.h>

/* === Configuration === */

/* Sampling rate for audio (Hz) — must be much higher than max baud rate */
#define POCSAG_SAMPLE_RATE  8000  /* 8 kHz — sufficient for 1200 baud */

/* Threshold for zero-crossing detection (ADC counts) */
#define POCSAG_ZERO_THRESHOLD  2048  /* Midpoint of 12-bit ADC range (0-4095) */

/* Minimum preamble bits to detect (POCSAG spec: 576 bits of 0x55 + 1) */
#define POCSAG_PREAMBLE_MIN_BITS  576

/* BPSK bit extraction: compare sample against threshold */
#define IS_BIT_ONE(sample)  ((sample) > POCSAG_ZERO_THRESHOLD)

/* === Global decoder instance === */
pocsag_decoder_t gPocsag;

/* === Internal state for bit slicing === */

/* Samples since last bit decision */
static uint16_t s_sample_counter;

/* Samples per bit for current baud rate */
static uint16_t s_samples_per_bit;

/* Accumulated samples for current bit decision */
static uint32_t s_bit_accumulator;
static uint8_t  s_bit_sample_count;

/* Preamble detection: count alternating bits */
static uint16_t s_preamble_alt_count;
static uint8_t s_last_bit;

/* Sync detection shift register (holds last 32 bits) */
static uint32_t s_sync_reg;

/* Bit buffer for assembling 32-bit words */
static uint32_t s_word_reg;
static uint8_t  s_word_bits;

/* Batch tracking */
static uint8_t s_batch_words;

/* Address/message assembly */
static uint32_t s_msg_address;
static uint8_t  s_msg_func;
static uint8_t  s_msg_buf[POCSAG_MSG_MAX_LEN];
static uint8_t  s_msg_len;
static bool     s_msg_is_alpha;

/* Alpha decoder state */
static uint32_t s_alpha_bits;
static uint8_t  s_alpha_bit_count;

/* State */
static pocsag_state_t s_state;

/* Previous save state for restoration */
static uint16_t s_saved_reg_30;
static uint16_t s_saved_reg_47;
static uint16_t s_saved_reg_33;

/* Timing counter — increments each time FeedSample is called (every ~10ms) */
static uint32_t s_tick_counter;

/* Last sync timestamp */
static uint32_t s_last_sync_tick;

/* === Baud rate lookup === */

/* Samples per bit for supported baud rates at 8kHz sample rate */
static uint16_t get_samples_per_bit(uint32_t baud)
{
    /* Avoid division: use precomputed values
     * 8000 / 512  = 15.625 -> 16
     * 8000 / 1200 = 6.667  -> 7 */
    if (baud == POCSAG_BAUD_512)
        return 16;
    if (baud == POCSAG_BAUD_1200)
        return 7;
    return 7;  /* Default to 1200 */
}

/* === Message ring buffer === */

static void msg_push(const pocsag_msg_t *msg)
{
    if (gPocsag.msg_count >= POCSAG_MSG_POOL_SIZE) {
        /* Buffer full — drop oldest */
        gPocsag.msg_read_idx = (gPocsag.msg_read_idx + 1) % POCSAG_MSG_POOL_SIZE;
        gPocsag.msg_count--;
    }

    gPocsag.messages[gPocsag.msg_write_idx] = *msg;
    gPocsag.msg_write_idx = (gPocsag.msg_write_idx + 1) % POCSAG_MSG_POOL_SIZE;
    gPocsag.msg_count++;
    gPocsag.msg_available = true;
}

/* === POCSAG word processing === */

static void process_pocsag_word(uint32_t word)
{
    /* Check parity first — quick reject using bit count */
    uint32_t v = word;
    v = v - ((v >> 1) & 0x55555555U);
    v = (v & 0x33333333U) + ((v >> 2) & 0x33333333U);
    v = (v + (v >> 4)) & 0x0F0F0F0FU;
    v = (v * 0x01010101U) >> 24;
    bool parity_ok = (v & 1U) == 0;

    /* Try BCH correction */
    int corr = bch31_correct(&word);

    if (corr < 0 && !parity_ok) {
        /* Uncorrectable — discard */
        gPocsag.uncorrectable_errors++;
        if (s_state == POCSAG_STATE_DATA) {
            s_state = POCSAG_STATE_SYNC;
            s_batch_words = 0;
        }
        return;
    }

    if (corr > 0)
        gPocsag.corrected_errors++;

    gPocsag.total_words++;

    /* Check for sync word */
    if (bch31_is_sync(word)) {
        s_batch_words = 0;
        s_last_sync_tick = s_tick_counter;
        return;
    }

    /* Check for idle word */
    if (bch31_is_idle(word)) {
        return;  /* Skip idle words */
    }

    /* In DATA state, process the word */
    if (s_state != POCSAG_STATE_DATA) {
        s_state = POCSAG_STATE_DATA;
        s_batch_words = 0;
    }

    /* Even/odd position in batch determines address vs data */
    if (s_batch_words % 2 == 0) {
        /* Address word */
        s_msg_address = bch31_get_data(word);
        s_msg_func = bch31_get_func(word);
        s_msg_len = 0;
        s_msg_is_alpha = false;
        s_msg_buf[0] = '\0';
        /* Reset alpha decoder state */
        s_alpha_bits = 0;
        s_alpha_bit_count = 0;
    } else {
        /* Data word */
        uint32_t data = bch31_get_data(word);

        if (s_msg_func == 0) {
            /* Numeric pager — data is BCD digits */
            uint32_t num = data & 0x000FFFFFU;
            char digits[12];
            int idx = 0;

            if (num == 0) {
                digits[idx++] = '0';
            } else {
                /* Extract digits using precomputed powers of 10 (no division) */
                static const uint32_t powers[] = {
                    1000000000U, 100000000U, 10000000U, 1000000U,
                    100000U, 10000U, 1000U, 100U, 10U, 1U
                };
                bool started = false;
                for (int p = 0; p < 10; p++) {
                    uint32_t pwr = powers[p];
                    uint8_t digit = 0;
                    while (num >= pwr) {
                        num -= pwr;
                        digit++;
                    }
                    if (digit > 0 || started || p == 9) {
                        digits[idx++] = '0' + digit;
                        started = true;
                    }
                }
            }
            digits[idx] = '\0';

            /* Push message */
            if (s_msg_len == 0) {
                pocsag_msg_t msg;
                memset(&msg, 0, sizeof(msg));
                msg.address = s_msg_address;
                msg.func = s_msg_func;
                msg.is_alpha = false;
                msg.timestamp = s_tick_counter;

                int copy_len = idx;
                if (copy_len > POCSAG_MSG_MAX_LEN - 1)
                    copy_len = POCSAG_MSG_MAX_LEN - 1;
                memcpy(msg.text, digits, copy_len);
                msg.text[copy_len] = '\0';
                msg.text_len = copy_len;

                msg_push(&msg);
            }
        } else if (s_msg_func == 2) {
            /* Alpha pager — 7-bit ASCII, 20 bits per word */
            uint32_t bits20 = data & 0x000FFFFFU;

            s_alpha_bits = (s_alpha_bits << 20) | bits20;
            s_alpha_bit_count += 20;

            while (s_alpha_bit_count >= 7 && s_msg_len < POCSAG_MSG_MAX_LEN - 1) {
                s_alpha_bit_count -= 7;
                uint8_t ch = (s_alpha_bits >> s_alpha_bit_count) & 0x7F;

                if (ch == 0x03) {
                    /* ETX — end of message */
                    s_msg_buf[s_msg_len] = '\0';
                    pocsag_msg_t msg;
                    memset(&msg, 0, sizeof(msg));
                    msg.address = s_msg_address;
                    msg.func = s_msg_func;
                    msg.is_alpha = true;
                    msg.timestamp = s_tick_counter;
                    memcpy(msg.text, s_msg_buf, s_msg_len);
                    msg.text[s_msg_len] = '\0';
                    msg.text_len = s_msg_len;
                    msg_push(&msg);

                    s_msg_len = 0;
                    s_alpha_bits = 0;
                    s_alpha_bit_count = 0;
                    break;
                }
                s_msg_buf[s_msg_len++] = ch;
            }
            s_msg_is_alpha = true;
        }
    }

    s_batch_words++;

    /* After 32 words, expect new sync */
    if (s_batch_words >= 32) {
        s_state = POCSAG_STATE_SYNC;
        s_batch_words = 0;
    }
}

/* === Bit processing === */

static void process_bit(bool bit)
{
    gPocsag.total_bits++;

    switch (s_state) {
    case POCSAG_STATE_IDLE:
    case POCSAG_STATE_PREAMBLE:
        s_state = POCSAG_STATE_PREAMBLE;

        /* Look for alternating pattern: 01010101... (0x55) ending with 1 (0xAB) */
        if (s_preamble_alt_count == 0) {
            s_last_bit = bit ? 1 : 0;
            s_preamble_alt_count = 1;
        } else if ((bit ? 1 : 0) != s_last_bit) {
            s_preamble_alt_count++;
            s_last_bit = bit ? 1 : 0;
        } else {
            s_preamble_alt_count = 1;
            s_last_bit = bit ? 1 : 0;
        }

        if (s_preamble_alt_count >= POCSAG_PREAMBLE_MIN_BITS) {
            /* Preamble detected — look for sync word */
            s_state = POCSAG_STATE_SYNC;
            s_sync_reg = 0;
            s_word_bits = 0;
            s_preamble_alt_count = 0;
            s_last_sync_tick = s_tick_counter;
        }
        break;

    case POCSAG_STATE_SYNC:
        /* Collect bits into 32-bit sync register */
        s_sync_reg = (s_sync_reg << 1) | (bit ? 1 : 0);

        if (s_sync_reg == 0x7CD21538U) {
            /* Sync word found! */
            s_state = POCSAG_STATE_DATA;
            s_word_reg = 0;
            s_word_bits = 0;
            s_batch_words = 0;
            gPocsag.signal_detected = true;
            s_last_sync_tick = s_tick_counter;
        }

        /* Timeout: ~1000 bits at 1200 baud ≈ 833ms ≈ 83 calls */
        if ((s_tick_counter - s_last_sync_tick) > 100) {
            s_state = POCSAG_STATE_IDLE;
            s_preamble_alt_count = 0;
            gPocsag.signal_detected = false;
        }
        break;

    case POCSAG_STATE_DATA:
        /* Collect 32-bit words */
        s_word_reg = (s_word_reg << 1) | (bit ? 1 : 0);
        s_word_bits++;

        if (s_word_bits >= 32) {
            process_pocsag_word(s_word_reg);
            s_word_reg = 0;
            s_word_bits = 0;
        }

        /* Sync loss check */
        if ((s_tick_counter - s_last_sync_tick) > 300) {
            s_state = POCSAG_STATE_SYNC;
            s_sync_reg = 0;
            gPocsag.signal_detected = false;
        }
        break;
    }
}

/* === Sample processing (hot path) === */

bool POCSAG_FeedSample(uint16_t audio_sample)
{
    s_tick_counter++;
    s_sample_counter++;

    /* Accumulate samples for bit decision */
    s_bit_accumulator += audio_sample;
    s_bit_sample_count++;

    if (s_bit_sample_count >= s_samples_per_bit) {
        /* Make bit decision from averaged sample.
         * Avoid division: compare accumulated value against threshold * count.
         *
         * For 1200 baud (7 samples): threshold * 7 = 2048 * 7 = 14336
         * For 512 baud  (16 samples): threshold * 16 = 2048 * 16 = 32768 */
        uint32_t thresh;
        if (gPocsag.baud_rate == POCSAG_BAUD_512)
            thresh = 32768;
        else
            thresh = 14336;

        bool bit = (s_bit_accumulator >= thresh);

        s_bit_accumulator = 0;
        s_bit_sample_count = 0;

        /* Process the bit */
        process_bit(bit);
        return true;
    }

    return false;
}

void POCSAG_FeedBit(bool bit)
{
    process_bit(bit);
}

/* === Public API === */

void POCSAG_Init(uint32_t baud_rate)
{
    memset(&gPocsag, 0, sizeof(gPocsag));
    gPocsag.baud_rate = baud_rate;
    gPocsag.state = POCSAG_STATE_IDLE;
    gPocsag.msg_read_idx = 0;
    gPocsag.msg_write_idx = 0;
    gPocsag.msg_count = 0;
    gPocsag.msg_available = false;

    s_sample_counter = 0;
    s_samples_per_bit = get_samples_per_bit(baud_rate);
    s_bit_accumulator = 0;
    s_bit_sample_count = 0;
    s_preamble_alt_count = 0;
    s_last_bit = 0;
    s_sync_reg = 0;
    s_word_reg = 0;
    s_word_bits = 0;
    s_batch_words = 0;
    s_msg_address = 0;
    s_msg_func = 0;
    s_msg_len = 0;
    s_msg_is_alpha = false;
    s_alpha_bits = 0;
    s_alpha_bit_count = 0;
    s_state = POCSAG_STATE_IDLE;
    s_tick_counter = 0;
    s_last_sync_tick = 0;
}

void POCSAG_SwitchBaud(uint32_t baud_rate)
{
    gPocsag.baud_rate = baud_rate;
    s_samples_per_bit = get_samples_per_bit(baud_rate);
    s_state = POCSAG_STATE_IDLE;
    s_preamble_alt_count = 0;
    s_sync_reg = 0;
    s_word_reg = 0;
    s_word_bits = 0;
    s_batch_words = 0;
    gPocsag.signal_detected = false;
}

uint32_t POCSAG_GetBaud(void)
{
    return gPocsag.baud_rate;
}

bool POCSAG_MessageAvailable(void)
{
    return gPocsag.msg_count > 0;
}

bool POCSAG_GetMessage(pocsag_msg_t *msg)
{
    if (gPocsag.msg_count == 0)
        return false;

    *msg = gPocsag.messages[gPocsag.msg_read_idx];
    gPocsag.msg_read_idx = (gPocsag.msg_read_idx + 1) % POCSAG_MSG_POOL_SIZE;
    gPocsag.msg_count--;

    if (gPocsag.msg_count == 0)
        gPocsag.msg_available = false;

    return true;
}

pocsag_state_t POCSAG_GetState(void)
{
    return s_state;
}

bool POCSAG_SignalDetected(void)
{
    return gPocsag.signal_detected;
}

void POCSAG_Reset(void)
{
    uint32_t baud = gPocsag.baud_rate;
    POCSAG_Init(baud);
}

void POCSAG_Process(void)
{
    gPocsag.state = s_state;
}

/* === Radio configuration === */

void POCSAG_ConfigureRadio(uint32_t frequency_hz)
{
    /* Save current radio state */
    s_saved_reg_30 = BK4819_ReadRegister(BK4819_REG_30);
    s_saved_reg_47 = BK4819_ReadRegister(BK4819_REG_47);
    s_saved_reg_33 = BK4819_ReadRegister(BK4819_REG_33);

    /* Set frequency */
    BK4819_SetFrequency(frequency_hz);
    BK4819_PickRXFilterPathBasedOnFrequency(frequency_hz);

    /* Configure for narrow FM (POCSAG is typically narrow FM) */
    BK4819_SetFilterBandwidth(BK4819_FILTER_BW_NARROW, true);

    /* Turn on RX */
    BK4819_RX_TurnOn();

    /* Set AF output to FM (demodulated audio) for sampling */
    BK4819_SetAF(BK4819_AF_FM);

    /* Disable squelch for continuous audio monitoring */
    BK4819_SetupSquelch(0, 0, 0, 0, 255, 0);

    /* Enable RX link, AF DAC, disc mode */
    BK4819_WriteRegister(BK4819_REG_30,
        BK4819_REG_30_ENABLE_VCO_CALIB |
        BK4819_REG_30_ENABLE_RX_LINK |
        BK4819_REG_30_ENABLE_AF_DAC |
        BK4819_REG_30_ENABLE_DISC_MODE |
        BK4819_REG_30_ENABLE_PLL_VCO |
        BK4819_REG_30_ENABLE_RX_DSP);

    /* Enable green LED */
    BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2_GREEN, true);
}

void POCSAG_Stop(void)
{
    BK4819_WriteRegister(BK4819_REG_30, s_saved_reg_30);
    BK4819_WriteRegister(BK4819_REG_47, s_saved_reg_47);
    BK4819_WriteRegister(BK4819_REG_33, s_saved_reg_33);

    gPocsag.signal_detected = false;
    s_state = POCSAG_STATE_IDLE;
}

/* === State description strings === */

const char *POCSAG_StateString(pocsag_state_t state)
{
    static const char *const strs[] = { "IDLE", "SEARCH", "SYNC", "DATA" };
    if ((unsigned int)state > 3u) return "???";
    return strs[(unsigned int)state];
}