/* -*- c++ -*- */
/*
 * Copyright 2004 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.
 */

/*
 * gr-baz by Balint Seeber (http://spench.net/contact)
 * Information, documentation & samples: http://wiki.spench.net/wiki/gr-baz
 */

/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <baz_correlator.h>
#include <gnuradio/io_signature.h>

#include <volk/volk.h>

#include <fstream>

/*
 * Create a new instance of baz_correlator and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
baz_correlator_sptr 
baz_make_correlator(
    float samp_rate,
    float symbol_rate,
    int window_length,
    float threshold,
    int width,
    const char* sync_path,
    int sync_length,
    int sync_offset,
    //sync_dtype='c8',
    int sync_window_length
)
{
  return baz_correlator_sptr(new baz_correlator(
    samp_rate,
    symbol_rate,
    window_length,
    threshold,
    width,
    sync_path,
    sync_length,
    sync_offset,
    //sync_dtype='c8',
    sync_window_length
));
}

/*
 * Specify constraints on number of input and output streams.
 * This info is used to construct the input and output signatures
 * (2nd & 3rd args to gr::block's constructor).  The input and
 * output signatures are used by the runtime system to
 * check that a valid number and type of inputs and outputs
 * are connected to this block.  In this case, we accept
 * only 1 input and 1 output.
 */
static const int MIN_IN = 1;	// mininum number of input streams
static const int MAX_IN = 1;	// maximum number of input streams
static const int MIN_OUT = 1;	// minimum number of output streams
static const int MAX_OUT = 2;	// maximum number of output streams

/*
 * The private constructor
 */
baz_correlator::baz_correlator(
    float samp_rate,
    float symbol_rate,
    int window_length,
    float threshold,
    int width,
    const char* sync_path,
    int sync_length,
    int sync_offset,
    //sync_dtype='c8',
    int sync_window_length
)
	: gr::block("correlator",
		gr::io_signature::make(MIN_IN,  MAX_IN,  sizeof(std::complex<float>)),
		gr::io_signature::make2(MIN_OUT, MAX_OUT, sizeof(float) * width, sizeof(float))),
	d_samp_rate(samp_rate),
    d_symbol_rate(symbol_rate),
    d_window_length(window_length),
    d_threshold(threshold),
    d_width(width),
    d_sync_window_length(sync_window_length),

    d_synced(false),
    d_current_idx(0),
    d_next_window_idx(0),
    d_max_peak(0),
    d_max_peak_idx(-1),
    d_sync_window_idx(-1),
    d_current_item_idx(0)
{
	const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
    set_alignment(std::max(1, alignment_multiple));

  	float relative_rate = /*(float)width*/1.0f / (samp_rate / symbol_rate * (float)window_length);
	set_relative_rate(relative_rate);

	fprintf(stderr, "[%s<%ld>] sample rate: %f, symbole rate: %f, window length: %d, threshold: %f, width: %d, sync path: \"%s\", sync length: %d, sync offset: %d, sync window length: %d, relative rate: %f\n", name().c_str(), unique_id(),
		samp_rate,
	    symbol_rate,
	    window_length,
	    threshold,
	    width,
	    sync_path,
	    sync_length,
	    sync_offset,
	    sync_window_length,

	    relative_rate
	);

	int res = 0;

	const int item_size = sizeof(std::complex<float>);
	d_sync.resize(sync_length);

	std::ifstream f(sync_path, std::ifstream::in | std::ifstream::binary);
	if (f.is_open() == false)
		throw std::runtime_error(boost::str(boost::format("failed to open: \"%s\"") % sync_path));
	f.seekg(sync_offset * item_size);
	std::istream::streampos posStart = f.tellg();
    // FIXME: Read into buffer from volk_malloc
	f.read((char*)&d_sync[0], sync_length * item_size);
	std::istream::streampos posEnd = f.tellg();
	int samples_read = (posEnd - posStart) / item_size;
	fprintf(stderr, "[%s<%ld>] read %d sync samples\n", name().c_str(), unique_id(), samples_read);

    if (samples_read < sync_length)
        throw std::runtime_error("not able to read all sync samples");

    // FIXME: volk_malloc
	d_conjmul_result.resize(sync_length);
    //d_abs_result.resize(sync_length);

    // FIXME: Msg port to restart search
}

/*
 * Our virtual destructor.
 */
baz_correlator::~baz_correlator()
{
}

