# include "stdlib.pxi"
# include "math.pxi"
# cython: cdivision=True

cdef extern from "math.h":
	double fabs(double x) nogil
	double ceil(double x) nogil
	double floor(double x) nogil
	double cos(double x) nogil
	double pow(double y, double x) nogil
	double hypot(double y, double x) nogil

cdef extern from "stdlib.h":
	long int labs(long int a) nogil

cdef extern from "gpklib.h":
	float **float2alloc(int a, int b) nogil
	void float2free(float **x) nogil

import sys
import math
import cython
# import numpy
from gmisclib import die
import numpy
cimport numpy
cimport libc.stdlib
ctypedef numpy.float_t FTYPE
ctypedef numpy.int32_t ITYPE

S = '.'

cdef struct g_node
cdef struct d_node
ctypedef g_node g_node_t
ctypedef d_node d_node_t
ctypedef g_node * g_node_p
ctypedef g_node ** g_node_pp

cdef struct g_node:
	short int i, j
	double gdist
	g_node_t *best

cdef bint valid_g_node(g_node *x, int imax, int jmax):
	if not x:
		return True
	if not(x.i>=0 and x.i<=imax):
		print "invalid i: %s" % x.i
		return False
	if not(x.j>=0 and x.j<=jmax):
		print "invalid j: %s" % x.j
		return False
	if not( x.gdist==-1.0 or x.gdist>=0.0):
		print "invalid gdist: %s" % x.gdist
		return False
	if not (x.j < jmax or x.i<imax):
		print "invalid smaller"
		return False
	if x.best and not valid_g_node(x.best, x.i, x.j):
		print "invalid recursion (%d,%d)" % (x.i, x.j)
		return False
	return True


cdef struct d_node:
	short int i, j
	float d

cdef int imax(int a, int b) nogil:
	if a > b:
		return a
	return b

cdef int imin(int a, int b) nogil:
	if a < b:
		return a
	return b

cdef double fmin(double a, double b) nogil:
	if a < b:
		return a
	return b

cdef int BINT = 100000


cdef class slice_c:
	cdef int i, j, istep, jstep
	cdef double xc, yc, cc, ie, je
	# cdef node *n

	def __init__(self, double slx1, double sly1, double slx2, double sly2):
		self.init(slx1, sly1, slx2, sly2)
	
	cdef int init(self, double slx1, double sly1, double slx2, double sly2) except -1:
		self.i = <int>(slx1+0.5)
		self.j = <int>(sly1+0.5)
		self.ie = slx2
		self.je = sly2
		cdef double xc = sly2-sly1
		cdef double yc = slx1-slx2
		cdef double xyc = hypot(xc, yc)
		assert xyc > 0.0
		self.xc = xc / xyc
		self.yc = yc / xyc
		self.cc = -self.xc*slx1 - self.yc*sly1
		if self.i >= self.ie:
			self.istep = -1
		else:
			self.istep = 1
		if self.j >= self.je:
			self.jstep = -1
		else:
			self.jstep = 1
		# assert self.istep!=0 and self.jstep!=0
		# assert self.dist(slx1, sly1) <= 0.01
		# assert self.dist(slx2, sly2) <= 0.01
		# assert self.dist(self.i, self.j) <= 0.72
		# assert self.dist(self.ie, self.je) <= 0.72
		# print 'slice set up: ij=%d,%d e=%.2f,%.2f step=%d,%d' % (self.i, self.j, self.ie, self.je, self.istep, self.jstep)
		return 0

	cdef bint ok(self):
		return (self.i-self.ie)*self.istep <= 0.5 and (self.j-self.je)*self.jstep <= 0.5

	cdef bint ok2(self):
		return self.ok() and not (self.i==<int>(self.ie+0.5) and self.j==<int>(self.je+0.5))

	cdef int next(self):
		if self.istep == 0:
			self.j += self.jstep
		elif self.jstep == 0:
			self.i += self.istep
		else:
			if self.dist(self.i+self.istep, self.j) < self.dist(self.i, self.j+self.jstep):
				self.i += self.istep
			else:
				self.j += self.jstep
		return 1


	cdef int next_diag(self):
		cdef double BIG = 1e30
		cdef double d = BIG, dtmp
		cdef int i=self.i, j=self.j, itmp, jtmp
		if self.istep !=0 and self.jstep != 0:
			i = self.i + self.istep
			j = self.j + self.jstep
			d = self.dist(i, j)
		if self.istep != 0:
			itmp = self.i + self.istep
			dtmp = self.dist(itmp, self.j)
			if dtmp < d:
				i = itmp
				j = self.j
				d = dtmp
		if self.jstep != 0:
			jtmp = self.j + self.jstep
			dtmp = self.dist(self.i, jtmp)
			if dtmp < d:
				i = self.i
				j = jtmp
				# d = dtmp
		self.i = i
		self.j = j
		return 1


	cdef double dist(self, int i, int j) nogil:
		return fabs(i*self.xc + j*self.yc + self.cc)

	cdef int len_estimate(self):
		return <int>(ceil(fabs(self.i-self.ie)) + ceil(fabs(self.j-self.je))+1.5)


