Fixed bug and cleaned up a little bit in the code (a lot more needed)