Refactored code, and added obstacles and walls