cdef double _slmf(int nx, int ny, double r):
	return r * (nx*nx+ny*ny) / <double>ny


cdef slice_c slicemaker(int nx, int ny, double r):
	cdef double EPS = 1e-6
	if -EPS < r < EPS:
		return slice_c(-EPS, EPS, EPS, -EPS)
	elif 1-EPS < r < 1+EPS:
		return slice_c(nx-EPS, ny+EPS, nx+EPS, ny-EPS)
	assert 0 < r <= 1, "r=%g" % r
	slx1 = _slmf(ny, nx, r)
	sly1 = 0.0
	if slx1 > nx:
		slx1 = nx
		sly1 = ny - _slmf(nx, ny, 1-r)
	sly2 = _slmf(nx, ny, r)
	slx2 = 0.0
	if sly2 > ny:
		sly2 = ny
		slx2 = nx - _slmf(ny, nx, 1-r)
	assert 0 <= slx1 <= nx, "slx1=%.3f vs nx=%d r=%.3f" % (slx1, nx, r)
	assert 0 <= slx2 <= nx, "slx2=%.3f vs nx=%d r=%.3f" % (slx2, nx, r)
	assert 0 <= sly1 <= ny, "sly1=%.3f vs ny=%d r=%.3f" % (sly1, ny, r)
	assert 0 <= sly2 <= ny, "sly2=%.3f vs ny=%d r=%.3f" % (sly2, ny, r)

	# print 'Slicemaker: slice_c(%.1f,%.1f,%.1f,%.1f)' % (slx1, sly1, slx2, sly2)
	return slice_c(slx1, sly1, slx2, sly2)






