Refactored Path Finding

I have refactored the code some more and managed to speed up the path
finding a bit. I am using a simple algorithm before using the full out
path finding to get around objects.

There is a bug with the screen size changing so I have commented it
out for now.

I need to get bot rendering going so that they move in realtime and
then most of the work is nearly finished regarding bots moving.