Started implementing the actual algorithm. Don't worry, the code will be tidied up!