Reorganized program

Cleaned up code and created helper functions with comments. If more comments are needed let me know.