Fix distance function.

Got bitten by C's operator precedence. No wonder why filters performed so
bad...
Implement k-NN filter
Remove unused debug statement