Fixed more bugs

probably still doesn't work.
More bug

Doesn't crash now. Doesn't seem to find goal though.