Agent stage optimization algorithm: FOC

[sbenthall]

Agent stage optimization algorithm. FOC

• for a single stage
• looping through a fully specified model (maybe multi-stage)

[sbenthall]

A general algorithm is implemented in #1283, but it uses a slightly different model representation. So it needs to be ported over.