Added Worklist algorithm.

Due to missing types all code is commented out.