Implement Algorithm

Implement core algorithm as well as test results and performance