/* -*- 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_radar_detector.h>
#include <gnuradio/io_signature.h>

#include <stdio.h>

/*
 * Create a new instance of baz_radar_detector and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
baz_radar_detector_sptr baz_make_radar_detector (int sample_rate, gr::msg_queue::sptr msgq)
{
	return baz_radar_detector_sptr (new baz_radar_detector (sample_rate, msgq));
}

/*
 * 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 = 2;	// maximum number of input streams
static const int MIN_OUT = 0;	// minimum number of output streams
static const int MAX_OUT = 1;	// maximum number of output streams

/*
 * The private constructor
 */
baz_radar_detector::baz_radar_detector (int sample_rate, gr::msg_queue::sptr msgq)
  : gr::block ("radar_detector",
		gr::io_signature::make (MIN_IN, MAX_IN, sizeof(float)),
		gr::io_signature::make (MIN_OUT, MAX_OUT, sizeof(float)))
	, d_sample_rate(sample_rate)
	, d_msgq(msgq)
	, d_base_level(0.0)
	, d_threshold(1.0)
	, d_in_burst(false)
	, d_first(0.0)
	, d_burst_start(0)
	, d_burst_flat_start(0)
	, d_sum(0.0)
	, d_max(0.0)
	, d_skip(0)
	, d_pulse_plateau(1.0)
	, d_last(0.0)
{
	fprintf(stderr, "[%s<%li>] sample rate: %i\n", name().c_str(), unique_id(), sample_rate);
}

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

void baz_radar_detector::set_base_level(float level)
{
	d_base_level = level;
}

void baz_radar_detector::set_threshold(float threshold)
{
	d_threshold = pow(10.0f, (threshold / 10.0f));
	
	fprintf(stderr, "[%s<%li>] threshold %f\n", name().c_str(), unique_id(), d_threshold);
}

void baz_radar_detector::set_pulse_plateau(float level)
{
	d_pulse_plateau = pow(10.0f, (level / 10.0f));
	
	fprintf(stderr, "[%s<%li>] pulse plateau %f\n", name().c_str(), unique_id(), d_pulse_plateau);
}

bool baz_radar_detector::set_param(const std::string& param, float value)
{
	fprintf(stderr, "[%s<%li>] setting parameter \"%s\" to: %f\n", name().c_str(), unique_id(), param.c_str(), value);
	
	if (param == "FIRPWR")
	{
	}
	else if (param == "RSSI")
	{
	}
	else if (param == "PHEIGHT")
	{
	}
	else if (param == "PRSSI")
	{
	}
	else if (param == "INBAND")
	{
	}
	
	return true;
}

void baz_radar_detector::skip(int skip)
{
	fprintf(stderr, "[%s<%li>] skip %i\n", name().c_str(), unique_id(), skip);
	
	d_skip = skip;
}

int baz_radar_detector::general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
	const float *in = (const float*)input_items[0];
	const float *lvl = NULL;
	if (input_items.size() > 1)
		lvl = (const float*)input_items[1];

	int out_count = 0;
	float* out = NULL;
	if (output_items.size() > 0)
		out = (float*)output_items[0];

	for (int i = 0; i < noutput_items; i++)
	{
		if (d_skip > 0)
		{
			--d_skip;
			continue;
		}
		
		float base_level = (lvl ? lvl[i] : d_base_level);
		float threshold = base_level * d_threshold;
		
		if (in[i] >= threshold)
		{
			bool first = false;
			
			if (d_in_burst == false)
			{
				first = true;
				
				d_in_burst = true;
				d_first = in[i];
				d_burst_start = nitems_read(0) + i;
				d_sum = 0;
				d_max = 0;
				
				d_in_plateau = false;
				d_flat_sum = 0.0;
				d_flat_sum_count = 0;
			}
			
			if (d_in_burst)
			{
				d_sum += in[i];
				d_max = std::max(d_max, (double)in[i]);
				
				if (first == false)
				{
					if (d_in_plateau == false)
					{
						float diff = in[i] / d_last;
						
						if ((diff <= d_pulse_plateau) && (diff >= (1.0 / d_pulse_plateau)))
						{
							d_flat_sum += in[i];
							++d_flat_sum_count;
							d_in_plateau = true;
							d_burst_flat_start = nitems_read(0) + i;
							
							if (out)
								out[out_count++] = in[i];
						}
					}
					else
					{
						//uint64_t len = (nitems_read(0) + i) - d_burst_flat_start;
						double ave = d_flat_sum / (double)/*len*/d_flat_sum_count;
						double diff = in[i] / ave;
						
						if ((diff <= d_pulse_plateau) && (diff >= (1.0 / d_pulse_plateau)))
						{
							d_flat_sum += in[i];
							++d_flat_sum_count;
							
							if (out)
								out[out_count++] = in[i];
						}
						else
						{
							//d_in_plateau = false;
						}
					}
				}
				
				//if (out)
				//	out[out_count++] = in[i];
				
				d_last = in[i];
			}
		}
		else
		{
			if (d_in_burst)
			{
				d_in_burst = false;
				
				uint64_t len = (nitems_read(0) + i) - d_burst_start;
				
				struct ath5k_radar_error e;
				e.type = /*1*/0;
				e.subtype = 0;
				double tsf = (double)d_burst_start / ((double)d_sample_rate / 1e6);
				e.tsf = (uint32_t)(((int)tsf) % 0x7fff);
				
				double diff = (double)len / ((double)d_sample_rate / 1e6);
				int width = (int)/*ceil*/(diff/* * 1e6*/);
				width = std::min(width, 255);
				e.width = (uint8_t)width;	// 0: immediate, 255: timeout
				
				double rssi = (10.0 * log10(d_max)) - (10.0 * log10(/*threshold*/base_level));
				e.rssi = (uint8_t)ceil(rssi);
				
				double ave = d_sum / (double)len;
				
				//fprintf(stderr, "[%s<%i>] triggered level: %f, (RSSI: %f), length: %i (samples: %i), ave: %f, max: %f\n", name().c_str(), unique_id(),
				//	d_first, rssi, width, (int)len, ave, d_max);
				
				if (d_msgq)
				{
					if (d_msgq->full_p() == false)
					{
						gr::message::sptr msg = gr::message::make(0, 0, 0, sizeof(e));
						memcpy(msg->msg(), &e, sizeof(e));
						d_msgq->insert_tail(msg);
						msg.reset();
					}
					else
					{
						//fprintf(stderr, "[%s<%i>] message queue full\n", name().c_str(), unique_id());
					}
				}
			}
		}
	}
	
	consume_each(noutput_items);
	
	return out_count;
}
