#    This program 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 of the License, or
#    (at your option) any later version.
#
#    This program 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 program.  If not, see <http://www.gnu.org/licenses/>.
#
#============================================================
#GAMMATONE_C: A C implementation of the 4th order gammatone filter
#-------------------------------
#  [bm, env, instp, ] = gammatone_c(x, fs, cf, hrect) 
#
#  x     - input signal
#  fs    - sampling frequency (Hz)
#  cf    - centre frequency of the filter (Hz)
#  hrect - half-wave rectifying if hrect = 1 (default 0)
#
#
#  bm    - basilar membrane displacement
#  env   - instantaneous envelope
#  instp - instantaneous phase (unwrapped radian)
#
#  >> mex gammatone_c.c
#  >> bm = gammatone_c(x, 16000, 200);
#
# This was translated from MATLAB to python by Greg Kochanski.
# More detail on the original can be found at:
#  http://www.dcs.shef.ac.uk/~ning/resources/gammatone/
# Original author:
#  Ning Ma, University of Sheffield
#  n.ma@dcs.shef.ac.uk, 09 Mar 2006
# 

import math
import cython
import numpy
cimport numpy
ctypedef numpy.float_t DTYPE

cdef extern from "math.h":
	double floor(double)
	double exp(double)
	double sqrt(double)
	double sin(double x)
	double cos(double x)
	double fabs(double x)
	double atan2(double y, double x)
	double pow(double y, double x)

# /*=======================
 # * Useful Const
 # *=======================
cdef double BW_CORRECTION = 1.0190
cdef double M_PI = math.pi

# /*=======================
 # * Utility functions
 # *=======================

cdef double myMod(double x, double y):
	return x - y*floor(x/y)

cdef double erb(double x):
	return 24.7*(4.37e-3*x+1.0)

M_BASILAR = 0
M_BASILAR_RECT = 1
M_AMP_PHASE = 2

cdef class a_phase:
	cdef double a, phase

#  [bm, env, instp, ] = gammatone_c(x, fs, cf, hrect) 
cdef class gammatone_c:
	cdef double cf, dt
	cdef double oldphase = 0.0
	cdef double tpt = ( M_PI + M_PI ) * dt
	cdef double tptbw = tpt * erb ( cf ) * BW_CORRECTION
	cdef double a = exp ( -tptbw )

	#/* based on integral of impulse response */
	cdef double gain = ( tptbw*tptbw*tptbw*tptbw ) / 3.0

	#/* Update filter coefficiants */
	cdef double a1 = 4.0*a
	cdef double a2 = -6.0*a*a
	cdef double a3 = 4.0*a*a*a
	cdef double a4 = -a*a*a*a
	cdef double a5 = 4.0*a*a
	cdef double p0r = 0.0
	cdef double p1r = 0.0
	cdef double p2r = 0.0
	cdef double p3r = 0.0
	cdef double p4r = 0.0
	cdef double p0i = 0.0
	cdef double p1i = 0.0
	cdef double p2i = 0.0
	cdef double p3i = 0.0
	cdef double p4i = 0.0
 
	#/*===========================================================
	# * exp(a+i*b) = exp(a)*(cos(b)+i*sin(b))
	# * q = exp(-i*tpt*cf*t) = cos(tpt*cf*t) + i*(-sin(tpt*cf*t))
	# * qcos = cos(tpt*cf*t)
	# * qsin = -sin(tpt*cf*t)
	# *===========================================================
	cdef double coscf = cos ( tpt * cf )
	cdef double sincf = sin ( tpt * cf )
	cdef double qcos = 1.0	#   /* t=0 & q = exp(-i*tpt*t*cf)*/
	cdef double qsin = 0.0
	cdef double oldcs, oldsn, dp, dps, u0r, u0i

	def __init__(self, double dt, double cf):
		self.dt = dt
		self.cf = cf

	cdef double filter_bmr(self, double x):
		#/* Filter part 1 & shift down to d.c. */
		self.p0r = self.qcos*x + self.a1*self.p1r + self.a2*self.p2r + self.a3*self.p3r + self.a4*self.p4r
		self.p0i = self.qsin*x + self.a1*self.p1i + self.a2*self.p2i + self.a3*self.p3i + self.a4*self.p4i

		#/* Filter part 2 */
		self.u0r = self.p0r + self.a1*self.p1r + self.a5*self.p2r
		self.u0i = self.p0i + self.a1*self.p1i + self.a5*self.p2i

		#/* Update filter results */
		self.p4r = self.p3r
		self.p3r = self.p2r
		self.p2r = self.p1r
		self.p1r = self.p0r
		self.p4i = self.p3i
		self.p3i = self.p2i
		self.p2i = self.p1i
		self.p1i = self.p0i
  
		#/*==========================================
		#* Basilar membrane response
		#* 1/ shift up in frequency first: (u0r+i*u0i) * exp(i*tpt*cf*t)
		#				= (u0r+i*u0i) * (qcos + i*(-qsin))
		#* 2/ take the real part only: bm = real(exp(j*wcf*kT).*u) * gain;
		#*==========================================
		cdef double rv = ( self.u0r * self.qcos + self.u0i * self.qsin ) * self.gain

		# /*====================================================
		# * The basic idea of saving computational load:
		# * cos(a+b) = cos(a)*cos(b) - sin(a)*sin(b)
		# * sin(a+b) = sin(a)*cos(b) + cos(a)*sin(b)
		# * qcos = cos(tpt*cf*t) = cos(tpt*cf + tpt*cf*(t-1))
		# * qsin = -sin(tpt*cf*t) = -sin(tpt*cf + tpt*cf*(t-1))
		# *====================================================
		self.oldcs = self.qcos
		self.oldsn = self.qsin
		self.qcos = self.coscf * self.oldcs + self.sincf * self.oldsn
		self.qsin = self.coscf * self.oldsn - self.sincf * self.oldcs
		return rv


	cdef double filter_ear(self, double x, double threshold, double e):
		cdef double tmp = self.filter_bmr(x)
		if tmp < 0:
			return 0.0
		return pow(tmp/threshold, 2*e)


	cdef a_phase filter_a_phase(self, double x):
		#/* Filter part 1 & shift down to d.c. */
		self.p0r = self.qcos*x + self.a1*self.p1r + self.a2*self.p2r + self.a3*self.p3r + self.a4*self.p4r
		self.p0i = self.qsin*x + self.a1*self.p1i + self.a2*self.p2i + self.a3*self.p3i + self.a4*self.p4i

		#/* Filter part 2 */
		self.u0r = self.p0r + self.a1*self.p1r + self.a5*self.p2r
		self.u0i = self.p0i + self.a1*self.p1i + self.a5*self.p2i

		#/* Update filter results */
		self.p4r = self.p3r
		self.p3r = self.p2r
		self.p2r = self.p1r
		self.p1r = self.p0r
		self.p4i = self.p3i
		self.p3i = self.p2i
		self.p2i = self.p1i
		self.p1i = self.p0i
  
		#/*==========================================
		#* Instantaneous Hilbert envelope
		#* env = abs(u) * gain;
		#*==========================================
		cdef a_phase rv
		rv.a = sqrt ( u0r * u0r + u0i * u0i ) * gain

		#/*==========================================
		#* Instantaneous phase
		#* instp = unwrap(angle(u));
		#*==========================================
		rv.phase = atan2 ( u0i, u0r )
		# /* unwrap it */
		dp = rv.phase - oldphase
		if fabs ( dp ) > M_PI:
			dps = myMod ( dp + M_PI, 2 * M_PI) - M_PI;
			if dps == -M_PI and dp > 0:
				dps = M_PI
			rv.phase += dps - dp
		oldphase = rv.phase

		# /*====================================================
		# * The basic idea of saving computational load:
		# * cos(a+b) = cos(a)*cos(b) - sin(a)*sin(b)
		# * sin(a+b) = sin(a)*cos(b) + cos(a)*sin(b)
		# * qcos = cos(tpt*cf*t) = cos(tpt*cf + tpt*cf*(t-1))
		# * qsin = -sin(tpt*cf*t) = -sin(tpt*cf + tpt*cf*(t-1))
		# *====================================================
		self.oldcs = self.qcos
		self.oldsn = self.qsin
		self.qcos = self.coscf * self.oldcs + self.sincf * self.oldsn
		self.qsin = self.coscf * self.oldsn - self.sincf * self.oldcs
		return rv


