Added test of terminal cost. Removed debug printouts.