/*
 *
 *
 *    es.h
 *
 *    Exponential smoothers.
 *
 *    Copyright (C) 2017,2018 by Matt Roberts.
 *    License: GNU GPL3 (www.gnu.org)
 *
 *
 */

#ifndef __KK5JY__EXPS
#define __KK5JY__EXPS

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

namespace KK5JY {
	namespace DSP {
		// ---------------------------------------------------------------------------
		//
		//   class ISmoother<sample_t>
		//
		//   Common interface for smoothers.
		//
		// ---------------------------------------------------------------------------
		template <typename sample_t>
		class ISmoother {
			public:
				virtual sample_t run(sample_t) = 0;
				virtual ~ISmoother() { };
		};

		// ---------------------------------------------------------------------------
		//
		//   class Cascade<HL, sample_t>
		//
		//   A convenience wrapper for cascading several identical stages.
		//
		// ---------------------------------------------------------------------------
		template <typename HL, typename sample_t>
		class Cascade : public ISmoother<sample_t> {
			public:
				const int Order;
			private:
				HL **stages;
			public:
				sample_t Alpha() const { return stages[0].Alpha; }
				sample_t run(sample_t);
			public:
				Cascade(int order, double alpha);
				virtual ~Cascade();
		};

		//
		//  Cascade::ctor
		//
		template <typename HL, typename sample_t>
		inline Cascade<HL, sample_t>::Cascade(int order, double alpha) : Order(order) {
			stages = new HL*[Order];
			for (int i = 0; i != Order; ++i) {
				stages[i] = new HL(alpha);
			}
		}

		//
		//  Cascade::dtor
		//
		template <typename HL, typename sample_t>
		inline Cascade<HL, sample_t>::~Cascade() {
			for (int i = 0; i != Order; ++i) {
				delete stages[i];
			}
			delete[] stages;
			stages = 0;
		}

		//
		//  Cascade::run(...)
		//
		template <typename HL, typename sample_t>
		inline sample_t Cascade<HL, sample_t>::run(sample_t value) {
			for (int i = 0; i != Order; ++i) {
				value = stages[i]->run(value);
			}
			return value;
		}


		// ---------------------------------------------------------------------------
		//
		//   class Smoother<T>
		//
		//   Exponential smoothing class for floating point types.
		//   This is also an approximation of a first-order RC lowpass circuit.
		//
		// ---------------------------------------------------------------------------
		template <typename T>
		class Smoother : public ISmoother<T> {
			public:
				const T Alpha;
			private:
				T state;
			public:
				Smoother(T alpha, T seed = 0);
				T run(T in);
				T value() const { return state; }
		};

		//
		//  Smoother::ctor
		//
		template <typename T>
		inline Smoother<T>::Smoother(T alpha, T seed) : Alpha(alpha), state(seed) { }

		//
		//  Smoother::run
		//
		template <typename T>
		inline T Smoother<T>::run(T in) {
			return state = (in * Alpha) + (state * (1.0 - Alpha));
		}

		//
		//  Calculate alpha value to emulate RC lowpass filter in smoother.
		//
		inline double LowpassToAlpha(size_t fs, double fc) {
			// caluclate the equivalent R-C constant
			double rc = 1.0 / (2.0 * M_PI * fc);

			// calculate the sampling period
			double dt = 1.0 / fs;

			// calculate equivalent alpha value
			return dt / (rc + dt);
		}

		//
		//  calculate lowpass fc value from alpha when using smoother as LP filter.
		//
		inline double AlphaToLowpass(double alpha, size_t fs) {
			// calculate the sampling period
			double dt = 1.0 / fs;

			// calculate RC value from alpha
			double rc = (dt * (1.0 - alpha)) / alpha;

			// caluclate fc from RC
			return 1.0 / (2.0 * M_PI * rc);
		}

		// ---------------------------------------------------------------------------
		//
		//   class Desmoother<T>
		//
		//   First-order approximation of a high-pass RC filter
		//
		// ---------------------------------------------------------------------------
		template <typename T>
		class Desmoother : public ISmoother<T> {
			public:
				const T Alpha;
			private:
				T x1, y1;
			public:
				Desmoother(T alpha, T seed = 0);
				T run(T in);
				T value() const { return y1; }
		};

		//
		//  Desmoother::ctor
		//
		template <typename T>
		inline Desmoother<T>::Desmoother(T alpha, T seed) : Alpha(alpha), x1(seed), y1(seed) { }

		//
		//  Desmoother::run
		//
		template <typename T>
		inline T Desmoother<T>::run(T x0) {
			y1 = (Alpha * y1) + (Alpha * (x0 - x1));
			x1 = x0;
			return y1;
		}

