Iss022 integrate simulator and ctrl

This is intended to:

1. make the controller compile
2. make the simulator tightly integrated in the closed loop

This is not intended to have controller bug fixed, or a working controller flow.

Fixes #22 (closed)