audiocodec/decoder.c

//
// Created by bruno on 13. 7. 2026.
//

#include "decoder.h"

#include <SDL.h>
#include <SDL_audio.h>
#include <SDL_stdinc.h>
#include <math.h>
#include <stdio.h>
#include <string.h>

#include "texteditor.h"

Decoder decoder;

float tone_energy(
    Decoder *dec,
    int index) {
    return goertzel_energy(
        &dec->tones[index]
    );
}

static void decoder_reset_symbol_filters(Decoder *dec) {
    const int tones = dec->bitCount * 2 + 2;

    for (int i = 0; i < tones; ++i) {
        dec->tones[i].s1 = 0.0f;
        dec->tones[i].s2 = 0.0f;
    }
}

static void decoder_history_push(Decoder *dec, float sample) {
    dec->sampleHistory[dec->sampleHistoryWrite] = sample;
    dec->sampleHistoryWrite =
        (dec->sampleHistoryWrite + 1) % DECODER_SAMPLE_HISTORY;

    if (dec->sampleHistoryCount < DECODER_SAMPLE_HISTORY)
        dec->sampleHistoryCount++;

    dec->totalSamples++;
}

static bool decoder_history_get(
    const Decoder *dec,
    uint64_t sampleIndex,
    float *sample)
{
    if (sampleIndex >= dec->totalSamples)
        return false;

    uint64_t oldest =
        dec->totalSamples - dec->sampleHistoryCount;

    if (sampleIndex < oldest)
        return false;

    uint32_t start =
        (dec->sampleHistoryWrite + DECODER_SAMPLE_HISTORY - dec->sampleHistoryCount)
        % DECODER_SAMPLE_HISTORY;

    uint32_t rel =
        (uint32_t)(sampleIndex - oldest);

    *sample =
        dec->sampleHistory[(start + rel) % DECODER_SAMPLE_HISTORY];

    return true;
}

static uint8_t decode_symbol_from_work(
    Goertzel *tones,
    uint8_t bitCount)
{
    uint8_t byte = 0;
    int index = 0;

    for (int bit = 0; bit < bitCount; ++bit) {
        float e0 = goertzel_energy(&tones[index++]);
        float e1 = goertzel_energy(&tones[index++]);

        if (e1 > e0)
            byte |= 1u << bit;
    }

    return byte;
}

static bool decode_symbol_at(
    Decoder *dec,
    uint64_t sampleIndex,
    uint8_t *out)
{
    const int tones = dec->bitCount * 2 + 2;
    Goertzel work[MAX_BITS * 2 + 2];

    memcpy(work, dec->tones, sizeof(work));

    for (int i = 0; i < tones; ++i) {
        work[i].s1 = 0.0f;
        work[i].s2 = 0.0f;
    }

    for (uint32_t s = 0; s < dec->symbolSamples; ++s) {
        float sample;
        if (!decoder_history_get(dec, sampleIndex + s, &sample))
            return false;

        for (int t = 0; t < tones; ++t)
            goertzel_add(&work[t], sample);
    }

    *out = decode_symbol_from_work(work, dec->bitCount);
    return true;
}

static void decoder_seed_live_symbol(
    Decoder *dec,
    uint64_t startSample)
{
    const int tones = dec->bitCount * 2 + 2;

    decoder_reset_symbol_filters(dec);
    dec->sampleCount = 0;

    for (uint64_t index = startSample; index < dec->totalSamples; ++index) {
        float sample;
        if (!decoder_history_get(dec, index, &sample))
            break;

        for (int t = 0; t < tones; ++t)
            goertzel_add(&dec->tones[t], sample);

        dec->sampleCount++;
    }
}

static bool try_phase_search(Decoder *dec) {
    const uint32_t phaseStep = 16;
    const uint32_t syncSymbols = (uint32_t)sizeof(syncBytes);
    const uint64_t syncSamples =
        (uint64_t)syncSymbols * (uint64_t)dec->symbolSamples;
    const uint64_t oldest =
        dec->totalSamples - dec->sampleHistoryCount;

    int bestScore = -1;
    int bestPhase = 0;
    uint64_t bestStart = 0;

    if (dec->sampleHistoryCount < syncSamples + dec->symbolSamples)
        return false;

    for (uint32_t phase = 0; phase < dec->symbolSamples; phase += phaseStep) {
        int64_t start =
            (int64_t)dec->totalSamples -
            (int64_t)syncSamples -
            (int64_t)phase;

        if (start < (int64_t)oldest)
            continue;

        int score = 0;

        for (size_t i = 0; i < sizeof(syncBytes); ++i) {
            uint8_t byte;
            if (!decode_symbol_at(
                    dec,
                    (uint64_t)start +
                    (uint64_t)i * dec->symbolSamples,
                    &byte)) {
                score = -1;
                break;
            }

            if (byte != syncBytes[i])
                break;

            score++;
        }

        if (score > bestScore) {
            bestScore = score;
            bestPhase = (int)phase;
            bestStart = (uint64_t)start;
        }
    }

    dec->bestSyncScore = bestScore;
    dec->bestSyncPhase = bestPhase;

    if (bestScore != (int)sizeof(syncBytes)) {
        printf("PHASE SEARCH FAILED bestScore=%d (need %zu)\n", bestScore, sizeof(syncBytes));
        return false;
    }

    uint64_t dataStart =
        bestStart + syncSamples;

    printf("SYNC ALIGN phase=%d score=%d\n", bestPhase, bestScore);
    dec->phaseOffset = bestPhase;
    dec->phaseWait = 0;
    dec->phaseSearchReady = false;
    dec->rxState = RX_LOCKED;
    dec->locked = true;
    dec->bufferSize = 0;
    dec->clockHistory = 0;
    dec->clockCount = 0;
    decoder_seed_live_symbol(dec, dataStart);
    return true;
}


