Added some basic terrain and move costs
Add unit types and fix G calculation.

Needed to account for diagonal distance when computing G.
Cppify terrain with vectors and add ifdef to unit.