Fix bugs in heuristic and cost training.
Simultaneously train heuristic and cost function.
Add option for ontrajectory.
Working version.*