Cleaned code and might have found some bugs.