cdef class state_c:
	cdef g_node **g_nodes
	cdef int *gns, *gne
	cdef d_node *d_nodes
	cdef int nx, ny, nparams, n_d_nodes
	cdef double alpha, beta, bw
	cdef double BIG
	cdef float **x, **y

	def __cinit__(self, numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
				double alpha, double beta, bw):
		self.nx = x.shape[0]
		self.ny = y.shape[0]
		self.g_nodes = <g_node_t **>libc.stdlib.malloc(cython.sizeof(g_node_p)*self.nx)
		self.gns = <int *>libc.stdlib.malloc(cython.sizeof(int)*self.nx)
		self.gne = <int *>libc.stdlib.malloc(cython.sizeof(int)*self.nx)
		self.x = float2alloc(x.shape[0], x.shape[1])
		self.y = float2alloc(y.shape[0], y.shape[1])

		self.n_d_nodes = 3*(self.nx+self.ny+23)
		while self.n_d_nodes%17==0 or self.n_d_nodes%97==0:
			self.n_d_nodes += 1
		self.d_nodes = <d_node_t *>libc.stdlib.malloc(cython.sizeof(d_node)*self.n_d_nodes)

	def __dealloc__(self):
		with nogil:
			if self.g_nodes:
				libc.stdlib.free(self.g_nodes)
			if self.gns:
				libc.stdlib.free(self.gns)
			if self.gne:
				libc.stdlib.free(self.gne)
			if self.d_nodes:
				libc.stdlib.free(self.d_nodes)
			if self.x:
				float2free(self.x)
			if self.y:
				float2free(self.y)


	def __init__(self, numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
				double alpha, double beta, double bw):
		if not self.g_nodes or not self.gns or not self.gne:
			raise MemoryError, "Cannot allocate enough memory."
		if not self.d_nodes or not self.x or not self.y:
			raise MemoryError, "Cannot allocate enough memory."
		if not (bw <0.0 or bw > 1.0):
			raise ValueError, "Illegal bandwidth: %s" % str(bw)
		if not (self.nx>1 and self.ny>1):
			raise ValueError, "Need at least two elements in time series: have %d %d" % (self.nx, self.ny)
		if x.shape[1]!=y.shape[1]:
			raise ValueError, "x.shape[1] must equal y.shape[1]: %d and %d" % (x.shape[1], y.shape[1])
		self.nparams = x.shape[1]
		cdef int i, t, ngi, ngj
		for t from 0 <= t < self.nx:
			for i from 0 <= i < self.nparams:
				self.x[t][i] = x[t,i]
		for t from 0 <= t < self.ny:
			for i from 0 <= i < self.nparams:
				self.y[t][i] = y[t,i]
		if self.g_nodes:
			if not self._init_g_nodes(bw):
				raise MemoryError, "self._init_g_nodes() fails."
		cdef d_node_t *nd
		for i from 0 <= i < self.n_d_nodes:
			nd = & self.d_nodes[i]
			nd.i = -1
			nd.j = -1
			nd.d = -1.0
		# self.gn(0, 0).gdist = 0.0
		self.alpha = alpha
		self.beta = beta
		self.BIG = 1e30
		
	cdef g_node_t * gn(self, int i, int j) nogil:
		return & self.g_nodes[i][j-self.gns[i]]

	cdef int _init_g_nodes(self, double bw):
		cdef double EPS = 1e-6
		cdef double xbw = (self.nx-1)*(self.ny) - EPS
		if bw > 0.0:
			xbw = fmin(xbw, bw*hypot(self.nx-1, self.ny-1))
		cdef int i, j, s, e
		cdef g_node *n, *gni
		with nogil:
			for i from 0 <= i < self.nx:
				# print 'nx=', self.nx, 'ny=', self.ny, 'i=', i, 'bw=', bw
				s = imax(0, <int>floor((i*(self.ny-1)-xbw)/(self.nx-1)))
				# print "self.ny-1", self.ny-1, "xbw/nx-1=", xbw/(self.nx-1), "ceil=", ceil((i*(self.ny-1)+xbw)/(self.nx-1))
				e = imin(self.ny-1, <int>ceil((i*(self.ny-1)+xbw)/(self.nx-1)))
				# print "i=%d, bw=%s e=%s s=%s" % (i, bw, e, s)
				gni = <g_node_t *>libc.stdlib.malloc(cython.sizeof(g_node)*(e-s+1))
				if gni == <g_node_t *>0:
					return 0
				self.g_nodes[i] = gni
				self.gns[i] = s
				self.gne[i] = e
				for j from s <= j <= e:
					n = & gni[j-s]
					n.i = i
					n.j = j
					n.best = <g_node_t *> 0
					n.gdist = -1.0
		return 1


	cdef void _dealloc_g_nodes(self) nogil:
		cdef int i
		if self.g_nodes:
			for i from 0 <= i < self.nx:
				libc.stdlib.free(self.g_nodes[i])
			libc.stdlib.free(self.g_nodes)

	cdef int inbeam(self, int i, int j) nogil:
		return self.gns[i] <= j <= self.gne[i]

	cdef float get(self, g_node_t *n) nogil:
		cdef float *xi, *yj
		cdef double s
		cdef int k, h

		h = (17*n.i + 97*n.j) % self.n_d_nodes
		cdef d_node *b = & self.d_nodes[h]
		if b.i==n.i and b.j==n.j and b.d >= 0.0:
			return b.d
		s = 0.0
		xi = self.x[n.i]
		yj = self.y[n.j]
		cdef double tmp
		if self.alpha == 1:
			for k from 0 <= k < self.nparams:
				s += fabs(xi[k]-yj[k])
		elif self.alpha == 2:
			for k from 0 <= k < self.nparams:
				tmp = fabs(xi[k]-yj[k])
				s += tmp*tmp
		else:
			for k from 0 <= k < self.nparams:
				s += pow(fabs(xi[k]-yj[k]), self.alpha)
		b.d = pow(s/self.nparams, self.beta)
		b.i = n.i
		b.j = n.j
		return b.d


	cdef double search(self, g_node_t *n) nogil:
		if n.gdist >= 0:
			return n.gdist
		elif n.i==0 and n.j==0:
			n.best = <g_node_t *>0
			n.gdist = 0.0
			return n.gdist

		cdef double gtmp = self.BIG
		n.gdist = gtmp
		cdef g_node_t *ntmp = <g_node_t*>0
		if n.i>0 and n.j>0 and self.inbeam(n.i-1, n.j-1):
			ntmp = self.gn(n.i-1, n.j-1)
			gtmp = self.search(ntmp)
			if gtmp < n.gdist:
				gtmp += self.get(ntmp)
				if gtmp < n.gdist:
					n.gdist = gtmp
					n.best = ntmp
		if n.i > 0 and self.inbeam(n.i-1, n.j):
			ntmp = self.gn(n.i-1, n.j)
			gtmp = self.search(ntmp)
			if gtmp < n.gdist:
				gtmp += self.get(ntmp)
				if gtmp < n.gdist:
					n.gdist = gtmp
					n.best = ntmp
		if n.j > 0 and self.inbeam(n.i, n.j-1):
			ntmp = self.gn(n.i, n.j-1)
			gtmp = self.search(ntmp)
			if gtmp < n.gdist:
				gtmp += self.get(ntmp)
				if gtmp < n.gdist:
					n.gdist = gtmp
					n.best = ntmp
		return n.gdist


	# @cython.boundscheck(False)
	cdef numpy.ndarray path(self, g_node_t *n):
		cdef g_node_t *tgt, *tmp, *nn
		cdef int ns = 0
		nn = n
		while nn:
			ns += 1
			nn = nn.best
		# print 'ns=', ns
		cdef numpy.ndarray[ITYPE, ndim=2, mode="c"] o = numpy.zeros((ns, 2), numpy.int32)
		while n:
			ns -= 1
			o[ns,0] = n.i
			o[ns,1] = n.j
			n = n.best
		assert ns == 0
		return o


cdef int _inbeam(g_node *tgt, g_node *n) nogil:
	return tgt.i <= n.i and tgt.j <= n.j


