Cleanup, some optmizs, more c++11 use

Cleaned up some icky, confusing parts of the code to make it more
understandable (such as the stack memory storage). Fixed a bug where
some paths that might not finish cause the preallocated memory storage
to run out. Now using lambdas and features built into c++ standard
library instead of some preexisting functions.