Got the algorithm more or less working.