cdef class state_jump_c:
	cdef g_node **g_nodes
	cdef d_node *d_nodes
	cdef int nx, ny, nparams, n_d_nodes
	cdef double alpha, beta
	cdef double BIG
	cdef float **x, **y

	def __cinit__(self, numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
				double alpha, double beta):
		self.nx = x.shape[0]
		self.ny = y.shape[0]
		cdef int max_g_nodes = max(self.nx, self.ny) + 1
		self.g_nodes = <g_node_t **>libc.stdlib.malloc(cython.sizeof(g_node_p)*max_g_nodes)
		self.x = float2alloc(x.shape[0], x.shape[1])
		self.y = float2alloc(y.shape[0], y.shape[1])

		self.n_d_nodes = 3*(self.nx+self.ny+23)
		while self.n_d_nodes%17==0 or self.n_d_nodes%97==0:
			self.n_d_nodes += 1
		self.d_nodes = <d_node_t *>libc.stdlib.malloc(cython.sizeof(d_node)*self.n_d_nodes)


	cdef int _init_g_nodes(self) except -1:
		self.g_nodes[0] = <g_node_t *>libc.stdlib.malloc(cython.sizeof(g_node)*2)
		if self.g_nodes[0] == <g_node_t *>0:
			raise MemoryError, "Cannot allocate self.g_nodes[0]"
		cdef g_node *n = &self.g_nodes[0][0]
		n.i = 0
		n.j = 0
		n.gdist = -1.0
		n.best = <g_node_t *>0
		assert valid_g_node(n, BINT, BINT), "_init_g_nodes n 1"
		n = & self.g_nodes[0][1]
		n.i = -1
		n.j = -1
		n.gdist = -1.0
		n.best = <g_node_t *>0
		self.g_nodes[1] = <g_node_t *>0
		return 0

	cdef void _dealloc_g_nodes(self) nogil:
		cdef int i = 0
		if self.g_nodes:
			while self.g_nodes[i]:
				libc.stdlib.free(self.g_nodes[i])
				i += 1
			libc.stdlib.free(self.g_nodes)

	def __dealloc__(self):
		cdef int i = 0
		with nogil:
			self._dealloc_g_nodes()
			if self.d_nodes:
				libc.stdlib.free(self.d_nodes)
			if self.x:
				float2free(self.x)
			if self.y:
				float2free(self.y)



	# @cython.boundscheck(False)
	def __init__(self, numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
				double alpha, double beta):
		if not self.d_nodes or not self.x or not self.y or not self.g_nodes:
			raise MemoryError, "Cannot allocate enough memory."
		if self.g_nodes:
			self._init_g_nodes()
		assert x.shape[1]==y.shape[1]
		self.nparams = x.shape[1]
		# print 'Nparams=', self.nparams, 'self.nx, ny=', self.nx, self.ny
		cdef int i, j
		for t from 0 <= t < self.nx:
			for i from 0 <= i < self.nparams:
				self.x[t][i] = x[t,i]
		for t from 0 <= t < self.ny:
			for i from 0 <= i < self.nparams:
				self.y[t][i] = y[t,i]
		cdef g_node_t *gn
		cdef d_node_t *dn
		with nogil:
			for i from 0 <= i < self.n_d_nodes:
				nd = & self.d_nodes[i]
				nd.i = -1
				nd.j = -1
				nd.d = -1.0
		self.alpha = alpha
		self.beta = beta
		self.BIG = 1e30
		

	cdef float get(self, short int i, short int j) nogil:
		cdef float *xi, *yj
		cdef double s
		cdef int k, h

		h = (17*i + 97*j) % self.n_d_nodes
		cdef d_node *b = & self.d_nodes[h]
		if b.i==i and b.j==j and b.d >= 0.0:
			return b.d
		s = 0.0
		xi = self.x[i]
		yj = self.y[j]
		cdef double tmp
		if self.alpha == 1:
			for k from 0 <= k < self.nparams:
				s += fabs(xi[k]-yj[k])
		elif self.alpha == 2:
			for k from 0 <= k < self.nparams:
				tmp = fabs(xi[k]-yj[k])
				s += tmp*tmp
		else:
			for k from 0 <= k < self.nparams:
				s += pow(fabs(xi[k]-yj[k]), self.alpha)
		b.d = pow(s/self.nparams, self.beta)
		b.i = i
		b.j = j
		return b.d


	cdef g_node * set_newslice(self, double r, int j) except <g_node *>0:
		cdef int i
		cdef slice_c slice = slicemaker(self.nx-1, self.ny-1, r)
		cdef int ne = slice.len_estimate() + 1
		cdef g_node *newslice = <g_node *>libc.stdlib.malloc(cython.sizeof(g_node)*ne)
		if not newslice:
			raise MemoryError, "Cannot allocate newslice"
		cdef g_node *n

		i = 0
		while slice.ok():
			assert i < ne
			n = &newslice[i]
			n.i = slice.i
			n.j = slice.j
			n.gdist = -1.0
			n.best = <g_node *>0
			assert valid_g_node(n, BINT, BINT)
			i += 1
			slice.next()
		assert i < ne
		n = & newslice[i]
		n.i = -1
		n.j = -1
		n.gdist = -1.0
		n.best = <g_node *>0
		self.g_nodes[j] = newslice
		self.g_nodes[j+1] = <g_node *>0
		return newslice



	cdef int searchjump(self, g_node_t *n, g_node_t *oldslice, int depth) except -1:
		# print '%s--sjump node[%d,%d] --' % (S*depth, n.i, n.j)
		cdef g_node_t *tgt
		cdef double g
		cdef double gbest = self.BIG
		cdef int i, ki, kj, r, phase, found = 0
		assert valid_g_node(n, BINT, BINT)
		if n.best:	# Already computed in a previous slice
			assert valid_g_node(n.best, BINT, BINT)
			return 1
		# n.gdist = -1
		# n.best = <g_node_t *>0
		cdef slice_c line = slice_c(1, 0, 0, 1)

		i = 0
		while oldslice[i].i >= 0:
			tgt = & oldslice[i]
			assert valid_g_node(tgt, BINT, BINT), "i=%d, depth=%d" % (i, depth)
			# print "Checking oldslice[%d,%d,%g,%d]" % (tgt.i, tgt.j, tgt.gdist, <long>tgt.best)
			i += 1
			if tgt.gdist < 0.0:
				continue
			if tgt.i==n.i and tgt.j==n.j:
				if tgt.gdist < gbest:
					gbest = tgt.gdist
				found = 1
				continue
			if not _inbeam(tgt, n):
				continue
			g = tgt.gdist
			assert g >= 0.0, "tgt(%d,%d) g=%s when computing [%d,%d]" % (tgt.i, tgt.j, g, n.i, n.j)
			found = 1
			if g > gbest:
				# print '%s searchjump=%g > gbest=%g bailout for (%d,%d) -> (%d,%d)' % (S*depth, g, gbest, n.i, n.j, tgt.i, tgt.j)
				continue


			# print '%s -- partial g=%g search [%d,%d] -> [%d,%d]' % (S*depth, g, n.i, n.j, tgt.i, tgt.j)
			line.init(n.i, n.j, tgt.i, tgt.j)
			while line.ok2():
				g += self.get(line.i, line.j)
				if g > gbest:
					break
				line.next_diag()
			if g < gbest:
				# print '%s Setting n.best=[%d,%d] g=%g from [%d,%d]' % (S*depth, n.i, n.j, g, tgt.i, tgt.j)
				gbest = g
				n.gdist = g
				assert valid_g_node(n, BINT, BINT)
				assert valid_g_node(tgt, BINT, BINT)
				n.best = tgt
			else:
				# print '%s Not the best g=%g' % (S*depth, g)
				pass
		if not found:
			n.gdist = self.BIG
			assert valid_g_node(n, BINT, BINT)
			n.best = <g_node_t *>0
			# print 'No route from [%d,%d] to oldslice' % (n.i, n.j)
			return 0
		return 1


	# @cython.boundscheck(False)
	cdef numpy.ndarray path(self, g_node_t *n):
		cdef g_node_t *tgt, *tmp, *nn
		cdef slice_c line = slice_c(1, 0, 0, 1)
		cdef int ns = 0
		# with nogil:
		assert valid_g_node(n, BINT, BINT)
		nn = n
		while nn and nn.best:
			assert valid_g_node(n, BINT, BINT)
			tgt = nn.best
			assert valid_g_node(tgt, BINT, BINT)
			line.init(nn.i, nn.j, tgt.i, tgt.j)
			while line.ok2():
				ns += 1
				line.next_diag()
			nn = tgt
		ns += 1
		# print 'ns=%d' % ns
		cdef numpy.ndarray[ITYPE, ndim=2, mode="c"] o = numpy.zeros((ns, 2), numpy.int32)
		assert valid_g_node(n, BINT, BINT)
		while n and n.best:
			tgt = n.best
			assert valid_g_node(tgt, BINT, BINT)
			line.init(n.i, n.j, tgt.i, tgt.j)
			while line.ok2():
				ns -= 1
				# print 'step to (%d,%d) ns=%d' % (line.i, line.j, ns)
				o[ns,0] = line.i
				o[ns,1] = line.j
				line.next_diag()
			n = tgt
		ns -= 1
		o[ns,0] = n.i
		o[ns,1] = n.j
		# print 'ns=', ns, 'o=', o
		assert ns == 0, "ns=%d" % ns
		return o



