Fixed major bug in both test and algorithm