Fixed some more stupid math mistakes etc.

Fixed some stupid math mistakes and added a test map with a path.