Added comments and polished the program. Also added test function for program testing purposes