Finished prototype
Improved performance and memory usage
Moved distance functions to math.cc
Added a exemplary workset