refactoring, added second order, WIP
WIP, Added benchmarking, a little bit of optimizations and fixed the algorithm