std::complex<float> baz_correlator::correlate(const std::complex<float>* in, const std::complex<float>* sync)
{
  	//volk_32fc_x2_multiply_conjugate_32fc(&d_conjmul_result[0], in, sync, d_conjmul_result.size());

    std::complex<float> c(0,0);
    for (size_t n = 0; n < d_sync.size(); ++n)
    {
        //d_conjmul_result[n] = in[n] * std::conj(sync[n]);   // FIXME: Pre-compute conj
        //d_abs_result[n] = std::abs(d_conjmul_result[n]);
        c += (in[n] * std::conj(sync[n]));
    }

  	//volk_32fc_magnitude_32f

  	//volk_32f_accumulator_s32f

  	//volk_32f_index_max_16u

  	//for (size_t i = 0; i < d_conjmul_result.size(); i++)
  	//	c += d_conjmul_result[i];

  	//return c / (float)d_conjmul_result.size();
    return c;
}

int baz_correlator::general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
  	const std::complex<float>* in = (const std::complex<float> *)input_items[0];
  	float* out = (float *) output_items[0];
  	float* out_corr = NULL;
  	if (output_items.size() > 1)
  		out_corr = (float*)output_items[1];

  	uint64_t read = nitems_read(0);
  	int in_items = ninput_items[0];

  	int out_corr_produced = 0;

  	if (d_synced == false)
  	{
        if (in_items < d_sync.size())
            return 0;   // Need more inputs FIXME: forcast?

        int64_t sync_limit = (in_items - (d_sync.size() - 1));  // [0, limit)

        size_t n = 0;

  		for (; n < sync_limit; n++)
  		{
			std::complex<float> c = correlate(in + n, &d_sync[0]);

			float f = std::abs(c);

			if (out_corr)
			{
				out_corr[out_corr_produced] = f;
				++out_corr_produced;
			}

			if (f > d_threshold)
            {
                if (d_sync_window_idx == -1)
                {
                    d_sync_window_idx = d_sync_window_length;
                    d_max_peak = 0;
                    d_max_peak_idx = -1;
                }
            }

            if (d_sync_window_idx > -1)
            {
                if (f > d_max_peak)
                {
                    d_max_peak = f;
                    d_max_peak_idx = read + n;
                }

                --d_sync_window_idx;
            }
            
            if ((d_sync_window_idx == -1) && (d_max_peak_idx > -1))
            {
                d_synced = true;
                d_current_idx = d_max_peak_idx;
                d_next_window_idx = d_max_peak_idx + d_window_length + (d_width / 2);
                d_current_item_idx = 0;
                d_max_peak_idx = -1;

                ++n;
                break;
            }
		}

        consume(0, n);
  	}
  	else
  	{
        int64_t offset = d_next_window_idx - read;

        if (offset <= 0)
        {
            if (in_items < d_sync.size())
                return 0;   // Need more input FIXME: forcast?

            int64_t sync_limit = (in_items - (d_sync.size() - 1));  // [0, limit)
            int64_t width_limit = offset + (d_width - 1) + d_sync.size();

            int64_t n = 0;

            d_current_item_idx = -offset;

            for (; n < std::min(std::min(sync_limit, width_limit), (int64_t)noutput_items); n++)
            {
                std::complex<float> c = correlate(in + n, &d_sync[0]);

                out[n] = std::abs(c);

                if (-offset == ((d_width / 2) - (d_sync_window_length / 2)))
                {
                    d_sync_window_idx = d_sync_window_length;
                    d_max_peak = 0;
                    d_max_peak_idx = -1;
                }

                if (d_sync_window_idx > -1)
                {
                    if (out[n] > d_max_peak)
                    {
                        d_max_peak = out[n];
                        d_max_peak_idx = n;
                    }

                    --d_sync_window_idx;
                }
            }

            produce(0, n);

            if ((-offset + n) == d_width)
            {
                d_current_idx = d_next_window_idx;
                d_next_window_idx += d_window_length;

                if (true)
                {
                    int adj = d_max_peak_idx - (d_sync_window_length / 2);
                    d_next_window_idx += adj;
                }

                d_current_item_idx = 0;
                d_max_peak_idx = -1;
            }
        }
        else
        {
            consume(0, std::min((int64_t)in_items, offset));
        }
  	}

  	if (out_corr_produced > 0)
  		produce(1, out_corr_produced);

	return WORK_CALLED_PRODUCE;	// Tell runtime system how many output items we produced.
}
