Refactored init code, started entity implementation, Refactored Draw methods to always reset matrix after draw commands