Start merging test-bench and algorithm