Cleaned up code
Rearranged functions and trying to fix count_i