Fix SweepAndPrune

Use stacks for ID collections
Cleanup and Javadocs
More thourough SAP test
Signed-off-by: Aleksi Sapon <QcTechs@gmail.com>