def dtw(numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y, alpha, beta, bw=None):
	"""A standard Dynamic Time Warp match of two sequences of feature vectors.
	The allowed time increments are (0,1), (1,0), and (1,1).
	@param x: one array of feature vectors
	@type x: numpy.ndarray, 2-dimensional.   First index is the time axis,
		second index is the feature axis.
	@param y: the other array of feature vectors
	@type y: numpy.ndarray, 2-dimensional.   First index is the time axis,
		second index is the feature axis.
	@param alpha: exponent for differences in features.   2=Euclidean, 1=city block
	@param beta: exponent for summing along time axis.   Normally 1.
	@type alpha: float
	@type beta: float
	@rtype: numpy.ndarray, 2-dimensional array of integers with shape [n,2].
		[i,0] is the index (time) in x, [i,1] is the index (time) in y.
	@return: An array that maps from indices in x to indices into y.
		x[r[i,0]] matches y[r[i,1]] for all i.
	"""
	if x.shape[1] != y.shape[1]:
		raise ValueError, "Feature vectors different lengths: %dx%d and %dx%d" % (x.shape[0], x.shape[1], y.shape[0], y.shape[1])
	cdef state_c s = state_c(x, y, alpha, beta, bw if bw is not None else -1.0)
	assert s.nx>0 and s.ny>0
	cdef g_node_t *n = s.gn(s.nx-1, s.ny-1)
	assert valid_g_node(n, BINT, BINT)
	with nogil:
		s.search(n)
	assert valid_g_node(n, BINT, BINT)
	return s.path(n)


