Completed main algorithm. Only lacks leaf finalisation.