some optimal control added, but it DESPERATELY needs to be turned into the version with constraints -- this flatly put doesn't work