class NoRoute(Exception):
	def __init__(self, *s):
		Exception.__init__(self, *s)

def dtwjump(numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y, mm, alpha, beta):
	"""A constrained Dynamic Time Warp match of two sequences of feature vectors.
	It constrains the time mapping to be a piecewise linear function with a corner
	every C{mm} samples.
	@param x: A sequence of feature vectors.
	@type x: L{numpy} 2-dimensional array of floats.  The first index is time,
		the second index is within each feature vector.
	@param y: A sequence of feature vectors.
	@type y: L{numpy} 2-dimensional array of floats.  The first index is time,
		the second index is within each feature vector.
	"""
	# print 'Entering dtwjump'
	cdef int m = int(round(mm))
	assert m > 0
	if x.shape[1] != y.shape[1]:
		raise ValueError, "Feature vectors different lengths: %dx%d and %dx%d" % (x.shape[0], x.shape[1], y.shape[0], y.shape[1])
	cdef state_jump_c s = state_jump_c(x, y, alpha, beta)
	assert s.nx>1 and s.ny>1
	cdef int found, i, j, nsteps = (max(s.nx,s.ny)-1)//m
	assert nsteps > 0, "Sorry! Don't handle the case of large m and small data."
	cdef g_node_t *newslice, *oldslice
	cdef double r
	# print 's.n=(%d,%d), nsteps=%d' % (s.nx, s.ny, nsteps)
	# with nogil:

	oldslice = s.set_newslice(0.0, 0)
	oldslice[0].gdist = 0.0
	assert valid_g_node(&oldslice[0], BINT, BINT)
	for j from 0 <= j < nsteps:
		r = <double>(j+1)/<double>nsteps
		# print 'r=%g from j=%d // nsteps=%d' % (r, j, nsteps)
		newslice = s.set_newslice(r, j+1)
		# print 'starting searchjump calls'
		found = 0
		i = 0
		while newslice[i].i >= 0:
			found += s.searchjump(&newslice[i], oldslice, i)
			i += 1
		if found == 0:
			raise NoRoute
		oldslice = newslice
	# print 'loop done'
	return s.path(&newslice[0])


def map2index(numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[ITYPE, ndim=1] tmap):
	cdef int t, j, i, n=tmap.shape[0], np=x.shape[1]
	cdef numpy.ndarray[FTYPE, ndim=2, mode="c"] o = numpy.zeros((n, np))
	assert tmap[0] >= 0, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0], tmap[-1])
	assert tmap[-1] <= x.shape[0]-1, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0], tmap[-1])
	for i from 0 <= i < n:
		t = tmap[i]
		for j from 0 <= j < np:
			o[i,j] = x[t,j]
	return o


def map2shared(numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
		numpy.ndarray[ITYPE, ndim=2] tmap):
	"""This is used after dtw() to map the two sequences onto the same time
	axis.
	@param x: a sequence of feature vectors, typically the same data you pass
		into L{dtw}().
	@param y: a sequence of feature vectors, typically the same data you pass
		into L{dtw}().
	@param tmap: a time mapping as returned from L{dtw}() or L{dtwjump}(). 
	@type tmap: numpy.ndarray[integer, 2-dimensional, shape is Nx2]
	@return: a tuple with a time axis, the x-feature vectors mapped onto that time
		axis, and the y-feature vectors mapped onto the same axis.
		Analogous to x and y, the first index is the pseudotime index.
	@rtype: tuple(numpy.ndarray(float, 1-d), numpy.ndarray(float, 2-d),
			numpy.ndarray(float, 2-d))
	"""
	if x.shape[1] != y.shape[1]:
		raise ValueError, "Feature vectors different lengths: %dx%d and %dx%d" % (x.shape[0], x.shape[1], y.shape[0], y.shape[1])
	assert tmap.shape[1]==2
	cdef int i, n=tmap.shape[0], np=x.shape[1]
	cdef numpy.ndarray[FTYPE, ndim=1, mode="c"] t = numpy.zeros((n,))
	for i from 0 <= i < n:
		t[i] = 0.5*(tmap[i,0] + tmap[i,1])
	return (t, map2index(x, tmap[:,0]), map2index(y, tmap[:,1]))


