Corrected weighting
Move Vechash into Vec class
Templated
step() to allow visual debugging. I saw that I took a shortcut in code so that when I make a change to the map, the pathfinding starts from the beginning. I should check if the change is on the closed list line and if it is, start the pathfinding from one step before the changed node