/*
 *
 *
 *    tpfdisc.h
 *
 *    Twin-peak filter discriminator.
 *
 *    Copyright (C) 2017 by Matt Roberts.
 *    License: GNU GPL3 (www.gnu.org)
 *
 *
 */

#ifndef __KK5JY_TPFDISC_H
#define __KK5JY_TPFDISC_H

#include <stdint.h>
#include <math.h>

#include "cavg.h"
#include "favg.h"
#include "es.h"
#include "resiir.h"
#include "nlimits.h"
#include "hysteresis.h"
#include "sign.h"
#include "ops.h"

// how many bit-times in the time constant for the selective fading detector decay time
#define DEFAULT_SF_DETECTOR_DIVISOR (10)

namespace KK5JY {
	namespace DSP {
		//
		//  class TPOperations - template operations used by the discriminator
		//
		template <typename sample_t>
		class TPOperations {
			public:
				static sample_t compute_angle(sample_t a, sample_t b);
				static sample_t trigger_rail();
		};

		// TPOperations<sample_t>::compute_angle
		template<typename sample_t>
		inline sample_t TPOperations<sample_t>::compute_angle(sample_t a, sample_t b) {
			// just use arithmetic difference for integral types
			return a - b;
		}

		// TPOperations<float>::compute_angle
		template<>
		inline float TPOperations<float>::compute_angle(float a, float b) {
			return atan(a / b) - (M_PI / 4.0);
		}

		// TPOperations<double>::compute_angle
		template<>
		inline double TPOperations<double>::compute_angle(double a, double b) {
			return atan(a / b) - (M_PI / 4.0);
		}

		// TPOperations<sample_t>::trigger_rail
		template<typename sample_t>
		inline sample_t TPOperations<sample_t>::trigger_rail() {
			return 0.707 * norm_limits<sample_t>::maximum;
		}

		// TPOperations<float>::trigger_rail
		template<>
		inline float TPOperations<float>::trigger_rail() {
			return M_PI / 4.0;
		}

		// TPOperations<double>::trigger_rail
		template<>
		inline double TPOperations<double>::trigger_rail() {
			return M_PI / 4.0;
		}

		//
		//
		//    class TwinPassbandDiscriminator - discriminator based on twin passband filter
		//
		//
		template <typename sample_t, typename accum_t, typename filter_t = double>
		class TwinPassbandDiscriminator {
			public:
				struct Config {
					// the sampling rate
					size_t fs;
					// the center frequency
					double f0;
					// the overall shift
					double shift;
					// the filter Q
					double R;
					// bits-per-second
					size_t bps;
					// the length of the output integrators
					size_t int_len;

					// trigger limit (deviation, normalized)
					double trigger;

					// selective-fading compensatino integrator length (bit-times)
					size_t sfc_divisor;

					// set some default values
					Config() {
						bps = 45;
						trigger = 0.1;
						sfc_divisor = DEFAULT_SF_DETECTOR_DIVISOR;
					}
				};

			private:
				// the individual tone filters
				ResonantFilter<filter_t> high;
				ResonantFilter<filter_t> low;

				// integrators for detecting each envelope size
				FixedAverager<sample_t, accum_t> c_high;
				FixedAverager<sample_t, accum_t> c_low;

				// selective-fade detector
				Decay<double> m_decay_high;
				Decay<double> m_decay_low;

				// hysteresis counter
				Hysteresis<accum_t> trigger;

				// storage for recent output values
				sample_t sample_high, sample_low;
				sample_t m_Angle;
				bool m_Value;

			public:
				TwinPassbandDiscriminator(const Config &c);

			public:
				// run the algorithm
				bool run(sample_t in);
				
				// return cached value of last algorithm output
				bool value() const { return m_Value; }

				// return deviation, in range of [-100, 100] by default
				int8_t deviation(sample_t scale = 100) const { return (scale * m_Angle) / TPOperations<sample_t>::trigger_rail(); }

				// get the current trigger level (valid range is 0.0 to 1.0)
				double trig_level() const { return trigger.high() / (double)TPOperations<sample_t>::trigger_rail(); }

				// reconfigure the trigger level (valid range is 0.0 to 1.0)
				void trig_level(double new_value) {
					trigger.low(new_value * -TPOperations<accum_t>::trigger_rail());
					trigger.high(new_value * TPOperations<accum_t>::trigger_rail());
				}

				// these are mostly for debugging
				sample_t output() const { return sample_high + sample_low; }
				sample_t output_high() const { return sample_high; }
				sample_t output_low() const { return sample_low; }
				sample_t level_high() const { return c_high.value(); }
				sample_t level_low() const { return c_low.value(); }
				sample_t angle() const { return m_Angle; }
				sample_t offset() const { return trigger.offset(); }
		};

		//
		//  TwinPassbandDiscriminator<T>::ctor(...)
		//
		template <typename sample_t, typename accum_t, typename filter_t>
		inline TwinPassbandDiscriminator<sample_t, accum_t, filter_t>::TwinPassbandDiscriminator(const TwinPassbandDiscriminator<sample_t, accum_t, filter_t>::Config &c)
			: high((c.f0 + (c.shift / 2.0)) / c.fs, c.R), // the normalized frequency of the high tone
			  low ((c.f0 - (c.shift / 2.0)) / c.fs, c.R), // the normalized frequency of the low tone
			  c_high(c.int_len * (c.fs / (c.f0 + (c.shift / 2.0)))),
			  c_low (c.int_len * (c.fs / (c.f0 - (c.shift / 2.0)))),
			  m_decay_high(LowpassToAlpha(c.fs, static_cast<double>(c.bps) / c.sfc_divisor)),
			  m_decay_low(LowpassToAlpha(c.fs, static_cast<double>(c.bps) / c.sfc_divisor)),
			  trigger(
				c.trigger * -TPOperations<sample_t>::trigger_rail(),
				c.trigger * TPOperations<sample_t>::trigger_rail()
			  ),
			  m_Value(0) {
			  // DEBUG:
			  //std::cerr << "c_SF = " << m_decay_high.Alpha << std::endl;
		}

		//
		//  TwinPassbandDiscriminator<T>::run(...) - this is the discriminator algorithm
		//
		template <typename sample_t, typename accum_t, typename filter_t>
		inline bool TwinPassbandDiscriminator<sample_t, accum_t, filter_t>::run(sample_t sample) {
			// run the two tone filters
			sample_high = high.filter(sample);
			sample_low  = low.filter(sample);

			// then do LTA of each
			accum_t hi = c_high.run(Operations<sample_t>::absv(sample_high));
			accum_t lo = c_low.run(Operations<sample_t>::absv(sample_low));

			// compute the angle between the two
			m_Angle = TPOperations<sample_t>::compute_angle(hi, lo);

			// compute the amount of selective fading, and use that to adjust the hysteresis threshold
			trigger.offset(
				(m_decay_high.run(hi) - m_decay_low.run(lo)) / -2
			);

			// DEBUG:
			//std::cerr << "sf_high = " << (int)m_decay_high.value() << std::endl;
			//std::cerr << "sf_low  = " << (int)m_decay_low.value() << std::endl;

			// then run hysteresis trigger on the angle
			return (m_Value = trigger.run(m_Angle));
		}
	}
}

#endif // __KK5JY_TPFDISC_H
