/*
 * Benchmark: gstlal_interpolator upsampling extracted as standalone C (float32).
 *
 * Uses cblas_sgemv to match gstlal's convolve32() which calls
 * gsl_blas_sgemv(CblasTrans, 1.0, &input_matrix, kernel, 0, &output).
 *
 * Input:  1000 channels × 1040 time samples (interleaved), factor=2
 * Output: 1000 channels × 2048 time samples
 *
 * Compile (with MKL or OpenBLAS):
 *   gcc -O3 -march=native -o bench_gstlal bench_gstlal.c -lm -lcblas
 */

#include <mkl_cblas.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#define PI 3.141592653589793f

/* ── kernel generation (from gstlal_interpolator.c upkernel32) ───────── */

static float **upkernel(int half_length, int factor, int *out_sub_klen) {
    int kernel_length = 2 * half_length * factor + 1;
    int sub_kernel_length = 2 * half_length + 1;
    int c = kernel_length / 2;

    float *full = calloc(kernel_length, sizeof(float));
    for (int i = 0; i < kernel_length; i++) {
        int x = i - c;
        if (x == 0)
            full[i] = 1.0f;
        else
            full[i] = sinf(PI * x / factor) / (PI * x / factor) *
                       sinf(PI * x / c) / (PI * x / c);
    }

    float **vecs = malloc(factor * sizeof(float *));
    for (int j = 0; j < factor; j++) {
        vecs[j] = calloc(sub_kernel_length, sizeof(float));
        for (int i = 0; i < sub_kernel_length; i++) {
            int index = i * factor + j;
            if (index < kernel_length)
                vecs[j][sub_kernel_length - i - 1] = full[index];
        }
    }

    free(full);
    *out_sub_klen = sub_kernel_length;
    return vecs;
}

/* ── convolve32: cblas_sgemv, matches gstlal gsl_blas_sgemv call ─────── */
/*    output = input^T * kernel                                            */
/*    input is (kernel_length × channels) row-major, kernel is (klen,)     */
/*    output is (channels,)                                                */

static inline void convolve(float *restrict output,
                            const float *restrict kernel,
                            const float *restrict input, int kernel_length,
                            int channels) {
    cblas_sgemv(CblasRowMajor, CblasTrans,
                kernel_length, channels,
                1.0f, input, channels,
                kernel, 1,
                0.0f, output, 1);
}

/* ── upsample (from gstlal_interpolator.c upsample32) ───────────────── */

static void upsample(float *output, float **kernels, const float *input,
                     int kernel_length, int factor, int channels,
                     int n_output) {
    for (int samp = 0; samp < n_output; samp++) {
        int k_offset = samp % factor;
        int in_offset = (samp / factor) * channels;
        int out_offset = samp * channels;
        convolve(output + out_offset, kernels[k_offset], input + in_offset,
                 kernel_length, channels);
    }
}

/* ── upsample + identity shortcut ──────────────────────────────────── */
/*    Sub-kernel 0 is delta → memcpy instead of sgemv.                  */

static void upsample_identity(float *output, float **kernels,
                               const float *input,
                               int kernel_length, int factor, int channels,
                               int n_output, int half_length) {
    for (int samp = 0; samp < n_output; samp++) {
        int k_offset = samp % factor;
        int in_offset = (samp / factor) * channels;
        int out_offset = samp * channels;
        if (k_offset == 0) {
            /* sub-kernel 0 is delta centred at half_length */
            memcpy(output + out_offset,
                   input + in_offset + half_length * channels,
                   channels * sizeof(float));
        } else {
            convolve(output + out_offset, kernels[k_offset],
                     input + in_offset, kernel_length, channels);
        }
    }
}

/* ── timing helper ───────────────────────────────────────────────────── */

static double now_sec(void) {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ts.tv_sec + ts.tv_nsec * 1e-9;
}

/* ── main ────────────────────────────────────────────────────────────── */

int main(int argc, char **argv) {
    const int half_length = 8;
    int channels = 1000;
    int input_time = 1040;
    int factor = 2;

    if (argc >= 4) {
        channels = atoi(argv[1]);
        input_time = atoi(argv[2]);
        factor = atoi(argv[3]);
    }
    int sub_klen;

    float **kernels = upkernel(half_length, factor, &sub_klen);
    int n_output = (input_time - sub_klen + 1) * factor; /* 2048 */

    /* allocate interleaved data: data[time][channel] in row-major */
    float *input = malloc(input_time * channels * sizeof(float));
    float *output = malloc(n_output * channels * sizeof(float));

    srand(42);
    for (int i = 0; i < input_time * channels; i++)
        input[i] = (float)rand() / RAND_MAX - 0.5f;

    printf("Input:   %d time x %d channels (float32)\n", input_time, channels);
    printf("Output:  %d time x %d channels (float32)\n", n_output, channels);
    printf("Kernel:  %d sub-kernels x %d taps\n\n", factor, sub_klen);

    /* warmup */
    const int N_WARMUP = 20;
    const int N_ITER = 200;
    for (int i = 0; i < N_WARMUP; i++)
        upsample(output, kernels, input, sub_klen, factor, channels,
                 n_output);

    /* benchmark */
    double t0 = now_sec();
    for (int i = 0; i < N_ITER; i++)
        upsample(output, kernels, input, sub_klen, factor, channels,
                 n_output);
    double elapsed = now_sec() - t0;
    double per_call_us = elapsed / N_ITER * 1e6;

    printf("gstlal cblas_sgemv (C float32) %10.1f us/call  (%d iters)\n",
           per_call_us, N_ITER);

    /* warmup identity variant */
    for (int i = 0; i < N_WARMUP; i++)
        upsample_identity(output, kernels, input, sub_klen, factor, channels,
                          n_output, half_length);

    /* benchmark identity variant */
    t0 = now_sec();
    for (int i = 0; i < N_ITER; i++)
        upsample_identity(output, kernels, input, sub_klen, factor, channels,
                          n_output, half_length);
    elapsed = now_sec() - t0;
    per_call_us = elapsed / N_ITER * 1e6;

    printf("gstlal sgemv + identity        %10.1f us/call  (%d iters)\n",
           per_call_us, N_ITER);

    free(input);
    free(output);
    for (int i = 0; i < factor; i++)
        free(kernels[i]);
    free(kernels);
    return 0;
}