void decoder_init(
    Decoder *dec) {
    memset(dec, 0, sizeof(*dec));


    dec->bitCount = 8;


    dec->startFreq = 1000;
    dec->endFreq = 5000;
    dec->dataSpacing = 250;

    dec->symbolSamples =
            SAMPLE_RATE / 50;


    dec->clock0 = goertzel_bin_freq(
    6000,
    SAMPLE_RATE,
    dec->symbolSamples
);

    dec->clock1 = goertzel_bin_freq(
        6300,
        SAMPLE_RATE,
        dec->symbolSamples
    );


    /*
    float spacing =
            (float) (dec->endFreq - dec->startFreq)
            /
            (dec->bitCount * 2 - 1);

            */

    decoder_rebuild_tones(dec);
}

void decoder_rebuild_tones(
    Decoder *dec)
{
    int index = 0;

    for (int i = 0; i < dec->bitCount; i++) {
        goertzel_init(
            &dec->tones[index++],
            dec->startFreq +
            dec->dataSpacing * (i * 2),
            SAMPLE_RATE,
            dec->symbolSamples
        );

        goertzel_init(
            &dec->tones[index++],
            dec->startFreq +
            dec->dataSpacing * (i * 2 + 1),
            SAMPLE_RATE,
            dec->symbolSamples
        );
    }

    goertzel_init(
        &dec->tones[index++],
        dec->clock0,
        SAMPLE_RATE,
        dec->symbolSamples
    );

    goertzel_init(
        &dec->tones[index++],
        dec->clock1,
        SAMPLE_RATE,
        dec->symbolSamples
    );

    dec->sampleCount = 0;
    dec->sampleHistoryWrite = 0;
    dec->sampleHistoryCount = 0;
    dec->totalSamples = 0;
    dec->maxEnergy = 0.0f;
    dec->clockEnergy0 = 0.0f;
    dec->clockEnergy1 = 0.0f;
    dec->clockRatio = 0.0f;
    dec->bestSyncScore = 0;
    dec->bestSyncPhase = 0;
    dec->bufferSize = 0;
    dec->rxState = RX_SEARCH;
    dec->phaseSearchReady = false;
    dec->phaseWait = 0;
    dec->phaseOffset = 0;
    dec->clockHistory = 0;
    dec->clockCount = 0;
    dec->goodClocks = 0;
    dec->searchOffset = 0;
    dec->locked = false;
    memset(dec->energies, 0, sizeof(dec->energies));
}

static int detect_clock(Decoder *dec) {
    float e0 = tone_energy(dec, 16);
    float e1 = tone_energy(dec, 17);

    float max = fmaxf(e0, e1);
    float min = fminf(e0, e1);

    dec->clockEnergy0 = e0;
    dec->clockEnergy1 = e1;
    dec->clockRatio = min > 0.0f ? max / min : 0.0f;
    dec->energies[16] = e0;
    dec->energies[17] = e1;
    dec->maxEnergy = fmaxf(dec->maxEnergy * 0.85f, max);


    /*
        Ignore silence/noise.

        Your real signal is around 0.1+
        noise is around 0.001-0.01
    */
    if (max < 0.001f)
        return -1;

    return e1 > e0;
}

static void clock_search(
        Decoder *dec,
        int clock)
{
    if(clock < 0)
    {
        dec->clockHistory = 0;
        dec->clockCount = 0;
        return;
    }


    dec->clockHistory =
        (dec->clockHistory << 1) |
        (clock & 1);


    if(dec->clockCount < 8)
        dec->clockCount++;


    if(dec->clockCount >= 8)
    {
        uint8_t h = dec->clockHistory & 0xff;


        if(h == 0x55 || h == 0xAA)
        {
            printf("CLOCK LOCK pattern=%02X\n", h);
            dec->goodClocks = 8;
            dec->clockHistory = 0;
            dec->clockCount = 0;

            return;
        }

        dec->goodClocks = 0;
    }
}
static uint8_t decode_symbol(Decoder *dec) {
    uint8_t byte = 0;
    int index = 0;

    for (int bit = 0; bit < dec->bitCount; ++bit) {
        float e0 = tone_energy(dec, index);
        float e1 = tone_energy(dec, index + 1);
        float max = fmaxf(e0, e1);

        dec->energies[index++] = e0;
        dec->energies[index++] = e1;
        dec->maxEnergy = fmaxf(dec->maxEnergy * 0.85f, max);

        if (e1 > e0)
            byte |= 1u << bit;
    }

    dec->lastDecodedByte = byte;
    return byte;
}

