Still not working! But Committing for collaboration purposes.

Code structured in classes now.
1 Problem I can't get fixed is a problem with Grid::edges& in the
Astar::WikiAStar method.
Fixed all
Thanks to Menno!
added comment
Cleaned up includes, now only necessary includes
Revert "Cleaned up includes, now only necessary includes"

This reverts commit b7110f0d062bf91103f163e10b0140e8d49fdafe.