Fix bug in algorithm
Change constants to easy testing