/* -*- c++ -*- */
/*
 * Copyright 2014 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 "correlate_access_code_tag_fixed_impl.h"
#include <gnuradio/io_signature.h>
#include <boost/format.hpp>
#include <stdexcept>
#include <volk/volk.h>
#include <cstdio>
#include <iostream>

namespace gr {
  namespace satlab {

    correlate_access_code_tag_fixed::sptr
    correlate_access_code_tag_fixed::make(const std::string &access_code,
                                          int threshold,
                                          const std::string &tag_name,
                                          unsigned int pkt_len)
    {
        return gnuradio::get_initial_sptr(new correlate_access_code_tag_fixed_impl(
                                            access_code, threshold, tag_name, pkt_len));
    }

    correlate_access_code_tag_fixed_impl::correlate_access_code_tag_fixed_impl(
        const std::string &access_code, int threshold, const std::string &tag_name, unsigned int pkt_len)
      : block("correlate_access_code_tag_fixed",
              io_signature::make(1, 1, sizeof(char)),
              io_signature::make(1, 1, sizeof(char))),
        d_data_reg(0),
        d_mask(0),
        d_threshold(threshold),
        d_len(0),
        d_pkt_len(pkt_len),
        d_pkt_len_current(0)
    {
        set_tag_propagation_policy(TPP_DONT);

        if (!set_access_code(access_code)) {
            GR_LOG_ERROR(d_logger, "access_code is > 64 bits");
            throw std::out_of_range ("access_code is > 64 bits");
        }

        std::stringstream str;
        str << name() << unique_id();
        d_me = pmt::string_to_symbol(str.str());
        d_key = pmt::string_to_symbol(tag_name);

        d_state = STATE_SYNC_SEARCH;
        d_pkt_count = 0;
    }

    correlate_access_code_tag_fixed_impl::~correlate_access_code_tag_fixed_impl()
    {
    }

    bool
    correlate_access_code_tag_fixed_impl::set_access_code(const std::string &access_code)
    {
        d_len = access_code.length();
        if (d_len > 64)
            return false;

        // Set len least significant bits to 1.
        d_mask = ((~0ULL) >> (64 - d_len));

        d_access_code = 0;
        for (unsigned i = 0; i < d_len; i++)
            d_access_code = (d_access_code << 1) | (access_code[i] & 1);

        return true;
    }

    void
    correlate_access_code_tag_fixed_impl::set_packet_len(unsigned int pkt_len)
    {
        d_pkt_len = pkt_len;
    }

    void
    correlate_access_code_tag_fixed_impl::set_threshold(unsigned int threshold)
    {
        d_threshold = threshold;
    }

    int
    correlate_access_code_tag_fixed_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 unsigned char *in = (const unsigned char*)input_items[0];
        unsigned char *out = (unsigned char*)output_items[0];

        uint64_t abs_out_sample_cnt = nitems_written(0);

        int nprod = 0;
        int count = 0;

        while (count < noutput_items) {
            switch (d_state) {
            case STATE_SYNC_SEARCH:
                // Shift in new data
                d_data_reg = (d_data_reg << 1) | ((in[count++]) & 0x1);

                // Compute hamming distance between desired access code and current data
                uint64_t wrong_bits, nwrong;
                wrong_bits = (d_data_reg ^ d_access_code) & d_mask;
                volk_64u_popcnt(&nwrong, wrong_bits);

                // If number of wrong bits is below threshold, begin packet
                if (nwrong <= d_threshold) {
                    // Latch packet length here, since the flowgraph will crash if the
                    // length is changed in the middle of a frame because the output size
                    // will not match the tagged stream tag.
                    d_pkt_len_current = d_pkt_len;
                    d_pkt_count = 0;
                    d_state = STATE_SYNC_FOUND;
                }

                break;

            case STATE_SYNC_FOUND:
                // Emit tag for tagged stream on first packet bit
                if (d_pkt_count == 0)
                    add_item_tag(0, abs_out_sample_cnt + nprod, d_key, pmt::from_long(d_pkt_len_current), d_me);

                // Output data bytes and then go back to search
                if (d_pkt_count < d_pkt_len_current) {
                    out[nprod++] = in[count++];
                    d_pkt_count++;
                } else {
                    d_state = STATE_SYNC_SEARCH;
                }

                break;
            }
        }

        consume_each(noutput_items);
        return nprod;
    }

  } /* namespace satlab */
} /* namespace gr */
