Finished function implementation, added benchmarks