#include "costas.h"

#include<cmath>
#include <liquid/liquid.h>

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 agc_bw, 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);
    this->alpha = (4 * damping * loop_bw) / denom;
    this->beta = (4 * loop_bw * loop_bw) / denom;

    this->loop_freq = 0.0;
    this->loop_phase = 0.0;

    this->bandpass = firfilt_crcf_create(bandpass_taps,banpass_len);

    this->agc = agc_crcf_create();
    agc_crcf_set_bandwidth(this->agc, agc_bw);
}


std::complex<float> CostasBeaconSync::work(std::complex<float> in) {

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

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

    agc_crcf_execute(this->agc, filtered, &filtered);

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

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

    this->loop_freq += beta * error;
    this->loop_phase += this->loop_freq + alpha * error;

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

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


    return out;
}