/* -*- c++ -*- */
/*
 * Copyright 2013,2018 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

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

#include <gnuradio/filter/pfb_arb_resampler.h>
#include <gnuradio/logger.h>
#include <gnuradio/math.h>
#include <boost/math/special_functions/round.hpp>
#include <cstdio>
#include <stdexcept>

namespace gr {
namespace filter {
namespace kernel {

pfb_arb_resampler_ccf::pfb_arb_resampler_ccf(float rate,
                                             const std::vector<float>& taps,
                                             unsigned int filter_size)
{
    d_acc = 0; // start accumulator at 0

    /* The number of filters is specified by the user as the
       filter size; this is also the interpolation rate of the
       filter. We use it and the rate provided to determine the
       decimation rate. This acts as a rational resampler. The
       flt_rate is calculated as the residual between the integer
       decimation rate and the real decimation rate and will be
       used to determine to interpolation point of the resampling
       process.
    */
    d_int_rate = filter_size;
    set_rate(rate);

    d_last_filter = (taps.size() / 2) % filter_size;

    d_filters = std::vector<fir_filter_ccf*>(d_int_rate);
    d_diff_filters = std::vector<fir_filter_ccf*>(d_int_rate);

    // Create an FIR filter for each channel and zero out the taps
    std::vector<float> vtaps(0, d_int_rate);
    for (unsigned int i = 0; i < d_int_rate; i++) {
        d_filters[i] = new fir_filter_ccf(1, vtaps);
        d_diff_filters[i] = new fir_filter_ccf(1, vtaps);
    }

    // Now, actually set the filters' taps
    set_taps(taps);

    // Delay is based on number of taps per filter arm. Round to
    // the nearest integer.
    float delay = rate * (taps_per_filter() - 1.0) / 2.0;
    d_delay = static_cast<int>(boost::math::iround(delay));

    // This calculation finds the phase offset induced by the
    // arbitrary resampling. It's based on which filter arm we are
    // at the filter's group delay plus the fractional offset
    // between the samples. Calculated here based on the rotation
    // around nfilts starting at start_filter.
    float accum = d_delay * d_flt_rate;
    int accum_int = static_cast<int>(accum);
    float accum_frac = accum - accum_int;
    int end_filter = static_cast<int>(
        boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int,
                                  static_cast<float>(d_int_rate))));

    d_est_phase_change = d_last_filter - (end_filter + accum_frac);
}

pfb_arb_resampler_ccf::~pfb_arb_resampler_ccf()
{
    for (unsigned int i = 0; i < d_int_rate; i++) {
        delete d_filters[i];
        delete d_diff_filters[i];
    }
}

void pfb_arb_resampler_ccf::create_taps(const std::vector<float>& newtaps,
                                        std::vector<std::vector<float>>& ourtaps,
                                        std::vector<fir_filter_ccf*>& ourfilter)
{
    unsigned int ntaps = newtaps.size();
    d_taps_per_filter = (unsigned int)ceil((double)ntaps / (double)d_int_rate);

    // Create d_numchan vectors to store each channel's taps
    ourtaps.resize(d_int_rate);

    // Make a vector of the taps plus fill it out with 0's to fill
    // each polyphase filter with exactly d_taps_per_filter
    std::vector<float> tmp_taps;
    tmp_taps = newtaps;
    while ((float)(tmp_taps.size()) < d_int_rate * d_taps_per_filter) {
        tmp_taps.push_back(0.0);
    }

    for (unsigned int i = 0; i < d_int_rate; i++) {
        // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
        ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
        for (unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j * d_int_rate];
        }

        // Build a filter for each channel and add it's taps to it
        ourfilter[i]->set_taps(ourtaps[i]);
    }
}

void pfb_arb_resampler_ccf::create_diff_taps(const std::vector<float>& newtaps,
                                             std::vector<float>& difftaps)
{
    // Calculate the differential taps using a derivative filter
    std::vector<float> diff_filter(2);
    diff_filter[0] = -1;
    diff_filter[1] = 1;

    for (unsigned int i = 0; i < newtaps.size() - 1; i++) {
        float tap = 0;
        for (unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j] * newtaps[i + j];
        }
        difftaps.push_back(tap);
    }
    difftaps.push_back(0);
}