		//
		//  Calculate alpha value to emulate RC lowpass filter in smoother.
		//
		inline double HighpassToAlpha(size_t fs, double fc) {
			// caluclate the equivalent R-C constant
			double rc = 1.0 / (2.0 * M_PI * fc);

			// calculate the sampling period
			double dt = 1.0 / fs;

			// calculate equivalent alpha value
			return rc / (rc + dt);
		}

		//
		//  calculate lowpass fc value from alpha when using smoother as LP filter.
		//
		inline double AlphaToHighpass(double alpha, size_t fs) {
			// calculate the sampling period
			double dt = 1.0 / fs;

			// calculate RC value from alpha
			double rc = (dt * (1.0 - alpha)) / alpha;

			// caluclate fc from RC
			return 1.0 / (2.0 * M_PI * rc);
		}

		// ---------------------------------------------------------------------------
		//
		//   class Decay<T>
		//
		//   Same as Smoother<T>, but with fast-rise; approximates an RC circuit
		//   with a diode pointing in the charging direction.
		//
		// ---------------------------------------------------------------------------
		template <typename T>
		class Decay : public ISmoother<T> {
			public:
				const T Alpha;
			private:
				T state;
			public:
				Decay(T alpha, T seed = 0);
				T run(T in);
				T value() const { return state; }
		};

		//
		//  Decay::ctor
		//
		template <typename T>
		inline Decay<T>::Decay(T alpha, T seed) : Alpha(alpha), state(seed) { }

		//
		//  Decay::run
		//
		template <typename T>
		inline T Decay<T>::run(T in) {
			if (in >= state) return state = in;
			return state = (in * Alpha) + (state * (1.0 - Alpha));
		}

		// ---------------------------------------------------------------------------
		//
		//   class FixedSmoother<T>
		//
		//   Exponential smoothing class for fixed point types.
		//
		// ---------------------------------------------------------------------------
		template <typename T>
		class FixedSmoother {
			public:
				const double Alpha;
			private:
				T fixedAlpha;
				T state;
			public:
				FixedSmoother(double alpha, T seed = 0);
				T run(T in);
				T value() const { return state; }
		};

		//
		//  FixedSmoother::ctor
		//
		template <typename T>
		inline FixedSmoother<T>::FixedSmoother(double alpha, T seed)
			: Alpha(alpha),
			  fixedAlpha(alpha * KK5JY::DSP::norm_limits<T>::maximum),
			  state(seed) {
			// nop
		}

