Working code - still needs comments

Working implementation with test suite embedded. Still needs some
documentation before submission, but OK to commit