void pfb_arb_resampler_ccf::set_taps(const std::vector<float>& taps)
{
    std::vector<float> dtaps;
    create_diff_taps(taps, dtaps);
    create_taps(taps, d_taps, d_filters);
    create_taps(dtaps, d_dtaps, d_diff_filters);
}

std::vector<std::vector<float>> pfb_arb_resampler_ccf::taps() const { return d_taps; }

void pfb_arb_resampler_ccf::print_taps()
{
    unsigned int i, j;
    for (i = 0; i < d_int_rate; i++) {
        printf("filter[%d]: [", i);
        for (j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e", d_taps[i][j]);
        }
        printf("]\n");
    }
}

void pfb_arb_resampler_ccf::set_rate(float rate)
{
    d_dec_rate = (unsigned int)floor(d_int_rate / rate);
    d_flt_rate = (d_int_rate / rate) - d_dec_rate;
}

void pfb_arb_resampler_ccf::set_phase(float ph)
{
    if ((ph < 0) || (ph >= 2.0 * GR_M_PI)) {
        throw std::runtime_error(
            "pfb_arb_resampler_ccf: set_phase value out of bounds [0, 2pi).\n");
    }

    float ph_diff = 2.0 * GR_M_PI / (float)d_filters.size();
    d_last_filter = static_cast<int>(ph / ph_diff);
}

float pfb_arb_resampler_ccf::phase() const
{
    float ph_diff = 2.0 * GR_M_PI / static_cast<float>(d_filters.size());
    return d_last_filter * ph_diff;
}

unsigned int pfb_arb_resampler_ccf::taps_per_filter() const { return d_taps_per_filter; }

float pfb_arb_resampler_ccf::phase_offset(float freq, float fs)
{
    float adj = (2.0 * GR_M_PI) * (freq / fs) / static_cast<float>(d_int_rate);
    return -adj * d_est_phase_change;
}

int pfb_arb_resampler_ccf::filter(gr_complex* output,
                                  gr_complex* input,
                                  int n_to_read,
                                  int& n_read)
{
    int i_out = 0, i_in = 0;
    unsigned int j = d_last_filter;
    ;
    gr_complex o0, o1;

    while (i_in < n_to_read) {
        // start j by wrapping around mod the number of channels
        while (j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1 * d_acc; // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
        }
        i_in += (int)(j / d_int_rate);
        j = j % d_int_rate;
    }
    d_last_filter = j; // save last filter state for re-entry

    n_read = i_in; // return how much we've actually read
    return i_out;  // return how much we've produced
}

/****************************************************************/

pfb_arb_resampler_ccc::pfb_arb_resampler_ccc(float rate,
                                             const std::vector<gr_complex>& taps,
                                             unsigned int filter_size)
{
    d_acc = 0; // start accumulator at 0

    /* The number of filters is specified by the user as the
       filter size; this is also the interpolation rate of the
       filter. We use it and the rate provided to determine the
       decimation rate. This acts as a rational resampler. The
       flt_rate is calculated as the residual between the integer
       decimation rate and the real decimation rate and will be
       used to determine to interpolation point of the resampling
       process.
    */
    d_int_rate = filter_size;
    set_rate(rate);

    d_last_filter = (taps.size() / 2) % filter_size;

    d_filters = std::vector<fir_filter_ccc*>(d_int_rate);
    d_diff_filters = std::vector<fir_filter_ccc*>(d_int_rate);

    // Create an FIR filter for each channel and zero out the taps
    std::vector<gr_complex> vtaps(0, d_int_rate);
    for (unsigned int i = 0; i < d_int_rate; i++) {
        d_filters[i] = new fir_filter_ccc(1, vtaps);
        d_diff_filters[i] = new fir_filter_ccc(1, vtaps);
    }

    // Now, actually set the filters' taps
    set_taps(taps);

    // Delay is based on number of taps per filter arm. Round to
    // the nearest integer.
    float delay = rate * (taps_per_filter() - 1.0) / 2.0;
    d_delay = static_cast<int>(boost::math::iround(delay));

    // This calculation finds the phase offset induced by the
    // arbitrary resampling. It's based on which filter arm we are
    // at the filter's group delay plus the fractional offset
    // between the samples. Calculated here based on the rotation
    // around nfilts starting at start_filter.
    float accum = d_delay * d_flt_rate;
    int accum_int = static_cast<int>(accum);
    float accum_frac = accum - accum_int;
    int end_filter = static_cast<int>(
        boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int,
                                  static_cast<float>(d_int_rate))));

    d_est_phase_change = d_last_filter - (end_filter + accum_frac);
}