def map2dist(numpy.ndarray[FTYPE, ndim=2] x, numpy.ndarray[FTYPE, ndim=2] y,
		numpy.ndarray[ITYPE, ndim=2] tmap, double alpha, double beta):
	if x.shape[1] != y.shape[1]:
		raise ValueError, "Feature vectors different lengths: %dx%d and %dx%d" % (x.shape[0], x.shape[1], y.shape[0], y.shape[1])
	cdef int tx, ty, j, i, n=tmap.shape[0], np=x.shape[1]
	assert tmap.shape[1]==2
	assert tmap[0,0] >= 0, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0,0], tmap[-1,0])
	assert tmap[0,1] >= 0, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0,1], tmap[-1,1])
	assert tmap[-1,0] <= x.shape[0]-1, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0,0], tmap[-1,0])
	assert tmap[-1,1] <= y.shape[0]-1, "np=%d n=%d, tmap=%d ... %d" % (np, n, tmap[0,1], tmap[-1,1])
	cdef numpy.ndarray[FTYPE, ndim=1, mode="c"] t = numpy.zeros((n,))
	cdef numpy.ndarray[FTYPE, ndim=1, mode="c"] d = numpy.zeros((n,))
	cdef double tmp
	for i from 0 <= i < n:
		t[i] = 0.5*(tmap[i,0] + tmap[i,1])
		tx = tmap[i,0]
		ty = tmap[i,1]
		tmp = 0.0
		for j from 0 <= j < np:
			tmp += pow(fabs(x[tx,j] - y[ty,j]), alpha)
		d[i] = pow(tmp/np, beta)
	return (t, d)


def scale_tmap(numpy.ndarray[ITYPE, ndim=2] idxmap, double dt0, double s0, double dt1, double s1):
	assert idxmap.shape[1] == 2
	cdef int n = idxmap.shape[0]
	cdef numpy.ndarray[FTYPE, ndim=2, mode="c"] rv = numpy.zeros((n,2))
	cdef int i
	for i from 0 <= i < n:
		rv[i,0] = idxmap[i,0]*dt0 + s0
		rv[i,1] = idxmap[i,1]*dt1 + s1
	return  rv

def shift_idxmap(numpy.ndarray[ITYPE, ndim=2] idxmap, int s0, int s1):
	assert idxmap.shape[1] == 2
	cdef int n = idxmap.shape[0]
	cdef numpy.ndarray[ITYPE, ndim=2, mode="c"] rv = numpy.zeros((n,2), numpy.int32)
	cdef int i
	for i from 0 <= i < n:
		rv[i,0] = idxmap[i,0] + s0
		rv[i,1] = idxmap[i,1] + s1
	return  rv

def test_same():
	LEN = 10
	FVL = 12
	x = numpy.random.normal(size=(LEN,FVL))
	q = dtw(x, x, 2, 1)
	print 'q=', q
	assert q.shape[1]==2 and q.shape[0]==LEN
	assert numpy.equal(q[:,0], q[:,1]).all()
	print 'q[:,0]=', q[:,0]
	print 'arange(%d)=' % LEN, numpy.arange(LEN)
	assert numpy.equal(q[:,0], numpy.arange(LEN)).all()
	for bw in range(20, 3, -1):
		qb = dtw(x, x, 2, 1, bw)
		assert numpy.equal(qb[:,0], qb[:,1]).all(), "fail: %s %s" % (qb[:,0], qb[:,1])
	for step in range(1,LEN):
		print '--------step=%d---------' % step
		try:
			qj = dtwjump(x, x, step, 2, 1)
		except RuntimeError, x:
			die.die("Fails with step=%d: %s" % (step, x))
		print 'q(%d)=%s' % (step, qj)
		assert q.shape[1]==2 and q.shape[0]==LEN
		assert LEN <= q.shape[0] <= 2*LEN, "q.shape=%s" % str(q.shape)
		assert numpy.equal(qj[:,0], qj[:,1]).all(), "fail(%d): %s %s" % (step, qj[:,0], qj[:,1])
		assert numpy.equal(qj[:,0], numpy.arange(LEN)).all()
		print


def test_partial(m):
	LEN = 11
	FVL = 20
	x = numpy.random.normal(size=(LEN,FVL))
	y = numpy.array(x, copy=True)
	y[0:6,:] = 0
	if m<=0:
		q = dtw(x, y, 2, 1)
	else:
		q = dtwjump(x, y, m, 2, 1)
	print 'test_partial: q(%d)=%s' % (m, str(q))
	assert numpy.equal(q[:,0], q[:,1]).all()
	assert numpy.equal(q[:,0], numpy.arange(LEN)).all()

def test_scaled(m, LEN=20):
	FVL = 30
	x = numpy.random.normal(size=(LEN,FVL))
	y = x*0.5 + numpy.random.normal(0.0, 0.01, size=(LEN,FVL))
	if m<=0:
		q = dtw(x, y, 2, 1)
	else:
		q = dtwjump(x, y, m, 2, 1)
	assert numpy.equal(q[:,0], q[:,1]).all()
	assert numpy.equal(q[:,0], numpy.arange(LEN)).all()


