Document and Optimise

Begin documentation and optimisations, move quick mslp function to
include
Some major refactoring - NOT WORKING

Test commit, to get this code on the Pi and test compile