#include "costas-beacon-sync.h"

#include <iostream>

const size_t banpass_len = 47;

// 512000 ksps, pass end 10kHz, stop start 25khz, 40dB
float bandpass_taps[banpass_len] = {
    0.0023692550603300333, 0.002578944666311145,  0.003062514355406165,
    0.0038352811243385077, 0.004904961679130793,  0.00627117557451129,
    0.007925170473754406,  0.009849807247519493,  0.012019773945212364,
    0.014402060769498348,  0.016956664621829987,  0.01963750831782818,
    0.022393573075532913,  0.025170154869556427,  0.027910303324460983,
    0.03055628389120102,   0.03305114805698395,   0.03534023091197014,
    0.03737267851829529,   0.039102789014577866,  0.04049133509397507,
    0.04150659218430519,   0.04212525859475136,   0.042333073914051056,
    0.04212525859475136,   0.04150659218430519,   0.04049133509397507,
    0.039102789014577866,  0.03737267851829529,   0.03534023091197014,
    0.03305114805698395,   0.03055628389120102,   0.027910303324460983,
    0.025170154869556427,  0.022393573075532913,  0.01963750831782818,
    0.016956664621829987,  0.014402060769498348,  0.012019773945212364,
    0.009849807247519493,  0.007925170473754406,  0.00627117557451129,
    0.004904961679130793,  0.0038352811243385077, 0.003062514355406165,
    0.002578944666311145,  0.0023692550603300333};

CostasBeaconSync::CostasBeaconSync(float loop_bw,
                                   float min_freq,
                                   float max_freq) {
    this->min_freq = min_freq;
    this->max_freq = max_freq;

    float damping = sqrtf(2.0f) / 2.0f;
    float denom = (1.0 + 2.0 * damping * loop_bw + loop_bw * loop_bw);
    alpha = (4 * damping * loop_bw) / denom;
    beta = (4 * loop_bw * loop_bw) / denom;

    std::cout << "Alpha: " << alpha << std::endl;
    std::cout << "Beta: " << beta << std::endl;
    std::cout << "Min f: " << min_freq << std::endl;
    std::cout << "Max f: " << max_freq << std::endl;

    loop_freq = 0.0;
    loop_phase = 0.0;

    locked = false;

    bandpass = firfilt_crcf_create(bandpass_taps, banpass_len);
}

std::complex<float> CostasBeaconSync::work(std::complex<float> in) {
    std::complex<float> filtered;
    firfilt_crcf_push(bandpass, in);  // push input sample
    firfilt_crcf_execute(bandpass, &filtered);

    std::complex<float> out =
        std::complex<float>(cos(-loop_phase), sin(-loop_phase));

    std::complex<float> costas_sample = filtered * out;

    float error = costas_sample.imag() * costas_sample.real();

    locked = std::abs(error) < 0.001;

    loop_freq += beta * error;
    loop_phase += loop_freq + alpha * error;

    while (loop_phase > (2 * M_PI))
        loop_phase -= 2 * M_PI;
    while (loop_phase < (-2 * M_PI))
        loop_phase += 2 * M_PI;

    if (loop_freq > max_freq)
        loop_freq = max_freq;
    else if (loop_freq < min_freq)
        loop_freq = min_freq;

    return out;
}

bool CostasBeaconSync::has_lock() {
    return locked;
}

CostasBeaconSync::~CostasBeaconSync() {
    firfilt_crcf_destroy(bandpass);
}