Moving to auto update discrete vars.
Const-casting discrete variable index.
Add cache entry indices.
Write accessors for SIMBICON state.
Add state handling.
Add state handling.
Add SIMBICON to the OutputReporter.
Make state handling code more concise.
Get state machine working.
Start writing balance control.

Also update some helper methods.
Think about how to obtain global angles.
Using map for segments.
Sagittal plane balance control, but buggy.
Saving progress.