Fixed Pathfinding

Pathfinding works in streight lines at least
Consider reworking map to make it more lightwieght -> to a voxelish
system