Initial

A* and test enviroment