AStar initial implementation

TO BE REFACTORED