pfb_arb_resampler_ccc::~pfb_arb_resampler_ccc()
{
    for (unsigned int i = 0; i < d_int_rate; i++) {
        delete d_filters[i];
        delete d_diff_filters[i];
    }
}

void pfb_arb_resampler_ccc::create_taps(const std::vector<gr_complex>& newtaps,
                                        std::vector<std::vector<gr_complex>>& ourtaps,
                                        std::vector<fir_filter_ccc*>& ourfilter)
{
    unsigned int ntaps = newtaps.size();
    d_taps_per_filter = (unsigned int)ceil((double)ntaps / (double)d_int_rate);

    // Create d_numchan vectors to store each channel's taps
    ourtaps.resize(d_int_rate);

    // Make a vector of the taps plus fill it out with 0's to fill
    // each polyphase filter with exactly d_taps_per_filter
    std::vector<gr_complex> tmp_taps;
    tmp_taps = newtaps;
    while ((float)(tmp_taps.size()) < d_int_rate * d_taps_per_filter) {
        tmp_taps.push_back(0.0);
    }

    for (unsigned int i = 0; i < d_int_rate; i++) {
        // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
        ourtaps[i] = std::vector<gr_complex>(d_taps_per_filter, 0);
        for (unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j * d_int_rate];
        }

        // Build a filter for each channel and add it's taps to it
        ourfilter[i]->set_taps(ourtaps[i]);
    }
}

void pfb_arb_resampler_ccc::create_diff_taps(const std::vector<gr_complex>& newtaps,
                                             std::vector<gr_complex>& difftaps)
{
    // Calculate the differential taps using a derivative filter
    std::vector<gr_complex> diff_filter(2);
    diff_filter[0] = -1;
    diff_filter[1] = 1;

    for (unsigned int i = 0; i < newtaps.size() - 1; i++) {
        gr_complex tap = 0;
        for (unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j] * newtaps[i + j];
        }
        difftaps.push_back(tap);
    }
    difftaps.push_back(0);
}

void pfb_arb_resampler_ccc::set_taps(const std::vector<gr_complex>& taps)
{
    std::vector<gr_complex> dtaps;
    create_diff_taps(taps, dtaps);
    create_taps(taps, d_taps, d_filters);
    create_taps(dtaps, d_dtaps, d_diff_filters);
}

std::vector<std::vector<gr_complex>> pfb_arb_resampler_ccc::taps() const
{
    return d_taps;
}

void pfb_arb_resampler_ccc::print_taps()
{
    unsigned int i, j;
    for (i = 0; i < d_int_rate; i++) {
        printf("filter[%d]: [", i);
        for (j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e + j%.4e", d_taps[i][j].real(), d_taps[i][j].imag());
        }
        printf("]\n");
    }
}

void pfb_arb_resampler_ccc::set_rate(float rate)
{
    d_dec_rate = (unsigned int)floor(d_int_rate / rate);
    d_flt_rate = (d_int_rate / rate) - d_dec_rate;
}