def test_stretched(m):
	LEN = 12
	FVL = 30
	x = numpy.random.normal(size=(LEN,FVL))
	y = numpy.random.normal(size=(2*LEN,FVL)) * 0.01
	for i in range(LEN):
		y[2*i,:] = x[i,:]
		numpy.add(y[2*i+1,:], x[i,:], y[2*i+1,:])
	if m==0:
		q = dtw(x, y, 2, 1)
	elif m < 0:
		q = dtw(x, y, 2, 1, 3+1.5*(-m))
	else:
		q = dtwjump(x, y, m, 2, 1)
	print 'test_stretched: q(%d)=%s' % (m, str(q))
	for i in range(2*LEN):
		assert abs(q[i,0]-q[i,1]//2) <= 1, "q[%d,:] = [%d,%d]" % (i, q[i,0], q[i,1])
		# assert q[i,1]==2*q[i,0] or q[i,1]==2*q[i,0]+1


def test():
	test_same()
	for m in [0, 1, 2, 3, 5, 7, 10]:
		print '# ----- test_partial %d -----------' % m
		test_partial(m)
		print '# ----- test_scaled %d -----------' % m
		test_scaled(m)
	for m in [0, -1, -2, -3, -5, -7, -8, -21, 2, 3, 5, 7, 10]:
		print '# ----- test_stretched %d -----------' % m
		test_stretched(m)


def time_it(lng):
	import time
	N = 40
	M = 5
	t0 = time.clock()
	for ipass in range(10):
		for m in range(N):
			test_scaled(5, lng)
	print (time.clock() - t0)/(N*M)
	
	
# test()

import gpkimgclass
from gmisclib import edit_distance

def namelist(x):
	i = 0
	rv = []
	while 'TTYPE%d'%i in x.hdr:
		rv.append(x.hdr['TTYPE%d' % i])
		i += 1
	return rv

def check_names(x, y, approx_name):
	xn = namelist(x)
	yn = namelist(y)
	if approx_name:
		xs = ';'.join(xn)
		ys = ';'.join(yn)
		return edit_distance.distf(xs, ys, edit_distance.def_cost) < 0.1*0.5*(len(xs)+len(ys))
	return xn == yn


def dtw_ti(x, y, alpha, beta, x0=None, xe=None, y0=None, ye=None, approx_name=False):
	"""Does a DTW on a pair of signals, but allows you to specifiy a region of each signal
	to match.   It returns the alignment in time units, rather than in integer indices.
	@type x: gpkimgclass.gpk_img
	@type y: gpkimgclass.gpk_img
	@return: An array of corresponding times.    The time rv[i,0] in utterance x and rv[i,1]
		in utterance y correspond to similar acoustic events.    Output has units of seconds.
	@rtype: numpy.ndarray - 2-dimensional array of floats.
	"""
	if x.d.shape[0] != y.d.shape[0]:
		raise ValueError, "Feature vectors different lengths: %d and %d" % (x.d.shape[0], y.d.shape[0])
	if not check_names(x, y, approx_name):
		raise ValueError, "Feature vectors are too different."
	if x0 is None:
		i0 = 0
	else:
		i0 = int(round(x.t_index(x0)))
	if xe is None:
		ie = x.d.shape[1]
	else:
		ie = int(round(x.t_index(xe)))
	if y0 is None:
		j0 = 0
	else:
		j0 = int(round(y.t_index(y0)))
	if ye is None:
		je = y.d.shape[1]
	else:
		je = int(round(y.t_index(ye)))
	imap = dtw(x.d[:,i0:ie+1], y.d[:,j0,je+1], alpha, beta)
	rv = numpy.array([x.time(imap[:,0]), y.time(imap[:,1])], axis=1)
	assert rv.shape[1] == 2
	return rv


def parse_args(args):
	"""
	@param args: an argument list, much like you might feed to a command line program.
	@type args: list(things) Things that are flags need to be strings.   Things that are numbers
		can be either floats, ints, or strings.   The last two things (corresponding to the
		"filenames") can be anything.
	"""
	x = {'loose': False, 'limit_edges': False, 'alpha': 2.0, 'beta': 0.5, 'step': 0.0,
		'x0': None, 'xe': None, 'y0': None, 'ye': None, 'o': '-'
		}
	x0, xe, y0, ye = None, None, None, None
	while args and isinstance(args[0], str) and args[0].startswith('-'):
		arg = args.pop(0)
		if arg == '-loose':
			x['loose'] = True
		elif arg == '-limit_range':
			x['limit_edges'] = True
		elif arg == '-o':
			x['o'] = args.pop(0)
		elif arg == '-step':
			x['step'] = float(args.pop(0))
			if not ( 0.0 <= x['step'] ):
				raise ValueError, "Bad -step: %s" % x['step']
		elif arg == '-xr':
			x['x0'] = float(args.pop(0))
			x['xe'] = float(args.pop(0))
		elif arg == '-yr':
			x['y0'] = float(args.pop(0))
			x['ye'] = float(args.pop(0))
		elif arg == '-d':
			x['alpha'] = float(args.pop(0))
			x['beta'] = float(args.pop(0))/x['alpha']
		elif arg == '-test':
			test()
			raise SystemExit
		elif arg == '-time':
			time_it(int(args.pop(0)))
			raise SystemExit
		else:
			die.die("Unrecognized flag: %s: %s" % (arg, args))
	if len(args) != 2:
		die.die("Should be two args: %s" % str(args))
	fA = args.pop(0)
	fB = args.pop(0)
	assert len(args)==0
	return (fA, fB, x)
