/*!
 * \file direct_resampler_conditioner_cs.cc
 * \brief Nearest neighborhood resampler with
 *        complex short input and complex short output
 * \author Luis Esteve, 2011. luis(at)epsilon-formacion.com
 *
 *
 * -----------------------------------------------------------------------------
 *
 * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
 * This file is part of GNSS-SDR.
 *
 * Copyright (C) 2010-2020  (see AUTHORS file for a list of contributors)
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 * -----------------------------------------------------------------------------
 */


#include "direct_resampler_conditioner_cs.h"
#include "sensor_data/sensor_data_resampler.h"
#include "sensor_data/sensor_identifier.h"
#include <gnuradio/io_signature.h>
#include <volk/volk.h>  // for lv_16sc_t
#include <algorithm>    // for min


direct_resampler_conditioner_cs_sptr direct_resampler_make_conditioner_cs(
    double sample_freq_in, double sample_freq_out)
{
    return direct_resampler_conditioner_cs_sptr(
        new direct_resampler_conditioner_cs(sample_freq_in,
            sample_freq_out));
}


direct_resampler_conditioner_cs::direct_resampler_conditioner_cs(
    double sample_freq_in,
    double sample_freq_out)
    : gr::block("direct_resampler_make_conditioner_cs",
          gr::io_signature::make(1, 1, sizeof(lv_16sc_t)),
          gr::io_signature::make(1, 1, sizeof(lv_16sc_t))),
      d_sample_freq_in(sample_freq_in),
      d_sample_freq_out(sample_freq_out),
      d_phase(0),
      d_lphase(0)
{
    const double two_32 = 4294967296.0;
    // Computes the phase step multiplying the resampling ratio by 2^32 = 4294967296
    if (d_sample_freq_in >= d_sample_freq_out)
        {
            d_phase_step = static_cast<uint32_t>(floor(two_32 * sample_freq_out / sample_freq_in));
        }
    else
        {
            d_phase_step = static_cast<uint32_t>(floor(two_32 * sample_freq_in / sample_freq_out));
        }

#ifdef GR_GREATER_38
    this->set_relative_rate(static_cast<uint64_t>(sample_freq_out), static_cast<uint64_t>(sample_freq_in));
#else
    this->set_relative_rate(sample_freq_out / sample_freq_in);
#endif

    // Do not propagate tags, they carry a sample offset that is not properly updated in
    // this resampler and thus needs manual handling.
    this->set_tag_propagation_policy(TPP_DONT);
}


void direct_resampler_conditioner_cs::forecast(int noutput_items,
    gr_vector_int &ninput_items_required)
{
    int nreqd = std::max(static_cast<unsigned>(1), static_cast<int>(static_cast<double>(noutput_items + 1) * sample_freq_in() / sample_freq_out()) + history() - 1);
    std::fill(ninput_items_required.begin(), ninput_items_required.end(), nreqd);
}


int direct_resampler_conditioner_cs::general_work(int noutput_items,
    gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
    gr_vector_void_star &output_items)
{
    const auto *in = reinterpret_cast<const lv_16sc_t *>(input_items[0]);
    auto *out = reinterpret_cast<lv_16sc_t *>(output_items[0]);

    int lcv = 0;
    int count = 0;

    if (d_sample_freq_in >= d_sample_freq_out)
        {
            while ((lcv < noutput_items))
                {
                    if (d_phase <= d_lphase)
                        {
                            out[lcv] = *in;
                            lcv++;
                        }

                    d_lphase = d_phase;
                    d_phase += d_phase_step;
                    in++;
                    count++;
                }
        }
    else
        {
            while ((lcv < noutput_items))
                {
                    d_lphase = d_phase;
                    d_phase += d_phase_step;
                    if (d_phase <= d_lphase)
                        {
                            in++;
                            count++;
                        }
                    out[lcv] = *in;
                    lcv++;
                }
        }

    // Sensor data tag resampling
    std::vector<gr::tag_t> sensor_tags;
    this->get_tags_in_window(sensor_tags, 0, 0, std::min(count, ninput_items[0]), pmt::mp("sensor_data"));
    for (auto &tag : resample_sensor_data_tags(sensor_tags, d_sample_freq_in, d_sample_freq_out))
        {
            this->add_item_tag(0, tag);
        }


    consume_each(std::min(count, ninput_items[0]));
    return lcv;
}