void pfb_arb_resampler_ccc::set_phase(float ph)
{
    if ((ph < 0) || (ph >= 2.0 * GR_M_PI)) {
        throw std::runtime_error(
            "pfb_arb_resampler_ccc: set_phase value out of bounds [0, 2pi).\n");
    }

    float ph_diff = 2.0 * GR_M_PI / (float)d_filters.size();
    d_last_filter = static_cast<int>(ph / ph_diff);
}

float pfb_arb_resampler_ccc::phase() const
{
    float ph_diff = 2.0 * GR_M_PI / static_cast<float>(d_filters.size());
    return d_last_filter * ph_diff;
}

unsigned int pfb_arb_resampler_ccc::taps_per_filter() const { return d_taps_per_filter; }

float pfb_arb_resampler_ccc::phase_offset(float freq, float fs)
{
    float adj = (2.0 * GR_M_PI) * (freq / fs) / static_cast<float>(d_int_rate);
    return -adj * d_est_phase_change;
}

int pfb_arb_resampler_ccc::filter(gr_complex* output,
                                  gr_complex* input,
                                  int n_to_read,
                                  int& n_read)
{
    int i_out = 0, i_in = 0;
    unsigned int j = d_last_filter;
    ;
    gr_complex o0, o1;

    while (i_in < n_to_read) {
        // start j by wrapping around mod the number of channels
        while (j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1 * d_acc; // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
        }
        i_in += (int)(j / d_int_rate);
        j = j % d_int_rate;
    }
    d_last_filter = j; // save last filter state for re-entry

    n_read = i_in; // return how much we've actually read
    return i_out;  // return how much we've produced
}

/****************************************************************/

pfb_arb_resampler_fff::pfb_arb_resampler_fff(float rate,
                                             const std::vector<float>& taps,
                                             unsigned int filter_size)
{
    d_acc = 0; // start accumulator at 0

    /* The number of filters is specified by the user as the
       filter size; this is also the interpolation rate of the
       filter. We use it and the rate provided to determine the
       decimation rate. This acts as a rational resampler. The
       flt_rate is calculated as the residual between the integer
       decimation rate and the real decimation rate and will be
       used to determine to interpolation point of the resampling
       process.
    */
    d_int_rate = filter_size;
    set_rate(rate);

    d_last_filter = (taps.size() / 2) % filter_size;

    d_filters = std::vector<fir_filter_fff*>(d_int_rate);
    d_diff_filters = std::vector<fir_filter_fff*>(d_int_rate);

    // Create an FIR filter for each channel and zero out the taps
    std::vector<float> vtaps(0, d_int_rate);
    for (unsigned int i = 0; i < d_int_rate; i++) {
        d_filters[i] = new fir_filter_fff(1, vtaps);
        d_diff_filters[i] = new fir_filter_fff(1, vtaps);
    }

    // Now, actually set the filters' taps
    set_taps(taps);

    // Delay is based on number of taps per filter arm. Round to
    // the nearest integer.
    float delay = rate * (taps_per_filter() - 1.0) / 2.0;
    d_delay = static_cast<int>(boost::math::iround(delay));

    // This calculation finds the phase offset induced by the
    // arbitrary resampling. It's based on which filter arm we are
    // at the filter's group delay plus the fractional offset
    // between the samples. Calculated here based on the rotation
    // around nfilts starting at start_filter.
    float accum = d_delay * d_flt_rate;
    int accum_int = static_cast<int>(accum);
    float accum_frac = accum - accum_int;
    int end_filter = static_cast<int>(
        boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int,
                                  static_cast<float>(d_int_rate))));

    d_est_phase_change = d_last_filter - (end_filter + accum_frac);
}

pfb_arb_resampler_fff::~pfb_arb_resampler_fff()
{
    for (unsigned int i = 0; i < d_int_rate; i++) {
        delete d_filters[i];
        delete d_diff_filters[i];
    }
}

