Cleaner code (fix #1)

This makes the code somewhat better but I think the underlying data structure could suit the problem better. Try a tree?