Implemented A* pathfinder

Cleaned up some stub impl (we can uses properties, they're far nice than Java styled accessors)
Annointed some classes as public
Added some extension methods for List<T>/SortedSet<T>