Fix bug and put in extensive tests