//$ nobt
//$ nocpp

/**
 * @file r8butil.h
 *
 * @brief The inclusion file with several utility functions.
 *
 * This file includes several utility functions used by various utility
 * programs like "calcErrorTable.cpp".
 *
 * r8brain-free-src Copyright (c) 2013-2014 Aleksey Vaneev
 * See the "License.txt" file for license.
 */

#ifndef R8BUTIL_INCLUDED
#define R8BUTIL_INCLUDED

#include "r8bbase.h"

namespace r8b
{

	/**
	 * @param re Real part of the frequency response.
	 * @param im Imaginary part of the frequency response.
	 * @return A magnitude response value converted from the linear scale to the
	 * logarithmic scale.
	 */

	inline double convertResponseToLog(const double re, const double im) { return (4.34294481903251828 * log(re * re + im * im + 1e-100)); }

	/**
	 * An utility function that performs frequency response scanning step update
	 * based on the current magnitude response's slope.
	 *
	 * @param[in,out] step The current scanning step. Will be updated on
	 * function's return. Must be a positive value.
	 * @param curg Squared magnitude response at the current frequency point.
	 * @param[in,out] prevg_log Previous magnitude response, log scale. Will be
	 * updated on function's return.
	 * @param prec Precision multiplier, affects the size of the step.
	 * @param maxstep The maximal allowed step.
	 * @param minstep The minimal allowed step.
	 */

	inline void updateScanStep(double& step, const double curg, double& prevg_log, const double prec, const double maxstep, const double minstep = 1e-11)
	{
		double curg_log = 4.34294481903251828 * log(curg + 1e-100);
		curg_log += (prevg_log - curg_log) * 0.7;

		const double slope = fabs(curg_log - prevg_log);
		prevg_log          = curg_log;

		if (slope > 0.0)
		{
			step /= prec * slope;
			step = max(min(step, maxstep), minstep);
		}
	}

	/**
	 * Function locates normalized frequency at which the minimum filter gain
	 * is reached. The scanning is performed from lower (left) to higher
	 * (right) frequencies, the whole range is scanned.
	 *
	 * Function expects that the magnitude response is always reducing from lower
	 * to high frequencies, starting at "minth".
	 *
	 * @param flt Filter response.
	 * @param fltlen Filter response's length in samples (taps).
	 * @param[out] ming The current minimal gain (squared). On function's return
	 * will contain the minimal gain value found (squared).
	 * @param[out] minth The normalized frequency where the minimal gain is
	 * currently at. On function's return will point to the normalized frequency
	 * where the new minimum was found.
	 * @param thend The ending frequency, inclusive.
	 */

	inline void findFIRFilterResponseMinLtoR(const double* const flt,
											 const int fltlen, double& ming, double& minth, const double thend)
	{
		const double maxstep = minth * 2e-3;
		double curth         = minth;
		double re;
		double im;
		calcFIRFilterResponse(flt, fltlen, M_PI * curth, re, im);
		double prevg_log = convertResponseToLog(re, im);
		double step      = 1e-11;

		while (true)
		{
			curth += step;

			if (curth > thend) { break; }

			calcFIRFilterResponse(flt, fltlen, M_PI * curth, re, im);
			const double curg = re * re + im * im;

			if (curg > ming)
			{
				ming  = curg;
				minth = curth;
				break;
			}

			ming  = curg;
			minth = curth;

			updateScanStep(step, curg, prevg_log, 0.31, maxstep);
		}
	}

	/**
	 * Function locates normalized frequency at which the maximal filter gain
	 * is reached. The scanning is performed from lower (left) to higher
	 * (right) frequencies, the whole range is scanned.
	 *
	 * Note: this function may "stall" in very rare cases if the magnitude
	 * response happens to be "saw-tooth" like, requiring a very small stepping to
	 * be used. If this happens, it may take dozens of seconds to complete.
	 *
	 * @param flt Filter response.
	 * @param fltlen Filter response's length in samples (taps).
	 * @param[out] maxg The current maximal gain (squared). On function's return
	 * will contain the maximal gain value (squared).
	 * @param[out] maxth The normalized frequency where the maximal gain is
	 * currently at. On function's return will point to the normalized frequency
	 * where the maximum was reached.
	 * @param thend The ending frequency, inclusive.
	 */

