General tidyup and increased comments in files. Also implemented some benchmarking.