Any link to the original algorithm? Your code is very hard to understand.