Subgoal planning and ValueIteration working, most Affordance code added