def gammatone_ear_bank(numpy.ndarray[DTYPE, ndim=1] x, double dt, double e, cflist):
	cdef int i, t, n = len(cflist)
	cdef gammatone_c f
	cdef double cf
	cdef object fltr = []
	ttmp = []
	for (cfreq, thresh) in cflist:
		fltr.append(gammatone_c(dt, cf))
		n += 1
	numpy.ndarray[DTYPE, ndim=1, mode="c"] threshold = numpy.array(ttmp)
	cdef int m = x.shape[0]
	numpy.ndarray[DTYPE, ndim=1, mode="c"] rv = numpy.zeros((m, n))
	cdef double xx
	for t from 0 < t < m:
		xx = x[t]
		i = 0
		for f in fltr:
			rv[t,i] = f.filter_ear(xx, threshold[i], e)
			i += 1
	return rv


@cython.boundscheck(False)
def hipass_first_order(numpy.ndarray[DTYPE, ndim=1, mode="c"] d, double cutoff_freq):
	"""A first-order high-pass filter.
	Cutoff freq is measured in cycles per point."""

	assert cutoff_freq >= 0.0
	cdef int n = d.shape[0]
	cdef numpy.ndarray[DTYPE, ndim=1, mode="c"] o = numpy.zeros((n,))
	cdef double tau = 1.0/(2*M_PI*cutoff_freq)
	cdef double alpha = tau/(tau+1.0)
	cdef int i
	o[0] = alpha * d[0]
	with nogil:
		for i from 1 <= i < n:
			o[i] = alpha * (o[i-1] + (d[i] - d[i-1]))
	return o


@cython.boundscheck(False)
def lopass_first_order(numpy.ndarray[DTYPE, ndim=1, mode="c"] d, cutoff_freq):
	"""A first-order low-pass filter.
	Cutoff freq is measured in cycles per point."""
	assert cutoff_freq >= 0.0
	cdef int n = d.shape[0]
	cdef numpy.ndarray[DTYPE, ndim=1, mode="c"] o = numpy.zeros((n,))
	cdef double tau = 1.0/(2*M_PI*cutoff_freq)
	cdef double alpha = 1.0/(tau+1.0)
	cdef int i
	with nogil:
		for i from 1 <= i < n:
			o[i] = o[i-1] + alpha * (d[i]-o[i-1])
	return o
