Fixed collision handling

Also added #ifdef DEBUG_MODE macros
Revert "Fixed collision handling"

This reverts commit b3459c1337316c3eebead92cf1596affb28f6602.