/* -*- 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, 0.0, 0.0, 0.0)
			{
				
			}
			Peak(uint64_t _id, int _pos, float bandwidth, float fcenter, float frequency)
			: Peak(_id, _pos, 0, bandwidth, fcenter, frequency, 0.0, 0.0)
			{
				
			}
			Peak(uint64_t _id, int _pos, int _pbh, float _frequency_resolution, float _fcenter, float _frequency, float _height_rel, float _height_abs)
			: id(_id)
			, isModified(false)
			, state(0)
			, pos(_pos)
			, pbh(_pbh)
			, frequency_resolution(_frequency_resolution)
			, fcenter(_fcenter)
			, frequency(_frequency)
			, height_rel(_height_rel)
			, height_rel_last(_height_rel)
			, height_abs(_height_abs)
			, height_abs_last(_height_abs)
			{
				
			}
			uint64_t id;
			bool isModified;
			int state;
			int pos;
			int pbh;
			float height_rel;
			float height_abs;
			float height_rel_last;
			float height_abs_last;
			float frequency_resolution;
			float fcenter;
			float frequency;
			
			bool isInRange(const Peak &other) const
			{
				return (other.pos >= left()) and (other.pos <= right());
			}
			
			bool isIntersect(const Peak &other) const
			{
				return (right() >= other.left()) and (other.right() >= left());
			}
			
			void heightUpdate_rel(float const *pData, int length)
			{
				if (pbh)
				{
					int newPos = -1;
					float newHeight = -100.f;
					for (int i=left(); i <= right(); i++)
					{
						if (newHeight < pData[i])
						{
							newPos = i;
							newHeight = pData[i];
						}
					}

					if (pos != newPos)
					{
						isModified = true;
						pos = newPos;
					}

					height_rel = newHeight;
					if (std::fabs(height_rel_last-newHeight) >= 3.0f)
					{
						isModified = true;
						height_rel_last = height_rel;
					}
				}
			}
			
			void heightUpdate_abs(float const *pData, int length)
			{
				if (pbh)
				{
					int newPos = -1;
					float newHeight = -100.f;
					for (int i=left(); i <= right(); i++)
					{
						if (newHeight < pData[i])
						{
							newPos = i;
							newHeight = pData[i];
						}
					}

					if (pos != newPos)
					{
						isModified = true;
						pos = newPos;
					}

					height_abs = newHeight;
					if (std::fabs(height_abs_last-newHeight) >= 3.0f)
					{
						isModified = true;
						height_abs_last = height_abs;
					}

				}
			}

			bool makeBox(int pbh_min, int pbh_max, float const *pData, int length)
			{
				pbh = 0;
				int left = pos;
				int right = pos;
				float nom = 10;
				int left_found = 0;
				int right_found = 0;
				while(!(left_found & right_found))
				{
					// Determine peak width left from center (pos)
					if (pData[left] > nom)
					{
						if (left > 0)
						{
							left--;
						}
						else
						{
							break;
						}
					}
					else
					{
						left_found = 1;
					}
					
					// Determine peak width right from center (pos)
					if (pData[right] > nom)
					{
						if (right < (length-1))
						{
							right++;
						}
						else
						{
							break;
						}
					}
					else
					{
						right_found = 1;
					}
					
					int pbh_left = pos - left;
					int pbh_right = right - pos;

					// Take larger peak width
					pbh = std::max(pbh_left, pbh_right);

					// Ensure peak box is at least pb_min
					pbh = std::max(pbh, pbh_min);
					
					if (pbh > pbh_max)
					{
						pbh = 0;
						break;
					}
				}
//				isModified = true;
				return pbh > 0;
			}
			
			bool update(const Peak &other)
			{
				if (std::fabs(height_rel_last-other.height_rel) >= 3.0f)
				{
					height_rel_last = other.height_rel;
					isModified = true;
				}
				if (std::fabs(height_abs-other.height_abs) >= 3.0f)
				{
					height_abs_last = other.height_abs;
					isModified = true;
				}
				if (pos != other.pos)
				{
					isModified = true;
				}
				if (pbh != other.pbh)
				{
					isModified = true;
				}
				fcenter = other.fcenter;
				frequency = other.frequency;
				pos = other.pos;
				pbh = other.pbh;
				height_rel = other.height_rel;
				height_abs = other.height_abs;
				return isModified;
			}
			
			int left() const
			{
				return pos - pbh;
			}
			
			int right() const
			{
				return pos + pbh;
			}
			
			pmt::pmt_t msg()
			{
				pmt::pmt_t dict = pmt::make_dict();
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("id")), pmt::from_long(id));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("pos")), pmt::from_long(pos));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("pbh_Hz")), pmt::from_float((float)pbh*frequency_resolution));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("freq_abs_Hz")), pmt::from_float(fcenter + frequency));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("freq_rel_Hz")), pmt::from_float(frequency));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("level_abs")), pmt::from_float(height_abs));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("level_rel")), pmt::from_float(height_rel));
				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("state")), pmt::from_long(state));
				isModified = false;
				return dict;
			}
			
			void setState(int _state)
			{
				if (_state != state)
				{
					isModified = true;
				}
				state = _state;
			}
		
		};
		
	private:
		boost::mutex m_mutex;
		
		void check_pair(pmt::pmt_t pair)
		{
			pmt::pmt_t key = pmt::car(pair);
			pmt::pmt_t val = pmt::cdr(pair);

			// Check pair for data
			if (pmt::symbol_to_string(key) == std::string("freq"))
			{
				m_fcenter = pmt::to_float(val);
			}
		}
		void set_msg(pmt::pmt_t msg)
		{
			if (pmt::is_dict(msg))
			{
				pmt::pmt_t items = pmt::dict_items(msg);
				try
				{
					size_t len_items = pmt::length(items);
					for (int i=0; i < len_items; i++)
					{	
						pmt::pmt_t pair = pmt::nth(i, items);
						check_pair(pair);
					}
				}
				catch (const std::exception& e)
				{
					check_pair(msg);
				}
			}
		}
		
		Peak peakCheck(float const *pData, Peak const &peak, float thresh)
		{
			Peak result;
			
	
			return result;
		}
		
		bool addToListByPos(std::vector<Peak>&list, Peak peak)
		{
			bool found = false;
			for (std::vector<Peak>::iterator it = list.begin(); it != list.end(); it++)
			{
				if (it->pos == peak.pos)
				{
					found = true;
					break;
				}
			}
			
			if (!found)
			{
				list.push_back(peak);
			}
			return !found;
		}
		
		int m_vlen;
		float m_framerate;
		float m_bandwidth;
		float m_fcenter;
		float m_peak_thresh;
		float m_peak_hysteresis;
		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;
		pmt::pmt_t m_msg_port_in;
		pmt::pmt_t m_msg_port_out;
		
	public:
		peak_detect_impl(int vlen, float framerate, float bandwidth, float fcenter, float peak_thresh, float peak_hysteresis, float peak_width_min, float peak_width_max, float alpha_s, float alpha_m);
		~peak_detect_impl();

		// Where all the action really happens
		int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
		void forecast (int noutput_items, gr_vector_int &ninput_items_required);
		
	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);
		
		// Callbacks
		void set_bandwidth(float bandwidth_Hz) override;
		void set_centerFrequency(float freq_Hz) override;
		void set_peakThresh(float thresh_dB) override;
		void set_peakHysteresis(float hysteresis_dB) override;
		void set_peakWidthMin(float width_Hz) override;
		void set_peakWidthMax(float width_Hz) override;

		void sort(std::vector<Peak>&peaks)
		{
			int n = peaks.size();
			do
			{
				int newn = 1;
				for (int i=0; i < (n-1); i++)
				{
					if (peaks[i].height_rel < peaks[i+1].height_rel)
					{
						Peak temp = peaks[i+1];
						peaks[i+1] = peaks[i];
						peaks[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 */