cpp skeleton filled out
More work on astar
Matrix stuff merged