/* -*- c++ -*- */
/* 
 * Copyright 2019 Jay Arrowfield.
 * 
 * This 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.
 * 
 * This software 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 this software; 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/io_signature.h>
#include "peak_detect_impl.h"

namespace gr {
  namespace jay {

    peak_detect::sptr
    peak_detect::make(float alpha_s, float alpha_m, int vlen)
    {
      return gnuradio::get_initial_sptr
        (new peak_detect_impl(alpha_s, alpha_m, vlen));
    }

    /*
     * The private constructor
     */
    peak_detect_impl::peak_detect_impl(float alpha_s, float alpha_m, int vlen)
		: gr::sync_block("peak_detect"
		, gr::io_signature::make(1, 1, vlen*sizeof(float))
        , gr::io_signature::make(1, 1, vlen*sizeof(float)))
		, m_alpha_s (alpha_s)
		, m_alpha_m(alpha_m)
		, m_frameCount(0)
    {
		m_pPeak = new int[vlen];
		m_pXs = new float[vlen];
		m_pXm = new float[vlen];
		
		memset(m_pPeak, 0, vlen*sizeof(int));
		memset(m_pXs, 0, vlen*sizeof(float));
		memset(m_pXm, 0, vlen*sizeof(float));
		
		std::stringstream str;
		str << name() << unique_id();
		m_tag_id = pmt::string_to_symbol(str.str());
	}

    /*
     * Our virtual destructor.
     */
    peak_detect_impl::~peak_detect_impl()
    {
		delete [] m_pXm;
		delete [] m_pXs;
		delete [] m_pPeak;
    }

	float peak_detect_impl::mean(float* pSrc, int length)
	{
		float res = 0;
		for (int i=0; i < length; i++)
		{
			res += pSrc[i];
		}
		return res / length;
	}

	void peak_detect_impl::fill(float* pDst, int length, float value)
	{
		for (int i=0; i < length; i++)
		{
			pDst[i] = value;
		}
	}

    int
    peak_detect_impl::work(int noutput_items,
        gr_vector_const_void_star &input_items,
        gr_vector_void_star &output_items)
    {
		int nitems_per_block = this->output_signature()->sizeof_stream_item(0)/sizeof(float);

		float *iptr = (float *)input_items[0];
		float *optr = (float *)output_items[0];
		
		for (int k = 0; k < noutput_items; k++)
		{
			// Initialization on first frame
			if (m_frameCount == 0)
			{
				fprintf(stderr, "Peak Detector: Initialize\n");
				fprintf(stderr, "noutput_items = %u\n", (int)noutput_items);
				fprintf(stderr, "ninput_items = %u\n", (int)input_items.size());
				fprintf(stderr, "nitems_per_block = %u\n", (int)nitems_per_block);
			
				float xmean = mean(iptr, nitems_per_block);
				fill(m_pXs, nitems_per_block, xmean);
				fill(m_pXm, nitems_per_block, xmean);
			}
			
			// Peak detection
			for (int i = 0; i < nitems_per_block; i++)
			{
				float x = *(iptr++);
				
				// Update of Xs
				m_pXs[i] = (1.f-m_alpha_s)*m_pXs[i] + m_alpha_s*x;
				
				// Update of Xm
				m_pXm[i] = (1.f-m_alpha_m)*m_pXm[i] + m_alpha_m*x;
	
				// Conditional update of Xm
				float last = m_pXm[0];
				for (int j=1; j < nitems_per_block; j++)
				{
					if (m_pXm[j] < (last+2))
					{
						last = (1.f-m_alpha_m)*last + m_alpha_m*m_pXm[j];
					}
					else
					{
						m_pXm[j] = last;	
					}
				}
				
				float peak_dist = x-m_pXm[i];
				int peak = (int)(peak_dist > 10.f);
				m_pPeak[i] = peak;
				
				if (peak)
				{
					pmt::pmt_t tag_key = pmt::string_to_symbol("peak");
					pmt::pmt_t tag_value = pmt::from_float(peak_dist);
					add_item_tag(0, nitems_written(0) + i, tag_key, tag_value, m_tag_id);
				}
				
				*optr++ = x-m_pXm[i];
			}
			m_frameCount++;
		}

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

  } /* namespace jay */
} /* namespace gr */