#include <stdlib.h>
#include <liquid/liquid.h>
#include <math.h>
#include <complex.h>
#include <stdio.h>

const float SAMPLINGRATE = 1000000.0;
const float FFT_LEN = 512;

int spectral_bin_to_fft_idx(int bin) {
    if(bin == 0) {
        return FFT_LEN/2;
    } else if(bin > 0) {
        return bin - 1;
    } else {
        return bin + FFT_LEN;
    }
}


int main() {
    FILE* output_csv = fopen("output.csv", "w");
    FILE* output = fopen("output.raw", "w");
    FILE* input = fopen("input.raw", "r");

    nco_crcf correction = nco_crcf_create(LIQUID_NCO);
    nco_crcf_set_phase(correction, 0.0f);

    float complex * fft_in = (float complex*) malloc(FFT_LEN * sizeof(float complex));
    float complex * fft_out = (float complex*) malloc(FFT_LEN * sizeof(float complex));

    // create FFT plan
    fftplan fft = fft_create_plan(FFT_LEN, fft_in, fft_out, LIQUID_FFT_FORWARD, 0);

    int next_fft_in = 0;
    int pos = 0;
    float in[2];

    uint64_t sample_count = 0;

    while(fread(in, sizeof(float), 2, input) == 2) {
        complex float cplx_in = (in[1] + I * in[0]);
        sample_count++;

        if(next_fft_in <= 0) {
            fft_in[pos] = cplx_in;
            pos += 1;
            if(pos == FFT_LEN) {
                pos = 0;
                fft_execute(fft);

                float fft_max = cabsf(fft_out[0]);
                for(int i = 0; i < FFT_LEN; i++) {
                    float mag = cabsf(fft_out[i]);
                    if(mag > fft_max) {
                        fft_max = mag;
                    }
                }
                //printf("Min: %f Max: %f\n", fft_min, fft_max);

                for(int bin = -FFT_LEN/2; bin < FFT_LEN/2; bin++) {
                    int idx = spectral_bin_to_fft_idx(bin);
                    float mag = cabsf(fft_out[idx]);
                    mag = mag / fft_max;
                    fprintf(output_csv, "%f;", mag);
                }

                printf("===========\n");
                float max_levels = 0;
                int max_center = 0;
                for(int bin = -50; bin <= 50; bin++) {
                    int center_idx = spectral_bin_to_fft_idx(bin);
                    float center_val = cabsf(fft_out[center_idx]) / fft_max;
                    if(center_val > 0.25) {
                        printf("Found peak candidate at %d\n", bin);
                        int left_idx = spectral_bin_to_fft_idx(bin - 127);
                        int right_idx = spectral_bin_to_fft_idx(bin + 127);
                        float left_val = cabsf(fft_out[left_idx]) / fft_max;
                        float right_val = cabsf(fft_out[right_idx]) / fft_max;

                        if(center_val + left_val + right_val > max_levels) {
                            max_levels = center_val + left_val + right_val;
                            max_center = bin;
                        }
                    }
                }

                if(max_levels > 0.0) {
                    float center_freq = max_center * SAMPLINGRATE / FFT_LEN;
                    printf("Found center at %f\n", center_freq);
                    nco_crcf_set_frequency(correction, -(2 * M_PI * center_freq) / SAMPLINGRATE);
                }

                fprintf(output_csv, "\n");

                next_fft_in = SAMPLINGRATE / 4;
            }
        } else {
            next_fft_in--;
        }

        float complex x;
        // increment internal phase
        nco_crcf_step(correction);
        // compute complex exponential
        nco_crcf_cexpf(correction, &x);

        float complex cplx_out = cplx_in * x;

        float buffer[] = {creal(cplx_out), cimag(cplx_out)};
        fwrite(buffer, sizeof(float), 2, output);
    }

    printf("Processed %fs\n", sample_count / SAMPLINGRATE);

    fft_destroy_plan(fft);
    free(fft_in);
    free(fft_out);

    fclose(output_csv);
    fclose(output);
    fclose(input);

    return 0;
}