Current version of algorithm. Produces output, though it may not be correct atm