		//
		//  FixedSmoother<T>::run(...) -- this is the generic version, for use with unsigned math only
		//
		template <typename T>
		inline T FixedSmoother<T>::run(T in) {
			return state =
				(((in * fixedAlpha) +
				((state * (KK5JY::DSP::norm_limits<T>::maximum - fixedAlpha))))
					>> ((sizeof(T) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<T>::maximum);
		}

		//
		//  FixedSmoother<uint8_t>::run(...)
		//
		template <>
		inline uint8_t FixedSmoother<uint8_t>::run(uint8_t in) {
			return state =
				(((static_cast<uint16_t>(in) * fixedAlpha) +
				((static_cast<uint16_t>(state) * (static_cast<uint16_t>(KK5JY::DSP::norm_limits<uint16_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint8_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint8_t>::maximum);
		}

		//
		//  FixedSmoother<int8_t>::run(...)
		//
		template <>
		inline int8_t FixedSmoother<int8_t>::run(int8_t in) {
			return state =
				(((static_cast<int16_t>(in) * fixedAlpha) +
				((static_cast<int16_t>(state) * (static_cast<int16_t>(KK5JY::DSP::norm_limits<int8_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int8_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int16_t>::maximum);
		}

		//
		//  FixedSmoother<uint16_t>::run(...)
		//
		template <>
		inline uint16_t FixedSmoother<uint16_t>::run(uint16_t in) {
			return state =
				(((static_cast<uint32_t>(in) * fixedAlpha) +
				((static_cast<uint32_t>(state) * (static_cast<uint32_t>(KK5JY::DSP::norm_limits<uint32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint16_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint16_t>::maximum);
		}

		//
		//  FixedSmoother<int16_t>::run(...)
		//
		template <>
		inline int16_t FixedSmoother<int16_t>::run(int16_t in) {
			return state =
				(((static_cast<int32_t>(in) * fixedAlpha) +
				((static_cast<int32_t>(state) * (static_cast<int32_t>(KK5JY::DSP::norm_limits<int16_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int16_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int16_t>::maximum);
		}

		//
		//  FixedSmoother<uint32_t>::run(...)
		//
		template <>
		inline uint32_t FixedSmoother<uint32_t>::run(uint32_t in) {
			return state =
				(((static_cast<uint64_t>(in) * fixedAlpha) +
				((static_cast<uint64_t>(state) * (static_cast<uint64_t>(KK5JY::DSP::norm_limits<uint32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint32_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint32_t>::maximum);
		}

		//
		//  FixedSmoother<int32_t>::run(...)
		//
		template <>
		inline int32_t FixedSmoother<int32_t>::run(int32_t in) {
			return state =
				(((static_cast<int64_t>(in) * fixedAlpha) +
				((static_cast<int64_t>(state) * (static_cast<int64_t>(KK5JY::DSP::norm_limits<int32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int32_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int32_t>::maximum);
		}


		// ---------------------------------------------------------------------------
		//
		//   class FixedDecay<T>
		//
		//   Exponential smoothing class for fixed point types, with fast charge and
		//   slow decay.  Approximates an RC circuit with a diode pointed in the 
		//   charge direction.
		//
		// ---------------------------------------------------------------------------
		template <typename T>
		class FixedDecay {
			public:
				const double Alpha;
			private:
				T fixedAlpha;
				T state;
			public:
				FixedDecay(double alpha, T seed = 0);
				T run(T in);
				T value() const { return state; }
		};

		//
		//  FixedDecay::ctor
		//
		template <typename T>
		inline FixedDecay<T>::FixedDecay(double alpha, T seed)
			: Alpha(alpha),
			  fixedAlpha(alpha * KK5JY::DSP::norm_limits<T>::maximum),
			  state(seed) {
			// nop
		}

		//
		//  FixedDecay<T>::run(...) -- this is the generic version, for use with unsigned math only
		//
		template <typename T>
		inline T FixedDecay<T>::run(T in) {
			if (in >= state) return state = in;
			return state =
				(((in * fixedAlpha) +
				((state * (KK5JY::DSP::norm_limits<T>::maximum - fixedAlpha))))
					>> ((sizeof(T) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<T>::maximum);
		}

		//
		//  FixedDecay<uint8_t>::run(...)
		//
		template <>
		inline uint8_t FixedDecay<uint8_t>::run(uint8_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<uint16_t>(in) * fixedAlpha) +
				((static_cast<uint16_t>(state) * (static_cast<uint16_t>(KK5JY::DSP::norm_limits<uint16_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint8_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint8_t>::maximum);
		}

		//
		//  FixedDecay<int8_t>::run(...)
		//
		template <>
		inline int8_t FixedDecay<int8_t>::run(int8_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<int16_t>(in) * fixedAlpha) +
				((static_cast<int16_t>(state) * (static_cast<int16_t>(KK5JY::DSP::norm_limits<int8_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int8_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int16_t>::maximum);
		}

		//
		//  FixedDecay<uint16_t>::run(...)
		//
		template <>
		inline uint16_t FixedDecay<uint16_t>::run(uint16_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<uint32_t>(in) * fixedAlpha) +
				((static_cast<uint32_t>(state) * (static_cast<uint32_t>(KK5JY::DSP::norm_limits<uint32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint16_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint16_t>::maximum);
		}

		//
		//  FixedDecay<int16_t>::run(...)
		//
		template <>
		inline int16_t FixedDecay<int16_t>::run(int16_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<int32_t>(in) * fixedAlpha) +
				((static_cast<int32_t>(state) * (static_cast<int32_t>(KK5JY::DSP::norm_limits<int16_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int16_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int16_t>::maximum);
		}

		//
		//  FixedDecay<uint32_t>::run(...)
		//
		template <>
		inline uint32_t FixedDecay<uint32_t>::run(uint32_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<uint64_t>(in) * fixedAlpha) +
				((static_cast<uint64_t>(state) * (static_cast<uint64_t>(KK5JY::DSP::norm_limits<uint32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(uint32_t) * 8))); // i.o.w., / KK5JY::DSP::norm_limits<uint32_t>::maximum);
		}

		//
		//  FixedDecay<int32_t>::run(...)
		//
		template <>
		inline int32_t FixedDecay<int32_t>::run(int32_t in) {
			if (in >= state) return state = in;
			return state =
				(((static_cast<int64_t>(in) * fixedAlpha) +
				((static_cast<int64_t>(state) * (static_cast<int64_t>(KK5JY::DSP::norm_limits<int32_t>::maximum) - fixedAlpha))))
					>> ((sizeof(int32_t) * 8) - 1)); // i.o.w., / KK5JY::DSP::norm_limits<int32_t>::maximum);
		}

	}
}

#endif // __KK5JY__EXPS
