Added getStateLimits (per Hongkai's suggestion). Changed addFinalCost to take a function f(T,x) instead of f(h(1:end),x)