Finally got the pathfinding code, finally, now to implement it
So sleepy, dont remembr what I did