Refactored a lot of code, algorithm now gives 10 points on kattis.