Refactored, added self collision check
Diagonals still don't work though