Cleaned up code

* Cleaned up and fixed camera code
* Working towards working input for camera control