Correct algo,add granularity on priority comput'
Correct bugs, cleanup code & add a few tests