#include "hybbx/rf_tx_pace.h"
#include "hybbx/posix_time.h"

#include <pthread.h>

static pthread_mutex_t g_pace_lock = PTHREAD_MUTEX_INITIALIZER;
static struct timespec g_last_tx;
static struct timespec g_burst_start;
static int g_have_last_tx;

static void timespec_now(struct timespec *ts)
{
    clock_gettime(CLOCK_MONOTONIC, ts);
}

static long timespec_diff_ms(const struct timespec *later,
                             const struct timespec *earlier)
{
    return (later->tv_sec - earlier->tv_sec) * 1000L
           + (later->tv_nsec - earlier->tv_nsec) / 1000000L;
}

static long gap_ms_since_last(const struct timespec *now)
{
    long elapsed_ms;

    if (!g_have_last_tx) {
        return (long)HYBBX_RF_TX_MIN_GAP_MS;
    }
    elapsed_ms = timespec_diff_ms(now, &g_last_tx);
    if (elapsed_ms >= (long)HYBBX_RF_TX_MIN_GAP_MS) {
        return 0;
    }
    return (long)HYBBX_RF_TX_MIN_GAP_MS - elapsed_ms;
}

static void sleep_ms(long wait_ms)
{
    if (wait_ms > 0) {
        usleep((useconds_t)wait_ms * 1000u);
    }
}

static void enforce_burst_limit(struct timespec *now)
{
    long since_burst;
    long wait_ms;

    if (!g_have_last_tx) {
        g_burst_start = *now;
        return;
    }

    if (gap_ms_since_last(now) == 0) {
        g_burst_start = *now;
        return;
    }

    since_burst = timespec_diff_ms(now, &g_burst_start);
    if (since_burst < (long)HYBBX_RF_TX_MAX_BURST_MS) {
        return;
    }

    wait_ms = gap_ms_since_last(now);
    sleep_ms(wait_ms);
    timespec_now(now);
    g_burst_start = *now;
}

static void mark_tx(const struct timespec *now)
{
    g_last_tx = *now;
    g_have_last_tx = 1;
}

void hybbx_rf_tx_burst_guard(void)
{
    struct timespec now;

    pthread_mutex_lock(&g_pace_lock);
    timespec_now(&now);
    enforce_burst_limit(&now);
    mark_tx(&now);
    pthread_mutex_unlock(&g_pace_lock);
}

void hybbx_rf_tx_pace(void)
{
    struct timespec now;
    long wait_ms;

    pthread_mutex_lock(&g_pace_lock);
    timespec_now(&now);
    enforce_burst_limit(&now);
    wait_ms = gap_ms_since_last(&now);
    sleep_ms(wait_ms);
    timespec_now(&now);
    mark_tx(&now);
    pthread_mutex_unlock(&g_pace_lock);
}