Works for straight path - Node class need to be fix

Node class needs to be fixed so it does not rely constructor. For now
this is working for straight paths!!
Pathfinding works

Pathfinding works, code needs cleaned up
Merge pull request #2 from madrusec/master

Pull changes for fixed pathfinding