Fix Bug with null pointer (thanks jmaustpc!)
Clean up code