small improvements to algo
a lot of debug.
Merge pull request #4 from rgho/master

lotta stuffs.