void decoder_process(
    Decoder *dec,
    float *samples,
    uint32_t count) {
    const int tones = dec->bitCount * 2 + 2;

    for (uint32_t i = 0; i < count; i++) {
        decoder_history_push(dec, samples[i]);

        if (dec->rxState == RX_SEARCH || dec->rxState == RX_LOCKED) {
            for (int t = 0; t < tones; t++)
                goertzel_add(&dec->tones[t], samples[i]);
        }

        if (dec->rxState == RX_ALIGN) {
            if (dec->phaseWait > 0)
                dec->phaseWait--;

            if (dec->phaseWait == 0) {
                decoder_reset_symbol_filters(dec);
                dec->sampleCount = 0;
                dec->rxState = RX_LOCKED;
                printf("PHASE LOCKED offset=%d\n", dec->phaseOffset);
            }

            continue;
        }

        dec->sampleCount++;

        if (dec->sampleCount < dec->symbolSamples)
            continue;

        dec->sampleCount = 0;

        switch (dec->rxState) {
            case RX_SEARCH: {
                int clock = detect_clock(dec);
                clock_search(dec, clock);

                if (dec->goodClocks >= 8) {
                    dec->searchOffset++;
                    if (dec->searchOffset >= 4) {
                        dec->searchOffset = 0;
                        if (try_phase_search(dec)) {
                            dec->phaseSearchReady = false;
                            dec->sampleCount = 0;
                            dec->clockHistory = 0;
                            dec->clockCount = 0;
                            dec->goodClocks = 0;
                        } else {
                            // Phase search failed, reset and try again
                            dec->goodClocks = 0;
                            dec->clockHistory = 0;
                            dec->clockCount = 0;
                        }
                    }
                }
                break;
            }

            case RX_LOCKED: {
                int clock = detect_clock(dec);
                if (clock < 0) {
                    printf("clock lost\n");
                    dec->rxState = RX_SEARCH;
                    dec->locked = false;
                    dec->sampleCount = 0;
                    dec->clockHistory = 0;
                    dec->clockCount = 0;
                    dec->goodClocks = 0;
                    dec->searchOffset = 0;
                    decoder_reset_symbol_filters(dec);
                    break;
                }

                uint8_t b = decode_symbol(dec);

                if (dec->bufferSize < sizeof(dec->buffer))
                    dec->buffer[dec->bufferSize++] = b;

                printf("DATA %02X\n", b);
                break;
            }

        }
    }
}

void input_callback(
    void *userdata,
    Uint8 *stream,
    int len) {
    Decoder *dec = userdata;

    float *samples = (float *) stream;
    int count = len / sizeof(float);
    
    // Read from internal bus for loopback
    for (int i = 0; i < count; i++) {
        if (internalBusRead != internalBusWrite) {
            samples[i] = internalBusSamples[internalBusRead];
            internalBusRead = (internalBusRead + 1) % INTERNAL_BUFFER_SIZE;
        } else {
            samples[i] = 0.0f;
        }
    }

    decoder_process(
        dec,
        samples,
        count
    );
}

SDL_AudioDeviceID inputDev;
AudioData decodedAudioData;

void initAudioDecoder(SDL_Renderer *renderer) {
    memset(&decodedAudioData, 0, sizeof(AudioData));
    decoder_init(&decoder);
    decoder.renderer = renderer;

    SDL_AudioSpec inSpec = {0};
    inSpec.freq = SAMPLE_RATE;
    inSpec.format = AUDIO_F32SYS;
    inSpec.channels = 1;
    inSpec.samples = 512;
    inSpec.callback = input_callback;
    inSpec.userdata = &decoder;

    SDL_AudioSpec realSpec = {0};

    inputDev = SDL_OpenAudioDevice(NULL, 1, &inSpec, &realSpec, 0);
    printf("Decoder device opened: inputDev=%u\n", inputDev);
    if (inputDev == 0) {
        printf("Failed to open audio decoder: %s\n", SDL_GetError());
        SDL_Quit();
    }
    printf("Input device spec: freq=%d channels=%d samples=%d\n", 
           realSpec.freq, realSpec.channels, realSpec.samples);
    SDL_PauseAudioDevice(inputDev, 0);
}