Added and adapted functions to complete exploreStates

Adapted initGP
Added check for whether goal position has been left
Added "saveIfBetter"
Added "calcCost"