/* -*- 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.
 */

#ifndef INCLUDED_JAY_PEAK_DETECT_IMPL_H
#define INCLUDED_JAY_PEAK_DETECT_IMPL_H

#include <jay/peak_detect.h>

namespace gr {
namespace jay {

    class peak_detect_impl : public peak_detect
    {
		struct Peak
		{
			Peak()
			: Peak(0, 0, 0, 0.0, 0.0)
			{
				
			}
			Peak(uint64_t _id, int _pos, int _half_width, float _height_rel, float _height_abs)
			: id(_id)
			, pos(_pos)
			, half_width(_half_width)
			, height_rel(_height_rel)
			, height_abs(_height_abs)
			, isValid(true)
			{
				
			}
			uint64_t id;
			int pos;
			int half_width;
			float height_rel;
			float height_abs;
			bool isValid;
			
			bool isInRange(const Peak &other)
			{
				return (other.pos >= left()) and (other.pos <= right());
			}
			
			const Peak& update(const Peak &other)
			{
				float beta = 0.1f;
				float alpha = (1.f-beta);
				pos = alpha*pos + beta*other.pos;
				half_width = alpha*half_width + beta*other.half_width;
				height_rel = std::max(height_rel, other.height_rel);
				height_abs = std::max(height_abs, other.height_abs);
				isValid = true;
				return *this;
			}
			
			int left()
			{
				return pos - half_width;
			}
			
			int right()
			{
				return pos + half_width;
			}
		};
		
	private:
	
		Peak peakCheck(float const *pData, Peak const &peak, float thresh)
		{
			Peak result;
			
	
			return result;
		}
		
		bool addToList(std::vector<int>&list, int pos)
		{
			bool found = false;
			for (std::vector<int>::iterator it = list.begin(); it != list.end(); it++)
			{
				if (*it == pos)
				{
					found = true;
					break;
				}
			}
			
			if (!found)
			{
				list.push_back(pos);
			}
			return !found;
		}
		
		float m_framerate;
		float m_peak_height_min;
		int m_peak_width_min;
		int m_peak_width_max;
		float m_alpha_s;
		float m_alpha_m;
		float *m_pX;
		float *m_pXs;
		float *m_pXm;
		float *m_pXd;
		float *m_pXds;
		float *m_pPeakBox_rel;
		float *m_pPeakBox_abs;
		uint64_t m_frameCount;
		pmt::pmt_t m_tag_id;
		std::vector<Peak> m_peakList;
		std::vector<Peak> m_peakHistory;
		std::vector<Peak> temp;
		uint64_t m_peak_id;
		
	public:
		peak_detect_impl(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m);
		~peak_detect_impl();

		// Where all the action really happens
		int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
	private:
		void fill(float *pDst, int length, float value);
		float mean(float *pSrc, int length);
		std::vector<int> findPeakPos(float* pSrc, int length, float thresh);
		
		template <typename T>
		inline bool cmp(const T a, const T b)
		{
			return (a < b);
		}
		
		template <typename T>
		void sort(T const * const pData, std::vector<int>&pos)
		{
			int n = pos.size();
			do
			{
				int newn = 1;
				for (int i=0; i < (n-1); i++)
				{
					if (cmp(pData[pos[i]], pData[pos[i+1]]))
					{
						int temp = pos[i+1];
						pos[i+1] = pos[i];
						pos[i] = temp;
						newn = i+1;
					} // ende if
				} // ende for
				n = newn;
			} while (n > 1);
		}
		
		
    };

} // namespace jay
} // namespace gr

#endif /* INCLUDED_JAY_PEAK_DETECT_IMPL_H */