void pfb_arb_resampler_fff::create_taps(const std::vector<float>& newtaps,
                                        std::vector<std::vector<float>>& ourtaps,
                                        std::vector<fir_filter_fff*>& ourfilter)
{
    unsigned int ntaps = newtaps.size();
    d_taps_per_filter = (unsigned int)ceil((double)ntaps / (double)d_int_rate);

    // Create d_numchan vectors to store each channel's taps
    ourtaps.resize(d_int_rate);

    // Make a vector of the taps plus fill it out with 0's to fill
    // each polyphase filter with exactly d_taps_per_filter
    std::vector<float> tmp_taps;
    tmp_taps = newtaps;
    while ((float)(tmp_taps.size()) < d_int_rate * d_taps_per_filter) {
        tmp_taps.push_back(0.0);
    }

    for (unsigned int i = 0; i < d_int_rate; i++) {
        // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
        ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
        for (unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j * d_int_rate];
        }

        // Build a filter for each channel and add it's taps to it
        ourfilter[i]->set_taps(ourtaps[i]);
    }
}

void pfb_arb_resampler_fff::create_diff_taps(const std::vector<float>& newtaps,
                                             std::vector<float>& difftaps)
{
    // Calculate the differential taps using a derivative filter
    std::vector<float> diff_filter(2);
    diff_filter[0] = -1;
    diff_filter[1] = 1;

    for (unsigned int i = 0; i < newtaps.size() - 1; i++) {
        float tap = 0;
        for (unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j] * newtaps[i + j];
        }
        difftaps.push_back(tap);
    }
    difftaps.push_back(0);
}

void pfb_arb_resampler_fff::set_taps(const std::vector<float>& taps)
{
    std::vector<float> dtaps;
    create_diff_taps(taps, dtaps);
    create_taps(taps, d_taps, d_filters);
    create_taps(dtaps, d_dtaps, d_diff_filters);
}

std::vector<std::vector<float>> pfb_arb_resampler_fff::taps() const { return d_taps; }

void pfb_arb_resampler_fff::print_taps()
{
    unsigned int i, j;
    for (i = 0; i < d_int_rate; i++) {
        printf("filter[%d]: [", i);
        for (j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e", d_taps[i][j]);
        }
        printf("]\n");
    }
}

void pfb_arb_resampler_fff::set_rate(float rate)
{
    d_dec_rate = (unsigned int)floor(d_int_rate / rate);
    d_flt_rate = (d_int_rate / rate) - d_dec_rate;
}

void pfb_arb_resampler_fff::set_phase(float ph)
{
    if ((ph < 0) || (ph >= 2.0 * GR_M_PI)) {
        throw std::runtime_error(
            "pfb_arb_resampler_fff: set_phase value out of bounds [0, 2pi).\n");
    }

    float ph_diff = 2.0 * GR_M_PI / (float)d_filters.size();
    d_last_filter = static_cast<int>(ph / ph_diff);
}

float pfb_arb_resampler_fff::phase() const
{
    float ph_diff = 2.0 * GR_M_PI / static_cast<float>(d_filters.size());
    return d_last_filter * ph_diff;
}

unsigned int pfb_arb_resampler_fff::taps_per_filter() const { return d_taps_per_filter; }

float pfb_arb_resampler_fff::phase_offset(float freq, float fs)
{
    float adj = (2.0 * GR_M_PI) * (freq / fs) / static_cast<float>(d_int_rate);
    return -adj * d_est_phase_change;
}

int pfb_arb_resampler_fff::filter(float* output, float* input, int n_to_read, int& n_read)
{
    int i_out = 0, i_in = 0;
    unsigned int j = d_last_filter;
    ;
    float o0, o1;

    while (i_in < n_to_read) {
        // start j by wrapping around mod the number of channels
        while (j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1 * d_acc; // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
        }
        i_in += (int)(j / d_int_rate);
        j = j % d_int_rate;
    }
    d_last_filter = j; // save last filter state for re-entry

    n_read = i_in; // return how much we've actually read
    return i_out;  // return how much we've produced
}

} /* namespace kernel */
} /* namespace filter */
} /* namespace gr */
