/* -*- c++ -*- */
/*
 * Copyright 2004,2007,2010,2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "sro_model_impl.h"
#include <gnuradio/io_signature.h>
#include <stdexcept>

namespace gr {
namespace channels {

sro_model::sptr sro_model::make(double sample_rate_hz,
                                double std_dev_hz,
                                double max_dev_hz,
                                double noise_seed)
{
    return gnuradio::make_block_sptr<sro_model_impl>(
        sample_rate_hz, std_dev_hz, max_dev_hz, noise_seed);
}

sro_model_impl::sro_model_impl(double sample_rate_hz,
                               double std_dev_hz,
                               double max_dev_hz,
                               double noise_seed)
    : block("sro_model",
            io_signature::make(1, 1, sizeof(gr_complex)),
            io_signature::make(1, 1, sizeof(gr_complex))),
      d_mu(0.0),
      d_mu_inc(1.0),
      d_sro(0.0),
      d_samp_rate(sample_rate_hz),
      d_max_dev_hz(max_dev_hz),
      d_std_dev_hz(std_dev_hz),
      d_interp(new gr::filter::mmse_fir_interpolator_cc()),
      d_noise(gr::analog::fastnoise_source_f::make(
          analog::GR_GAUSSIAN, std_dev_hz, noise_seed)),
      d_noise_seed(noise_seed)
{
    // set_inverse_relative_rate(interp_ratio);
    set_relative_rate(1, 1);
}

sro_model_impl::~sro_model_impl() { delete d_interp; }

void sro_model_impl::forecast(int noutput_items, gr_vector_int& ninput_items_required)
{
    unsigned ninputs = ninput_items_required.size();
    for (unsigned i = 0; i < ninputs; i++) {
        ninput_items_required[i] =
            (int)ceil((noutput_items * (d_mu_inc + d_max_dev_hz / d_samp_rate)) +
                      d_interp->ntaps());
    }
}

int sro_model_impl::general_work(int noutput_items,
                                 gr_vector_int& ninput_items,
                                 gr_vector_const_void_star& input_items,
                                 gr_vector_void_star& output_items)
{
    const gr_complex* in = (const gr_complex*)input_items[0];
    gr_complex* out = (gr_complex*)output_items[0];

    int ii = 0; // input index
    int oo = 0; // output index

    while (oo < noutput_items) {

        // perform sample rate offset update
        d_sro += d_noise->sample_unbiased();
        d_sro = std::min(d_sro, d_max_dev_hz);
        d_sro = std::max(d_sro, -d_max_dev_hz);
        d_mu_inc = 1.0 + d_sro / d_samp_rate;

        out[oo++] = d_interp->interpolate(&in[ii], d_mu);

        double s = d_mu + d_mu_inc;
        double f = floor(s);
        int incr = (int)f;
        d_mu = s - f;
        ii += incr;
    }

    consume_each(ii);

    return noutput_items;
}

float sro_model_impl::mu() const { return d_mu; }

float sro_model_impl::interp_ratio() const { return d_mu_inc; }

void sro_model_impl::set_mu(float mu) { d_mu = mu; }

void sro_model_impl::set_interp_ratio(float interp_ratio) { d_mu_inc = interp_ratio; }

void sro_model_impl::setup_rpc()
{
#ifdef GR_CTRLPORT
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_variable<float>(alias(),
                                                            "sro",
                                                            &d_sro,
                                                            pmt::mp(-10.0f),
                                                            pmt::mp(10.0f),
                                                            pmt::mp(0.0f),
                                                            "",
                                                            "Current SRO in Hz",
                                                            RPC_PRIVLVL_MIN,
                                                            DISPTIME | DISPOPTSTRIP)));
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_variable<float>(alias(),
                                                            "sro_max",
                                                            &d_max_dev_hz,
                                                            pmt::mp(-10.0f),
                                                            pmt::mp(10.0f),
                                                            pmt::mp(0.0f),
                                                            "",
                                                            "Max SRO in Hz",
                                                            RPC_PRIVLVL_MIN,
                                                            DISPTIME | DISPOPTSTRIP)));
#endif /* GR_CTRLPORT */
}

} // namespace channels
} /* namespace gr */