	inline void findFIRFilterResponseMaxLtoR(const double* const flt,
											 const int fltlen, double& maxg, double& maxth, const double thend)
	{
		const double maxstep = maxth * 1e-4;
		double premaxth      = maxth;
		double premaxg       = maxg;
		double postmaxth     = maxth;
		double postmaxg      = maxg;

		double prevth = maxth;
		double prevg  = maxg;
		double curth  = maxth;
		double re;
		double im;
		calcFIRFilterResponse(flt, fltlen, M_PI * curth, re, im);
		double prevg_log = convertResponseToLog(re, im);
		double step      = 1e-11;

		bool WasPeak       = false;
		int AfterPeakCount = 0;

		while (true)
		{
			curth += step;

			if (curth > thend) { break; }

			calcFIRFilterResponse(flt, fltlen, M_PI * curth, re, im);
			const double curg = re * re + im * im;

			if (curg > maxg)
			{
				premaxth       = prevth;
				premaxg        = prevg;
				maxg           = curg;
				maxth          = curth;
				WasPeak        = true;
				AfterPeakCount = 0;
			}
			else if (WasPeak)
			{
				if (AfterPeakCount == 0)
				{
					postmaxth = curth;
					postmaxg  = curg;
				}

				if (AfterPeakCount == 5)
				{
					// Perform 2 approximate binary searches.

					for (int k = 0; k < 2; ++k)
					{
						double l     = (k == 0 ? premaxth : maxth);
						double curgl = (k == 0 ? premaxg : maxg);
						double r     = (k == 0 ? maxth : postmaxth);
						double curgr = (k == 0 ? maxg : postmaxg);

						while (true)
						{
							const double c = (l + r) * 0.5;
							calcFIRFilterResponse(flt, fltlen, M_PI * c, re, im);

							const double curgTmp = re * re + im * im;

							if (curgl > curgr)
							{
								r     = c;
								curgr = curgTmp;
							}
							else
							{
								l     = c;
								curgl = curgTmp;
							}

							if (r - l < 1e-11)
							{
								if (curgl > curgr)
								{
									maxth = l;
									maxg  = curgl;
								}
								else
								{
									maxth = r;
									maxg  = curgr;
								}

								break;
							}
						}
					}

					break;
				}

				AfterPeakCount++;
			}

			prevth = curth;
			prevg  = curg;

			updateScanStep(step, curg, prevg_log, 1.0, maxstep);
		}
	}

	/**
	 * Function locates normalized frequency at which the specified maximum
	 * filter gain is reached. The scanning is performed from higher (right)
	 * to lower (left) frequencies, scanning stops when the required gain
	 * value was crossed. Function uses an extremely efficient binary search and
	 * thus expects that the magnitude response has the "main lobe" form produced
	 * by windowing, with a minimal pass-band ripple.
	 *
	 * @param flt Filter response.
	 * @param fltlen Filter response's length in samples (taps).
	 * @param maxg Maximal gain (squared).
	 * @param[out] th The current normalized frequency. On function's return will
	 * point to the normalized frequency where "maxg" is reached.
	 * @param thend The leftmost frequency to scan, inclusive.
	 */

	inline void findFIRFilterResponseLevelRtoL(const double* const flt, const int fltlen, const double maxg, double& th, const double thend)
	{
		// Perform exact binary search.

		double l = thend;
		double r = th;

		while (true)
		{
			const double c = (l + r) * 0.5;

			if (r - l < 1e-14)
			{
				th = c;
				break;
			}

			double re;
			double im;
			calcFIRFilterResponse(flt, fltlen, M_PI * c, re, im);
			const double curg = re * re + im * im;

			if (curg > maxg) { l = c; }
			else { r = c; }
		}
	}
} // namespace r8b

#endif // R8BUTIL_INCLUDED