#include <utility>
#include <algorithm>
#include <math.h>
#include <tr1/unordered_map>
#include <assert.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <iostream>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

// Dynamic time-warp alignment of two sequences of feature vectors.

using namespace std;

typedef pair<int, int> intpair;

typedef struct {
	size_t operator()(const intpair& x) const {
		return (size_t)((long)(x.first)*3L + (long)(x.second)*197L);
	}
} intpair_hasher;

struct g_node {
	intpair ij;
	double gdist;
	g_node *best;
};


typedef boost::numeric::ublas::matrix<float> fmatrix;

const double BIG = 1e38;

class state_c {
	tr1::unordered_map<intpair, float, intpair_hasher> dcache;
	vector<vector<g_node> > g_nodes;
	vector<intpair> gnse;
	double alpha, beta, bw;
	fmatrix x, y;
public:
	int nx, ny, nparams;	// Really should have accessor functions.

	double search(g_node* n);
	vector< pair<int,int> > path(g_node *n);

	state_c( fmatrix& x, fmatrix& y, double alpha, double beta, double bw);

	g_node* gn(int i, int j) {
		return & g_nodes[i][j-gnse[i].first];
		}


	
private:
	void _init_g_nodes();

	bool inbeam(int i, int j) const {
		intpair se = gnse[i];
		return se.first <= j && j <= se.second;
		}
		
	float get(intpair ij);
};



float state_c::get(intpair ij) {
		tr1::unordered_map<intpair,float>::iterator q = dcache.find(ij);
		if(q != dcache.end())
			return q->second;
		double s = 0.0;
		int k;
		assert(x.size2()==nparams);
		assert(x.size1()==nx);
		assert(y.size1()==ny);
		assert(ij.first>=0 && ij.first<nx);
		assert(ij.second>=0 && ij.second<ny);
		for(k=0; k<nparams; k++)
			s += pow(fabs(x(ij.first, k)-y(ij.second, k)), alpha);
		s = pow(s/nparams, beta);
		dcache[ij] = s;
		return s;
	}


double state_c::search(g_node* n) {
		if(n->gdist >= 0.0)
			return n->gdist;
		else if(n->ij.first==0 && n->ij.second==0) {
			n->best = 0;
			n->gdist = 0.0;
			return n->gdist;
		}
		double gtmp = BIG;
		n->gdist = gtmp;
		g_node *ntmp = 0;
		if(n->ij.first > 0 && n->ij.second > 0 && inbeam(n->ij.first-1, n->ij.second-1)) {
			ntmp = gn(n->ij.first-1, n->ij.second-1);
			gtmp = search(ntmp);
			if(gtmp < n->gdist) {
				gtmp += get(ntmp->ij);
				if(gtmp < n->gdist) {
					n->gdist = gtmp;
					n->best = ntmp;
				}
			}
		}
		if(n->ij.first > 0 && inbeam(n->ij.first-1, n->ij.second)) {
			ntmp = gn(n->ij.first-1, n->ij.second);
			gtmp = search(ntmp);
			if( gtmp < n->gdist) {
				gtmp += get(ntmp->ij);
				if(gtmp < n->gdist) {
					n->gdist = gtmp;
					n->best = ntmp;
				}
			}
		if(n->ij.second > 0 && inbeam(n->ij.first, n->ij.second-1)) {
			ntmp = gn(n->ij.first, n->ij.second-1);
			gtmp = search(ntmp);
			if(gtmp < n->gdist) {
				gtmp += get(ntmp->ij);
				if(gtmp < n->gdist) {
					n->gdist = gtmp;
					n->best = ntmp;
				}
			}
		}
		return n->gdist;
	}
}



vector< pair<int,int> > state_c::path(g_node *n) {
	int ns = 0;
	g_node *nn = n;
	while(nn) {
		ns += 1;
		nn = nn->best;
	}
	vector< pair<int,int> > o(ns);
	while(n) {
		ns -= 1;
		o[ns] = n->ij;
		n = n->best;
	}
	assert(ns == 0);
	return o;
}


vector< pair<int,int> > dtw(fmatrix& x, fmatrix& y, double alpha, double beta, double bw=-1.0) {
	assert(x.size1() == y.size1());
	state_c s(x, y, alpha, beta, bw);
	g_node *n = s.gn(s.nx-1, s.ny-1);
	s.search(n);
	return s.path(n);
}




state_c::state_c( fmatrix& _x, fmatrix& _y,
		 	double _alpha, double _beta, double _bw)
: nx(_x.size1()), ny(_y.size1()), alpha(_alpha), beta(_beta),
	x(_x), y(_y), gnse(), nparams(_x.size2()),
	bw(_bw)
	{
		assert(nx>1 && ny>1);
		assert(nparams == y.size2());
		assert(bw <0.0 || bw > 1.0);
		_init_g_nodes();
	}



void state_c::_init_g_nodes() {
	const double EPS = 1e-6;
	double xbw = (nx-1)*(ny-1) - EPS;
	if(bw > 0.0)
		xbw = min(xbw, bw*hypot(nx-1, ny-1));
	int i;
	for(i=0; i<nx; i++) {
		int s = max(0, (int)(floor((i*(ny-1)-xbw)/(nx-1))));
		int e = min(ny-1, (int)(ceil((i*(ny-1)+xbw)/(nx-1))));
		vector<g_node> tmp(e-s+1);
		gnse.push_back( intpair(s, e) );
		int j;
		for(j=s; j<=e; j++) {
			g_node& n = tmp.begin()[j-s];
			n.ij = intpair(i, j);
			n.gdist = -1.0;
			n.best = 0;
		}
		g_nodes.push_back( tmp );
	}
}


typedef boost::mt19937 base_rng_t;


void test() {
	const int fvlen = 10;
	const int nfv = 11;
	fmatrix x(nfv, fvlen), y(nfv, fvlen);
	int i, j;
	base_rng_t rng; 
	boost::normal_distribution<float> nmlx(0.0, 1.0);
	boost::variate_generator<base_rng_t&, boost::normal_distribution<float> > normal(rng, nmlx);
	for(i=0; i<nfv; i++) {
		for(j=0; j<fvlen; j++) {
			float tmp = normal();
			x(i,j) = tmp;
			y(i,j) = tmp;
		}
	}
	vector< pair<int,int> > match = dtw(x, y, 2.0, 1.0);
	for(i=0; i<match.size(); i++)
		cout << match[i].first << " " << match[i].second << endl;

}

#ifdef TEST

int main()
{
	test();
	return 0;
}

#endif
